diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index cabf245190..de0e2ca512 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -978,7 +978,10 @@ void NavEKF3_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat, // extract yaw from the attitude float roll_rad, pitch_rad, yaw_rad; quat.to_euler(roll_rad, pitch_rad, yaw_rad); - writeEulerYawAngle(yaw_rad, angErr, timeStamp_ms, 2); + + // ensure yaw accuracy is no better than 5 degrees (some callers may send zero) + const float yaw_accuracy_rad = MAX(angErr, radians(5.0f)); + writeEulerYawAngle(yaw_rad, yaw_accuracy_rad, timeStamp_ms, 2); storedExtNav.push(extNavDataNew); }