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>

ROMFS/px4fmu_common/init.d-posix/airframes/10044_sihsim_hex

@@ -45,3 +45,5 @@ param set-default PWM_MAIN_FUNC5 105
 param set-default PWM_MAIN_FUNC6 106
 
 param set-default EKF2_GPS_DELAY 0
+
+param set-default THR_MDL_FAC 1 # simulated quadratic motor thrust

src/modules/simulation/simulator_sih/sih.cpp

@@ -337,10 +337,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 * (- 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