From d9e752777ff5f828912c1cd9e369d93feb521768 Mon Sep 17 00:00:00 2001 From: Rune Harlyk Date: Thu, 29 Jan 2026 17:04:27 +0100 Subject: [PATCH] =?UTF-8?q?=F0=9F=90=9B=20Clamps=20imu=20compensation=20to?= =?UTF-8?q?=20max=20roll=20and=20pitch?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- esp32/include/motion_states/state.h | 15 +++++++++------ esp32/include/utils/math_utils.h | 4 ++++ 2 files changed, 13 insertions(+), 6 deletions(-) diff --git a/esp32/include/motion_states/state.h b/esp32/include/motion_states/state.h index 7023ee5..04da498 100644 --- a/esp32/include/motion_states/state.h +++ b/esp32/include/motion_states/state.h @@ -17,9 +17,12 @@ class MotionState { body_state.ym = lerp(body_state.ym, target_body_state.ym, smoothing_factor); body_state.zm = lerp(body_state.zm, target_body_state.zm, smoothing_factor); body_state.phi = lerp(body_state.phi, target_body_state.phi, smoothing_factor); - body_state.psi = lerp(body_state.psi, target_body_state.psi - imuCompensate * psi_offset, smoothing_factor); - body_state.omega = - lerp(body_state.omega, target_body_state.omega - imuCompensate * omega_offset, smoothing_factor); + const float target_psi = + clamp(target_body_state.psi - imuCompensate * psi_offset, -KinConfig::max_pitch, KinConfig::max_pitch); + const float target_omega = + clamp(target_body_state.omega - imuCompensate * omega_offset, -KinConfig::max_roll, KinConfig::max_roll); + body_state.psi = lerp(body_state.psi, target_psi, smoothing_factor); + body_state.omega = lerp(body_state.omega, target_omega, smoothing_factor); } void updateFeet(body_state_t& body_state, const float smoothing_factor = default_smoothing_factor) { @@ -29,9 +32,9 @@ class MotionState { } public: - void updateImuOffsets(const float omega_offset, const float psi_offset) { - this->omega_offset = omega_offset * RAD2DEG_F; - this->psi_offset = psi_offset * RAD2DEG_F; + void updateImuOffsets(const float new_omega, const float new_psi) { + omega_offset = new_omega * RAD_TO_DEG; + psi_offset = new_psi * RAD_TO_DEG; } virtual ~MotionState() {} diff --git a/esp32/include/utils/math_utils.h b/esp32/include/utils/math_utils.h index f398a59..e2bb272 100644 --- a/esp32/include/utils/math_utils.h +++ b/esp32/include/utils/math_utils.h @@ -42,6 +42,10 @@ inline float lerp(float start, float end, float t) { return (1 - t) * start + t * end; } +inline float clamp(float value, float min_val, float max_val) { + return value < min_val ? min_val : (value > max_val ? max_val : value); +} + inline bool isEqual(float a, float b, float epsilon) { return std::fabs(a - b) < epsilon; } inline float round2(float value) { return (int)(value * 100 + 0.5) / 100.0; }