From f24de4e2bcf58ad95691b2c2fec8cb115b7063dc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Sep 2019 10:32:00 +1000 Subject: [PATCH] AP_NavEKF3: set a min yaw accuracy from GPS of 5 degrees GPS modules tend to be rather optimistic about their yaw accuracy. By setting a min or 5 degrees we prevent the user constantly getting warnings about yaw innovations --- libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 322e6aa0cc..b358e0306e 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -622,6 +622,12 @@ void NavEKF3_core::readGpsData() // if the GPS has yaw data then input that as well float yaw_deg, yaw_accuracy_deg; if (AP::gps().gps_yaw_deg(yaw_deg, yaw_accuracy_deg)) { + // GPS modules are rather too optimistic about their + // accuracy. Set to min of 5 degrees here to prevent + // the user constantly receiving warnings about high + // normalised yaw innovations + const float min_yaw_accuracy_deg = 5.0f; + yaw_accuracy_deg = MAX(yaw_accuracy_deg, min_yaw_accuracy_deg); writeEulerYawAngle(radians(yaw_deg), radians(yaw_accuracy_deg), gpsDataNew.time_ms, 2); }