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

This caused bad initialization of the wind estimate on tailsitters
4 years ago · 71fc1b8161
1 changed files with 3 additions and 1 deletions
--- a/EKF/airspeed_fusion.cpp
+++ b/EKF/airspeed_fusion.cpp
@ -173,7 +173,9 @@ void Ekf::get_true_airspeed(float *tas) const
				@@ -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