From 3504bc479075bbc363697edc4123978e4342c823 Mon Sep 17 00:00:00 2001
From: franchioping <franchioping@galard.uk>
Date: Sun, 19 Apr 2026 15:48:10 +0100
Subject: [PATCH] set max linvel

---
 CMakeLists.txt                      |  2 +-
 src/ccont.rs                        | 25 +++++++++++++++++++++++++
 src/config.rs                       |  2 +-
 src/stacked.rs                      |  3 +++
 src/stacked/modules.rs              |  7 +++++++
 src/stacked/modules/angular_rate.rs |  2 +-
 src/stacked/modules/position.rs     |  4 ++--
 src/stacked/modules/rotation.rs     |  2 +-
 src/stacked/modules/velocity.rs     |  2 +-
 9 files changed, 42 insertions(+), 7 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2c4f731..43ec385 100644
--- a/CMakeLists.txt
+++ b/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)
diff --git a/src/ccont.rs b/src/ccont.rs
index ffe9997..c1aa3cd 100644
--- a/src/ccont.rs
+++ b/src/ccont.rs
@@ -53,6 +53,17 @@ pub extern "C" fn set_input(ptr: *mut StackedController, inp: Input) {
     controller.set_input(inp);
 }
 
+
+#[unsafe(no_mangle)]
+pub extern "C" fn set_max_linvel(ptr: *mut StackedController, max_linvel: f32) {
+    let controller = unsafe {
+        assert!(!ptr.is_null());
+        &mut *ptr
+    };
+
+    controller.set_max_linvel(max_linvel);
+}
+
 #[unsafe(no_mangle)]
 pub extern "C" fn set_cur_rot(ptr: *mut StackedController, rot: QuatC) {
     let controller = unsafe {
@@ -92,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();
+
+}
diff --git a/src/config.rs b/src/config.rs
index b062f03..2595ec4 100644
--- a/src/config.rs
+++ b/src/config.rs
@@ -26,7 +26,7 @@ pub struct ControllerStackConfig {
 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,
diff --git a/src/stacked.rs b/src/stacked.rs
index e3ed2d7..6fb193d 100644
--- a/src/stacked.rs
+++ b/src/stacked.rs
@@ -42,6 +42,9 @@ 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;
diff --git a/src/stacked/modules.rs b/src/stacked/modules.rs
index 3c81cba..fd7ff22 100644
--- a/src/stacked/modules.rs
+++ b/src/stacked/modules.rs
@@ -1,6 +1,7 @@
 use crate::DroneState;
 use crate::PidConfig;
 use nalgebra as na;
+use nalgebra::zero;
 
 pub mod acceleration;
 pub mod angular_rate;
@@ -101,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>,
diff --git a/src/stacked/modules/angular_rate.rs b/src/stacked/modules/angular_rate.rs
index 5ebf2a5..20f19d0 100644
--- a/src/stacked/modules/angular_rate.rs
+++ b/src/stacked/modules/angular_rate.rs
@@ -1,7 +1,7 @@
 use crate::stacked::modules::*;
 
 pub struct AngularRateController {
-    pid: PidProcessor,
+    pub pid: PidProcessor,
 }
 
 impl AngularRateController {
diff --git a/src/stacked/modules/position.rs b/src/stacked/modules/position.rs
index b6d0303..2157d7d 100644
--- a/src/stacked/modules/position.rs
+++ b/src/stacked/modules/position.rs
@@ -1,8 +1,8 @@
 use crate::stacked::modules::*;
 
 pub struct PositionController {
-    pid: PidProcessor,
-    max_linvel: f32,
+    pub pid: PidProcessor,
+    pub max_linvel: f32,
 }
 
 impl PositionController {
diff --git a/src/stacked/modules/rotation.rs b/src/stacked/modules/rotation.rs
index 51c99a1..5b432f8 100644
--- a/src/stacked/modules/rotation.rs
+++ b/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 {
diff --git a/src/stacked/modules/velocity.rs b/src/stacked/modules/velocity.rs
index c1621f3..90b62bf 100644
--- a/src/stacked/modules/velocity.rs
+++ b/src/stacked/modules/velocity.rs
@@ -1,7 +1,7 @@
 use crate::stacked::modules::*;
 
 pub struct VelocityController {
-    pid: PidProcessor,
+    pub pid: PidProcessor,
 }
 
 impl VelocityController {