From 71fc1b81612fa9b5184d5abb93b69d109e9d0e4b Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 20 May 2021 15:46:18 +0200 Subject: [PATCH] airspeed fusion: use yaw from 312 sequence when pitch is close to 90deg This caused bad initialization of the wind estimate on tailsitters --- EKF/airspeed_fusion.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index 34f9c998a0..129dcf0970 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -173,7 +173,9 @@ void Ekf::get_true_airspeed(float *tas) const */ 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)) { // estimate wind using zero sideslip assumption and airspeed measurement if airspeed available