Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-09 22:08:56 +08:00
Code Issues Actions 3 Packages Projects Releases Wiki Activity

feat(sih): Add thrust model for hexacopter simulation (#26587)

Browse Source 
* 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 <gennaroguido2002@gmail.com>
Co-authored-by: Matthias Grob <maetugr@gmail.com>
Co-authored-by: Gennaro Guidone <gennaroguido2002@gmail.com>
This commit is contained in:
Claudio Chies
2026-04-29 17:18:05 +02:00
committed by GitHub
parent b93978fbf9
commit 76ce06c931
2 changed files with 11 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d-posix/airframes/10044_sihsim_hex
+2
View File
@@ -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
src/modules/simulation/simulator_sih/sih.cpp
+9 -4
View File
@@ -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