AP_ADSB: cache course-over-ground for GPS message

stops the vehicle flipping around as speed goes to/comes from 0
ArduPilot · Jun 19, 2024 · 05a22aa · 05a22aa
1 parent 43c604f
commit 05a22aa
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 2 deletions.
diff --git 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
@@ -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