From 76ce06c931906a428e1fba1e27f95ab9e35ed7be Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Wed, 29 Apr 2026 17:18:05 +0200 Subject: [PATCH] feat(sih): Add thrust model for hexacopter simulation (#26587) * Quadratic thrust for SIH hexarotor (#67) * sih: add thrust model hexacopter model * fix: add missing line break * sihsim_hey airframe: add THR_MDL_FAC to accomodate for simulated quadratic motor thrust. * 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 Co-authored-by: Matthias Grob Co-authored-by: Gennaro Guidone --- .../init.d-posix/airframes/10044_sihsim_hex | 2 ++ src/modules/simulation/simulator_sih/sih.cpp | 13 +++++++++---- 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10044_sihsim_hex b/ROMFS/px4fmu_common/init.d-posix/airframes/10044_sihsim_hex index 269a7737d2..f99b200359 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10044_sihsim_hex +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10044_sihsim_hex @@ -47,3 +47,5 @@ param set-default PWM_MAIN_FUNC6 106 param set-default SENS_GPS0_DELAY 0 param set-default SENS_GPS1_DELAY 0 + +param set-default THR_MDL_FAC 1 # simulated quadratic motor thrust diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index 843a22281f..befa63a584 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -366,11 +366,16 @@ void Sih::generate_force_and_torques(const float dt) m3 m2 ├1/2┤ ├ 1 ┤ */ - _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])); + float u_sq[6]; + for (int i = 0; i < 6; ++i) { + 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_sq[0] + u_sq[1] + u_sq[2] + u_sq[3] + u_sq[4] + u_sq[5])); + _Mt_B = Vector3f(_L_ROLL * _T_MAX * (-.5f * u_sq[0] - u_sq[1] - .5f * u_sq[2] + .5f * u_sq[3] + u_sq[4] + .5f * u_sq[5]), + _L_PITCH * _T_MAX * (M_SQRT3_F / 2.f) * (+u_sq[0] - u_sq[2] - u_sq[3] + 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 * _R_N2E * _v_apparent_N; // first order drag to slow down the aircraft _Ma_B = -_KDW * _w_B; // first order angular damper