From ea48f2d9848dc0ac824ec05791c50f8f14093151 Mon Sep 17 00:00:00 2001
From: franchioping <franchioping@galard.uk>
Date: Thu, 2 Apr 2026 20:02:52 +0100
Subject: [PATCH] position

---
 .gitignore             |  1 +
 rust-toolchain.toml    |  2 --
 src/config.rs          |  1 +
 src/input.rs           |  8 ++++----
 src/stacked.rs         | 28 ++++++++++++++++++++++------
 src/stacked/modules.rs |  2 ++
 6 files changed, 30 insertions(+), 12 deletions(-)
 delete mode 100644 rust-toolchain.toml

diff --git a/.gitignore b/.gitignore
index 2f7896d..c8a5150 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1 +1,2 @@
 target/
+rust-toolchain.toml
diff --git a/rust-toolchain.toml b/rust-toolchain.toml
deleted file mode 100644
index a2f5ab5..0000000
--- a/rust-toolchain.toml
+++ /dev/null
@@ -1,2 +0,0 @@
-[toolchain]
-channel = "esp"
diff --git a/src/config.rs b/src/config.rs
index 2409694..b062f03 100644
--- a/src/config.rs
+++ b/src/config.rs
@@ -19,6 +19,7 @@ pub struct ControllerStackConfig {
 
     /// Maximum angular rate (rad/s) that joystick input maps to
     pub max_rate: f32,
+    pub max_linvel: f32,
 }
 
 #[repr(C)]
diff --git a/src/input.rs b/src/input.rs
index 3f91d36..f45e302 100644
--- a/src/input.rs
+++ b/src/input.rs
@@ -13,9 +13,9 @@ pub struct JoystickInput {
 #[repr(C)]
 #[derive(Default, Clone, Copy, PartialEq, serde::Serialize, serde::Deserialize)]
 pub struct PositionInput {
-    pub lat: f32,
-    pub long: f32,
-    pub alt: f32,
+    pub x: f32,
+    pub y: f32,
+    pub z: f32,
 }
 
 #[repr(C)]
@@ -50,7 +50,7 @@ pub enum ModeInput {
     Rotation,
     Acceleration,
     Velocity,
-    Navigation,
+    Position,
 }
 
 #[repr(C)]
diff --git a/src/stacked.rs b/src/stacked.rs
index e1e462e..b9c6fb0 100644
--- a/src/stacked.rs
+++ b/src/stacked.rs
@@ -25,10 +25,11 @@ pub struct DroneState {
 }
 
 pub struct StackedController {
+    pub angvel_rt: ModuleRuntime<AngularRateController>,
     pub rotation_rt: ModuleRuntime<RotationController>,
-    pub rate_rt: ModuleRuntime<AngularRateController>,
     pub linaccel_rt: ModuleRuntime<AccelerationController>,
     pub linvel_rt: ModuleRuntime<VelocityController>,
+    pub position_rt: ModuleRuntime<PositionController>,
 
     mixer: MotorMixer,
     config: ControllerConfig,
@@ -45,6 +46,10 @@ impl StackedController {
         let min_throttle = 0.0;
         let max_throttle = 1.0;
 
+        let position_ctrl = PositionController::new(
+            PidProcessor::new(&config.stack.position_pid),
+            config.stack.max_linvel,
+        );
         let lin_vel_ctrl = VelocityController::new(PidProcessor::new(&config.stack.linvel_pid));
         let lin_accel_ctrl =
             AccelerationController::new(max_throttle, min_throttle, config.max_thrust);
@@ -54,8 +59,9 @@ impl StackedController {
         Self {
             linaccel_rt: ModuleRuntime::new(lin_accel_ctrl, config.stack.rotation_pid.frequency),
             rotation_rt: ModuleRuntime::new(rotation_ctrl, config.stack.rotation_pid.frequency),
-            rate_rt: ModuleRuntime::new(rate_ctrl, config.stack.rate_pid.frequency),
+            angvel_rt: ModuleRuntime::new(rate_ctrl, config.stack.rate_pid.frequency),
             linvel_rt: ModuleRuntime::new(lin_vel_ctrl, config.stack.linvel_pid.frequency),
+            position_rt: ModuleRuntime::new(position_ctrl, config.stack.position_pid.frequency),
             mixer: MotorMixer {
                 motor_map: config.motor_map,
                 min_throttle: 0.0,
@@ -111,8 +117,19 @@ impl DroneController for StackedController {
             let rotation_setpoint = if self.input.mode != ModeInput::Rotation {
                 let lin_accel_setpoint = if self.input.mode != ModeInput::Acceleration {
                     let lin_vel_setpoint: Velocity = if self.input.mode != ModeInput::Velocity {
-                        println!("LAYER NOT IMPLENETED. TAKING 0 AS INPUT;");
-                        Velocity(na::Vector3::zeros())
+                        let position_setpoint: Position = if self.input.mode != ModeInput::Position
+                        {
+                            println!("LAYER NOT IMPLENETED. TAKING 0 AS INPUT;");
+                            Position(na::Vector3::zeros())
+                        } else {
+                            Position(na::vector![
+                                self.input.position.x,
+                                self.input.position.y,
+                                self.input.position.z
+                            ])
+                        };
+                        self.position_rt
+                            .update(position_setpoint, &self.drone_state, frame_dt)
                     } else {
                         Velocity(na::vector![
                             self.input.velocity.x,
@@ -135,7 +152,6 @@ impl DroneController for StackedController {
                 throttle = ret.1 .0;
                 ret.0
             } else {
-                // println!("Rotation!");
                 Rotation(na::vector![
                     self.input.rotation.roll,
                     self.input.rotation.pitch,
@@ -155,7 +171,7 @@ impl DroneController for StackedController {
         };
 
         let torque = self
-            .rate_rt
+            .angvel_rt
             .update(angular_rate_setpoint, &self.drone_state, frame_dt);
 
         self.mixer.mix(throttle, torque.0)
diff --git a/src/stacked/modules.rs b/src/stacked/modules.rs
index d6c7a8e..3c81cba 100644
--- a/src/stacked/modules.rs
+++ b/src/stacked/modules.rs
@@ -4,11 +4,13 @@ use nalgebra as na;
 
 pub mod acceleration;
 pub mod angular_rate;
+pub mod position;
 pub mod rotation;
 pub mod velocity;
 
 pub use acceleration::AccelerationController;
 pub use angular_rate::AngularRateController;
+pub use position::PositionController;
 pub use rotation::RotationController;
 pub use velocity::VelocityController;