Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 15:40:31 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

SIH: add transport rate acceleration to local acceleration

Browse Source
This commit is contained in:
bresch
2024-11-26 11:45:46 +01:00
committed by Mathieu Bresciani
parent 674aa474e7
commit cd18138b1c
1 changed files with 5 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/simulation/simulator_sih/sih.cpp
+5 -1
View File
@@ -445,7 +445,11 @@ void Sih::equations_of_motion(const float dt)
_specific_force_E = acceleration_E - gravity_acceleration_E;
_v_E_dot = acceleration_E + coriolis_acceleration_E;
_v_N_dot = _R_N2E.transpose() * _v_E_dot; //TODO: add transport rate
// add fictitious transport rate acceleration as the local navigation frame rotates
// to stay tangent to the ellipsoid
const Vector3f transport_rate = -_lla.computeAngularRateNavFrame(_v_N).cross(_v_N);
_v_N_dot = _R_N2E.transpose() * _v_E_dot + transport_rate;
// forward Euler velocity intergation
Vector3f v_E_prev = _v_E;