position controller

src/stacked.rs

@@ -64,8 +64,8 @@ impl StackedController {
             position_rt: ModuleRuntime::new(position_ctrl, config.stack.position_pid.frequency),
             mixer: MotorMixer {
                 motor_map: config.motor_map,
-                min_throttle: 0.0,
-                max_throttle: 1.0,
+                min_throttle: min_throttle,
+                max_throttle: max_throttle,
                 mixing_mode: mixer::MotorMixingMode::default(),
             },
 

src/stacked/mixer.rs

@@ -82,9 +82,9 @@ impl MotorMixer {
         let delta_dif = max_delta - min_delta;
 
         if delta_dif > throttle_range_len {
-            scale *= throttle_range_len / delta_dif * 0.99;
-            max_delta *= throttle_range_len / delta_dif * 0.99;
-            min_delta *= throttle_range_len / delta_dif * 0.99;
+            scale *= throttle_range_len / delta_dif * 0.98;
+            max_delta *= throttle_range_len / delta_dif * 0.98;
+            min_delta *= throttle_range_len / delta_dif * 0.98;
         }
 
         let lim_throttle = throttle.clamp(

src/stacked/modules/position.rs

@@ -0,0 +1,26 @@
+use crate::stacked::modules::*;
+
+pub struct PositionController {
+    pid: PidProcessor,
+    max_linvel: f32,
+}
+
+impl PositionController {
+    pub fn new(pid: PidProcessor, max_linvel: f32) -> Self {
+        Self { pid, max_linvel }
+    }
+}
+
+impl ControllerModule for PositionController {
+    type Input = Position;
+    type Output = Velocity;
+
+    fn process(&mut self, input: Position, state: &DroneState, dt: f32) -> Velocity {
+        let target_rate = input.0;
+
+        let output: na::Vector3<f32> = self.pid.update(target_rate, state.position, dt);
+        let norm = output.norm().clamp(0.0, self.max_linvel);
+
+        Velocity(output.normalize() * norm)
+    }
+}