SIH: do not store delta_quaternion

Converting an AxisAngle to a Quaternion uses the exponenial
PX4 · Nov 19, 2024 · 0627e29 · 0627e29
1 parent 2b102b9
commit 0627e29
Show file tree

Hide file tree

Showing 2 changed files with 2 additions and 3 deletions.
diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp
@@ -455,9 +455,9 @@ void Sih::equations_of_motion(const float dt)
 		// trapezoidal position integration
 		_p_E = _p_E + (Vector3d(_v_E(0), _v_E(1), _v_E(2)) + v_E_prev) * 0.5 * dt;
 
-		_dq = Quatf::expq(0.5f * dt * _w_B);
+		const Quatf dq(AxisAnglef(0.5f * dt * _w_B));
 
-		_q_E = _q_E  * _dq;
+		_q_E = _q_E  * dq;
 		_q_E.normalize();
 
 		const Vector3f w_B_dot = _Im1 * (_Mt_B + _Ma_B - _w_B.cross(_I * _w_B)); // conservation of angular momentum

diff --git a/src/modules/simulation/simulator_sih/sih.hpp b/src/modules/simulation/simulator_sih/sih.hpp
@@ -215,7 +215,6 @@ class Sih : public ModuleBase<Sih>, public ModuleParams
 	matrix::Quatf       _q{};             // quaternion attitude in local navigation frame
 	matrix::Quatf       _q_E{};           // quaternion attitude in ECEF
 	matrix::Vector3f    _w_B{};           // body rates in body frame [rad/s]
-	matrix::Quatf       _dq{};            // quaternion differential
 	float       _u[NB_MOTORS] {};         // thruster signals
 
 	matrix::Vector3f _gravity_E{};