Spam GPS message at 10hz if msg is less than 10s old

src/mfd_crossbow.cpp

@@ -77,7 +77,7 @@ MFDCrossbow::send_gps_raw_int(int8_t system_id, int8_t component_id, int32_t upT
     // Pack the message
     mavlink_msg_gps_raw_int_pack(system_id, component_id, &msg, upTime, fixType, lat, lon, alt, eph, epv, vel, cog, satellites_visible, alt_ellipsoid, h_acc, v_acc, vel_acc, hdg_acc, yaw);
 
-    // // Copy the message to the send buffer
+    // Copy the message to the send buffer
     uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
 
     m_port->write(buf, len);
@@ -100,7 +100,7 @@ MFDCrossbow::send_global_position_int(int8_t system_id, int8_t component_id, int
     // Copy the message to the send buffer
     uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
 
-    // Send the message (.write sends as bytes)
+    // Send the message
     m_port->write(buf, len);
 }
 
@@ -133,4 +133,25 @@ MFDCrossbow::SendGpsTelemetry(crsf_packet_gps_t *packet)
     send_heartbeat(system_id, component_id, system_type, autopilot_type, system_mode, custom_mode, system_state);
     send_gps_raw_int(system_id, component_id, upTime, fixType, lat, lon, alt, gps_alt, heading, groundspeed, gps_hdop, gps_sats);
     send_global_position_int(system_id, component_id, upTime, lat, lon, alt, gps_alt, heading);
+
+    // We have received gps values, so we are ok to spam these out at 10hz
+    gpsLastUpdated = millis();
 }
+
+void
+MFDCrossbow::Loop(uint32_t now)
+{
+    ModuleBase::Loop(now);
+
+    // If the GPS has been updated in the last 10 seconds, keep spamming it out at 10hz
+    bool gpsIsValid = (now < gpsLastUpdated + 10000) && gps_sats > 0;
+
+    if (now > gpsLastSent + 100 && gpsIsValid)
+    {
+        send_heartbeat(system_id, component_id, system_type, autopilot_type, system_mode, custom_mode, system_state);
+        send_gps_raw_int(system_id, component_id, upTime, fixType, lat, lon, alt, gps_alt, heading, groundspeed, gps_hdop, gps_sats);
+        send_global_position_int(system_id, component_id, upTime, lat, lon, alt, gps_alt, heading);
+
+        gpsLastSent = now;
+    }
+}

src/mfd_crossbow.h

@@ -8,13 +8,16 @@
 class MFDCrossbow : public ModuleBase
 {
 public:
-    MFDCrossbow(HardwareSerial *port) : m_port(port) {};
+    MFDCrossbow(HardwareSerial *port) : m_port(port), gpsLastSent(0), gpsLastUpdated(0) {};
 
     void send_heartbeat(uint8_t system_id, uint8_t component_id, uint8_t system_type, uint8_t autopilot_type, uint8_t system_mode, uint32_t custom_mode, uint8_t system_state);
     void send_gps_raw_int(int8_t system_id, int8_t component_id, int32_t upTime, int8_t fixType, float lat, float lon, float alt, float gps_alt, int16_t heading, float groundspeed, float gps_hdop, int16_t gps_sats);
     void send_global_position_int(int8_t system_id, int8_t component_id, int32_t upTime, float lat, float lon, float alt, float gps_alt, uint16_t heading);
     void SendGpsTelemetry(crsf_packet_gps_t *packet);
+    void Loop(uint32_t now);
 
 private:
     HardwareSerial *m_port;
+    uint32_t gpsLastSent;
+    uint32_t gpsLastUpdated;
 };