fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix(sih): fix quadratic model in simulation

Browse Source 
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 <gennaroguido2002@gmail.com>
This commit is contained in:
Gennaro Guidone 2026-03-04 18:37:18 +01:00 committed by gguidone
parent 8f170bca13
commit a89c0b6e3d
1 changed files with 7 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/modules/simulation/simulator_sih/sih.cpp
View File

@ -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