From 05a22aaffcc2e7b6226d8156d5d06323e8eb3110 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 12 Jan 2024 17:05:39 +1100 Subject: [PATCH] AP_ADSB: cache course-over-ground for GPS message stops the vehicle flipping around as speed goes to/comes from 0 --- libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.cpp | 6 ++++-- libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.h | 4 ++++ 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.cpp b/libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.cpp index 6f0803d2796fd..67d3e73e25d8c 100644 --- a/libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.cpp +++ b/libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.cpp @@ -621,8 +621,10 @@ void AP_ADSB_Sagetech_MXS::send_gps_msg() const float speed_knots = speed.length() * M_PER_SEC_TO_KNOTS; snprintf((char*)&gps.grdSpeed, 7, "%03u.%02u", (unsigned)speed_knots, unsigned((speed_knots - (int)speed_knots) * 1.0E2)); - const float heading = wrap_360(degrees(speed.angle())); - snprintf((char*)&gps.grdTrack, 9, "%03u.%04u", unsigned(heading), unsigned((heading - (int)heading) * 1.0E4)); + if (!is_zero(speed_knots)) { + cog = wrap_360(degrees(speed.angle())); + } + snprintf((char*)&gps.grdTrack, 9, "%03u.%04u", unsigned(cog), unsigned((cog - (int)cog) * 1.0E4)); gps.latNorth = (latitude >= 0 ? true: false); diff --git a/libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.h b/libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.h index 5e8c841039bfe..fe8dd0d822b56 100644 --- a/libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.h +++ b/libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.h @@ -266,6 +266,10 @@ class AP_ADSB_Sagetech_MXS : public AP_ADSB_Backend { void populate_op_climbrate(const struct AP_ADSB::Loc &loc); void populate_op_airspeed_and_heading(const struct AP_ADSB::Loc &loc); + // last course-over-ground calculated from groundspeed vector. + // This is cached so we don't flip to a COG of 90-degrees when + // we stop moving. + float cog; }; #endif // HAL_ADSB_SAGETECH_MXS_ENABLED