From a89c0b6e3dc4408b4b06363b4e0b46b96bcce8cc Mon Sep 17 00:00:00 2001 From: Gennaro Guidone <99129829+gguidone@users.noreply.github.com> Date: Wed, 4 Mar 2026 18:37:18 +0100 Subject: [PATCH] fix(sih): fix quadratic model in simulation The square was done in place and the low pass filter also effected the same variable, the result was that for small numbers the low pass filter gain was not enouhg with respect to the decay caused by squaring numbers close to zero. As a result even if you where trying to send 1 xcommands in simulation it would stoip around 0.008. Now the square is done in a different variable, the problem is not there anymore. Tested in simulation Signed-off-by: Gennaro Guidone --- src/modules/simulation/simulator_sih/sih.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index 7bf37364aa..78b841b068 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -337,14 +337,16 @@ void Sih::generate_force_and_torques(const float dt) m3 m2 ├1/2┤ ├ 1 ┤ */ + float u_sq[6]; + for (int i = 0; i < 6; ++i) { - _u[i] *= _u[i]; // quadratic thrust model + u_sq[i] = _u[i] * _u[i]; // quadratic thrust model, keep _u[i] intact for the filter } - _T_B = Vector3f(0.0f, 0.0f, -_T_MAX * (+_u[0] + _u[1] + _u[2] + _u[3] + _u[4] + _u[5])); - _Mt_B = Vector3f(_L_ROLL * _T_MAX * (-.5f * _u[0] - _u[1] - .5f * _u[2] + .5f * _u[3] + _u[4] + .5f * _u[5]), - _L_PITCH * _T_MAX * (M_SQRT3_F / 2.f) * (+_u[0] - _u[2] - _u[3] + _u[5]), - _Q_MAX * (+_u[0] - _u[1] + _u[2] - _u[3] + _u[4] - _u[5])); + _T_B = Vector3f(0.0f, 0.0f, -_T_MAX * (+u_sq[0] + u_sq[1] + u_sq[2] + u_sq[3] + u_sq[4] + u_sq[5])); + _Mt_B = Vector3f(_L_ROLL * _T_MAX * (- u_sq[0] + u_sq[1] + .5f * u_sq[2] - .5f * u_sq[3] - .5f * u_sq[4] + .5f * u_sq[5]), + _L_PITCH * _T_MAX * (M_SQRT3_F / 2.f) * (+u_sq[2] - u_sq[3] + u_sq[4] - u_sq[5]), + _Q_MAX * (-u_sq[0] + u_sq[1] - u_sq[2] + u_sq[3] + u_sq[4] - u_sq[5])); _Fa_E = -_KDV * _v_E; // first order drag to slow down the aircraft _Ma_B = -_KDW * _w_B; // first order angular damper