set max linvel

Reviewed-on: #1

.gitignore

@@ -1 +1,2 @@
 target/
+rust-toolchain.toml

CMakeLists.txt

@@ -21,7 +21,7 @@ set(RUST_STATIC_LIBRARY "${RUST_TARGET_DIR}/${CARGO_TARGET}/${CARGO_BUILD_TYPE}/
 
 idf_component_register(
     SRCS "placeholder.c"
-    INCLUDE_DIRS "" "${RUST_INCLUDE_DIR}"
+    INCLUDE_DIRS "" 
     PRIV_REQUIRES "${RUST_DEPS}"
 )
 idf_build_get_property(sdkconfig SDKCONFIG)

build.rs

@@ -16,6 +16,7 @@ fn run_cbindgen(cargo_dir: &Path, target_dir: &Path) {
     cbindgen::Builder::new()
         .with_crate(cargo_dir)
         .with_language(cbindgen::Language::Cxx)
+        .with_namespace("dcont")
         .with_pragma_once(true)
         .generate()
         .expect("Unable to generate bindings")

rust-toolchain.toml

@@ -1,2 +0,0 @@
-[toolchain]
-channel = "esp"

src/ccont.rs

@@ -53,16 +53,25 @@ pub extern "C" fn set_input(ptr: *mut StackedController, inp: Input) {
     controller.set_input(inp);
 }
 
+
 #[unsafe(no_mangle)]
-pub extern "C" fn set_cur_rot(ptr: *mut StackedController, parts: Vec3C, scalar: f32) {
+pub extern "C" fn set_max_linvel(ptr: *mut StackedController, max_linvel: f32) {
     let controller = unsafe {
         assert!(!ptr.is_null());
         &mut *ptr
     };
 
-    let quat: na::UnitQuaternion<f32> =
+    controller.set_max_linvel(max_linvel);
-        na::UnitQuaternion::from_quaternion(na::Quaternion::from_parts(scalar, parts.into()));
+}
-    controller.set_rotation(quat);
+
+#[unsafe(no_mangle)]
+pub extern "C" fn set_cur_rot(ptr: *mut StackedController, rot: QuatC) {
+    let controller = unsafe {
+        assert!(!ptr.is_null());
+        &mut *ptr
+    };
+
+    controller.set_rotation(rot.into());
 }
 
 #[unsafe(no_mangle)]
@@ -94,3 +103,17 @@ pub extern "C" fn set_cur_pos(ptr: *mut StackedController, pos: Vec3C) {
 
     controller.set_pos(pos.into());
 }
+
+#[unsafe(no_mangle)]
+pub extern "C" fn reset_pid_states(ptr: *mut StackedController){
+    let controller = unsafe {
+        assert!(!ptr.is_null());
+        &mut *ptr
+    };
+    controller.angvel_rt.module.pid.reset_state();
+    controller.rotation_rt.module.pid.reset_state();
+    // controller.linaccel_rt.module.pid.reset_state();
+    controller.linvel_rt.module.pid.reset_state();
+    controller.position_rt.module.pid.reset_state();
+
+}

src/config.rs

@@ -19,13 +19,14 @@ pub struct ControllerStackConfig {
 
     /// Maximum angular rate (rad/s) that joystick input maps to
     pub max_rate: f32,
+    pub max_linvel: f32,
 }
 
 #[repr(C)]
 pub struct ControllerConfig {
     pub stack: ControllerStackConfig,
 
-    // For each motor, [Pitch, Roll, Yaw] (Rotation around x,y,z)
+    // For each motor, [Roll, Pitch, Yaw] (Rotation around x,y,z)
     pub motor_map: [[f32; 3]; 4],
 
     pub mass: f32,

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 x: f32,
-    pub long: f32,
+    pub y: f32,
-    pub alt: f32,
+    pub z: f32,
 }
 
 #[repr(C)]
@@ -50,7 +50,7 @@ pub enum ModeInput {
     Rotation,
     Acceleration,
     Velocity,
-    Navigation,
+    Position,
 }
 
 #[repr(C)]

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,
@@ -41,10 +42,17 @@ pub struct StackedController {
 }
 
 impl StackedController {
+    pub fn set_max_linvel(&mut self, max_linvel: f32) {
+        self.position_rt.module.max_linvel = max_linvel;
+    }
     pub fn new(config: ControllerConfig) -> Self {
         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,12 +62,13 @@ 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,
+                min_throttle: min_throttle,
-                max_throttle: 1.0,
+                max_throttle: max_throttle,
                 mixing_mode: mixer::MotorMixingMode::default(),
             },
 
@@ -110,7 +119,29 @@ impl DroneController for StackedController {
         let angular_rate_setpoint = if self.input.mode != ModeInput::Acro {
             let rotation_setpoint = if self.input.mode != ModeInput::Rotation {
                 let lin_accel_setpoint = if self.input.mode != ModeInput::Acceleration {
-                    panic!("Not Implemented")
+                    let lin_vel_setpoint: Velocity = if self.input.mode != ModeInput::Velocity {
+                        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,
+                            self.input.velocity.y,
+                            self.input.velocity.z
+                        ])
+                    };
+                    self.linvel_rt
+                        .update(lin_vel_setpoint, &self.drone_state, frame_dt)
                 } else {
                     Acceleration(na::vector![
                         self.input.acceleration.x,
@@ -124,7 +155,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,
@@ -144,7 +174,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)

src/stacked/mixer.rs

@@ -7,7 +7,7 @@ pub enum MotorMixingMode {
 }
 impl Default for MotorMixingMode {
     fn default() -> Self {
-        Self::ThrottleAuthorityReasonable { min_scale: 0.1 }
+        Self::ThrottleAuthorityReasonable { min_scale: 0.05 }
     }
 }
 
@@ -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;
+            scale *= throttle_range_len / delta_dif * 0.98;
-            max_delta *= throttle_range_len / delta_dif * 0.99;
+            max_delta *= throttle_range_len / delta_dif * 0.98;
-            min_delta *= throttle_range_len / delta_dif * 0.99;
+            min_delta *= throttle_range_len / delta_dif * 0.98;
         }
 
         let lim_throttle = throttle.clamp(

src/stacked/modules.rs

@@ -1,14 +1,17 @@
 use crate::DroneState;
 use crate::PidConfig;
 use nalgebra as na;
+use nalgebra::zero;
 
 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;
 
@@ -99,6 +102,12 @@ impl PidProcessor {
         }
     }
 
+    pub fn reset_state(&mut self) {
+        self.integral = na::Vector3::<f32>::zeros();
+        self.last_error = na::Vector3::<f32>::zeros();
+        // self.last_error = 0.0;
+    }
+
     pub fn update(
         &mut self,
         target: na::Vector3<f32>,

src/stacked/modules/angular_rate.rs

@@ -1,7 +1,7 @@
 use crate::stacked::modules::*;
 
 pub struct AngularRateController {
-    pid: PidProcessor,
+    pub pid: PidProcessor,
 }
 
 impl AngularRateController {

src/stacked/modules/position.rs

@@ -0,0 +1,26 @@
+use crate::stacked::modules::*;
+
+pub struct PositionController {
+    pub pid: PidProcessor,
+    pub 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)
+    }
+}

src/stacked/modules/rotation.rs

@@ -3,7 +3,7 @@ use nalgebra::UnitQuaternion;
 use crate::stacked::modules::*;
 
 pub struct RotationController {
-    pid: PidProcessor,
+    pub pid: PidProcessor,
 }
 impl RotationController {
     pub fn new(pid: PidProcessor) -> Self {

src/stacked/modules/velocity.rs

@@ -1,7 +1,7 @@
 use crate::stacked::modules::*;
 
 pub struct VelocityController {
-    pid: PidProcessor,
+    pub pid: PidProcessor,
 }
 
 impl VelocityController {