From 29f1c318545fbffcba79675e9bc48c67a8ec35cf Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 28 Nov 2022 11:27:16 +1100 Subject: [PATCH] AP_GPS: added GPYW logging for GPS yaw this makes it much easier to debug GPS yaw issues, by logging the raw data even if the yaw is rejected --- libraries/AP_GPS/AP_GPS_NMEA.cpp | 4 +++- libraries/AP_GPS/GPS_Backend.cpp | 22 +++++++++++++++++++--- 2 files changed, 22 insertions(+), 4 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index 04387c3235..24b457715e 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -470,7 +470,9 @@ bool AP_GPS_NMEA::_term_complete() const float dist = state.location.get_distance_NED(slave).length(); const float bearing = degrees(state.location.get_bearing(slave)); const float alt_diff = (state.location.alt - slave.alt)*0.01; - calculate_moving_base_yaw(bearing, dist, alt_diff); + if (calculate_moving_base_yaw(bearing, dist, alt_diff)) { + _last_yaw_ms = now; + } state.gps_yaw_configured = true; #endif } else { diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index dc45830840..7bf7e2e899 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -425,13 +425,29 @@ bool AP_GPS_Backend::calculate_moving_base_yaw(AP_GPS::GPS_State &interim_state, interim_state.have_gps_yaw = true; interim_state.gps_yaw_time_ms = AP_HAL::millis(); } + goto good_yaw; } - return true; - bad_yaw: interim_state.have_gps_yaw = false; - return false; + +good_yaw: + +#if HAL_LOGGING_ENABLED + // this log message helps diagnose GPS yaw issues + AP::logger().WriteStreaming("GPYW", "TimeUS,Id,RHD,RDist,RDown,OK", + "s#dmm-", + "F-----", + "QBfffB", + AP_HAL::micros64(), + state.instance, + reported_heading_deg, + reported_distance, + reported_D, + interim_state.have_gps_yaw); +#endif + + return interim_state.have_gps_yaw; } #endif // GPS_MOVING_BASELINE