From 8a9bac29a2962b36570e2991343d9f3e8d18a963 Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Mon, 25 Nov 2024 15:34:46 +0100
Subject: [PATCH] SIH-FW: allow pitching up during takeoff

Otherwise difficult to get lift
---
 src/modules/simulation/simulator_sih/sih.cpp | 8 +++-----
 1 file changed, 3 insertions(+), 5 deletions(-)

diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp
index 3210c33ed0..a184f1ac99 100644
--- a/src/modules/simulation/simulator_sih/sih.cpp
+++ b/src/modules/simulation/simulator_sih/sih.cpp
@@ -409,11 +409,9 @@ void Sih::equations_of_motion(const float dt)
 			// integration: Euler forward
 			_p_I = _p_I + _p_I_dot * dt;
 			_v_I = _v_I + _v_I_dot * dt;
-			Eulerf RPY = Eulerf(_q);
-			RPY(0) = 0.0f;	// no roll
-			RPY(1) = radians(0.0f); // pitch slightly up if needed to get some lift
-			_q = Quatf(RPY);
-			_w_B.setZero();
+			_q = _q * _dq;
+			_q.normalize();
+			_w_B = constrain(_w_B + _w_B_dot * dt, -6.0f * M_PI_F, 6.0f * M_PI_F);
 			_grounded = true;
 		}