spam gps msg at 10hz

ExpressLRS · Jul 2, 2024 · cd96da3 · cd96da3
1 parent 6a1fdd1
commit cd96da3
Showing 1 changed file with 15 additions and 0 deletions.
diff --git a/src/Vrx_main.cpp b/src/Vrx_main.cpp
@@ -72,6 +72,8 @@ bool gotInitialPacket = false;
 bool headTrackingEnabled = false;
 uint32_t lastSentRequest = 0;
 
+uint32_t lastSentGPS = 0;
+
 device_t *ui_devices[] = {
 #ifdef PIN_LED
  &LED_device,
@@ -100,6 +102,9 @@ MSP msp;
 ELRS_EEPROM eeprom;
 VrxBackpackConfig config;
 
+uint8_t cachedGPS[64];
+uint8_t cachedLen = 0;
+
 #ifdef RAPIDFIRE_BACKPACK
  Rapidfire vrxModule;
 #elif defined(RX5808_BACKPACK)
@@ -264,6 +269,8 @@ void ProcessMSPPacket(mspPacket_t *packet)
  }
  DBGLN(""); // Extra line for serial output readability
  Serial.write(packet->payload, packet->payloadSize);
+ memcpy(cachedGPS, packet->payload, packet->payloadSize);
+ cachedLen = packet->payloadSize;
  break;
  default:
  DBGLN("Unknown command from ESPNOW");
@@ -533,6 +540,14 @@ void loop()
  lastSentRequest = now;
  }
 
+ // spam out GPS msgs
+ if (cachedLen > 0 && now - lastSentGPS > 100 && connectionState != binding)
+ {
+ DBGLN("Send cached GPS...");
+ Serial.write(cachedGPS, cachedLen);
+ lastSentGPS = now;
+ }
+
 #if !defined(NO_AUTOBIND)
  // Power cycle must be done within 30s. Long timeout to allow goggles to boot and shutdown correctly e.g. Orqa.
  if (now > BINDING_TIMEOUT && config.GetBootCount() > 0)