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

airspeed fusion: use yaw from 312 sequence when pitch is close to 90deg

Browse Source 
This caused bad initialization of the wind estimate on tailsitters
This commit is contained in:
bresch
2021-05-20 15:46:18 +02:00
committed by Paul Riseborough
parent e57af44d71
commit 71fc1b8161
1 changed files with 3 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/airspeed_fusion.cpp
+3 -1
View File
@@ -173,7 +173,9 @@ void Ekf::get_true_airspeed(float *tas) const
*/ */
void Ekf::resetWindStates() void Ekf::resetWindStates()
{ {
const float euler_yaw = getEuler321Yaw(_state.quat_nominal); const float euler_yaw = shouldUse321RotationSequence(_R_to_earth)
? getEuler321Yaw(_state.quat_nominal)
: getEuler312Yaw(_state.quat_nominal);
if (_tas_data_ready && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us < (uint64_t)5e5)) { if (_tas_data_ready && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us < (uint64_t)5e5)) {
// estimate wind using zero sideslip assumption and airspeed measurement if airspeed available // estimate wind using zero sideslip assumption and airspeed measurement if airspeed available