AP_ADSB: cache course-over-ground for GPS message

stops the vehicle flipping around as speed goes to/comes from 0
ArduPilot · Jan 15, 2024 · d7f37d7 · d7f37d7
1 parent d4f59cb
commit d7f37d7
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 2 deletions.
diff --git a/libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.cpp b/libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.cpp
@@ -672,8 +672,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
@@ -260,6 +260,11 @@ class AP_ADSB_Sagetech_MXS : public AP_ADSB_Backend {
         sg_flightid_t fid;
         sg_ack_t ack;
     } mxs_state;
+
+    // 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