feat: send handset RTC to ELRS modules

radio/src/pulses/crossfire.cpp

@@ -28,6 +28,9 @@
 #include "mixer_scheduler.h"
 #include "hal/module_driver.h"
 #include "hal/module_port.h"
+#if defined(RTCLOCK)
+#include "rtc.h"
+#endif
 
 #include "crossfire.h"
 #include "telemetry/crossfire.h"
@@ -45,6 +48,9 @@
 #define MODULE_ALIVE_TIMEOUT  50                      // if the module has sent a valid frame within 500ms it is declared alive
 static tmr10ms_t lastAlive[NUM_MODULES];              // last time stamp module sent CRSF frames
 static bool moduleAlive[NUM_MODULES];                 // module alive status
+#if defined(RTCLOCK)
+static bool timeSyncSent[NUM_MODULES];
+#endif
 
 uint8_t createCrossfireBindFrame(uint8_t moduleIdx, uint8_t * frame)
 {
@@ -92,6 +98,38 @@ uint8_t createCrossfireModelIDFrame(uint8_t moduleIdx, uint8_t * frame)
   return buf - frame;
 }
 
+#if defined(RTCLOCK)
+uint8_t createCrossfireTimeFrame(uint8_t moduleIdx, uint8_t * frame)
+{
+  (void)moduleIdx;
+
+  struct gtm tm;
+  gettime(&tm);
+
+  uint8_t * buf = frame;
+  *buf++ = MODULE_ADDRESS;
+  *buf++ = 17;                              // type + dest + orig + 13 MSP bytes + CRC
+  *buf++ = MSP_WRITE_ID;
+  *buf++ = VIDEO_RECEIVER_ADDRESS;
+  *buf++ = RADIO_ADDRESS;
+  *buf++ = MSP_V2_STATUS_START;
+  *buf++ = 0;                               // MSP flags
+  *buf++ = MSP_ELRS_BACKPACK_SET_RTC & 0xFF;
+  *buf++ = MSP_ELRS_BACKPACK_SET_RTC >> 8;
+  *buf++ = 6;                               // MSP payload size, low byte
+  *buf++ = 0;                               // MSP payload size, high byte
+  *buf++ = tm.tm_year;
+  *buf++ = tm.tm_mon;
+  *buf++ = tm.tm_mday;
+  *buf++ = tm.tm_hour;
+  *buf++ = tm.tm_min;
+  *buf++ = tm.tm_sec;
+  *buf++ = crc8(frame + 6, 11);             // MSP CRC over flags..payload
+  *buf++ = crc8(frame + 2, 16);             // CRSF CRC over type..MSP CRC
+  return buf - frame;
+}
+#endif
+
 // Range for pulses (channels output) is [-1024:+1024]
 uint8_t createCrossfireChannelsFrame(uint8_t moduleIdx, uint8_t * frame, int16_t * pulses)
 {
@@ -182,6 +220,13 @@ static void setupPulsesCrossfire(uint8_t module, uint8_t*& p_buf,
     } else if (moduleState[module].mode == MODULE_MODE_BIND) {
       p_buf += createCrossfireBindFrame(module, p_buf);
       moduleState[module].mode = MODULE_MODE_NORMAL;
+#if defined(RTCLOCK)
+    } else if (crossfireModuleStatus[module].isELRS &&
+               crossfireModuleStatus[module].queryCompleted &&
+               !timeSyncSent[module]) {
+      p_buf += createCrossfireTimeFrame(module, p_buf);
+      timeSyncSent[module] = true;
+#endif
     } else {
       /* TODO: nChannels */
       p_buf += createCrossfireChannelsFrame(module, p_buf, channels);
@@ -373,6 +418,10 @@ static void _soft_irq_trigger(void* param)
 
 static void* crossfireInit(uint8_t module)
 {
+#if defined(RTCLOCK)
+  timeSyncSent[module] = false;
+#endif
+
   etx_module_state_t* mod_st = nullptr;
   etx_serial_init params(crsfSerialParams);
 
@@ -439,6 +488,10 @@ static void crossfireDeInit(void* ctx)
 {
   auto mod_st = (etx_module_state_t*)ctx;
 
+#if defined(RTCLOCK)
+  timeSyncSent[modulePortGetModule(mod_st)] = false;
+#endif
+
   memset(&crossfireModuleStatus[modulePortGetModule(mod_st)], 0,
          sizeof(CrossfireModuleStatus));
 

radio/src/telemetry/crossfire.cpp

@@ -412,7 +412,6 @@ void processCrossfireTelemetryFrame(uint8_t module, uint8_t* rxBuffer,
       }
       break;
 
-#if defined(LUA)
     default:
       if (id == DEVICE_INFO_ID && rxBuffer[4]== MODULE_ADDRESS) {
         uint8_t nameSize = rxBuffer[1] - 18;
@@ -437,10 +436,11 @@ void processCrossfireTelemetryFrame(uint8_t module, uint8_t* rxBuffer,
         crossfireModuleStatus[module].queryCompleted = true;
       }
 
+#if defined(LUA)
       // destination address and CRC are skipped
       pushTelemetryDataToQueues(rxBuffer + 1, rxBufferCount - 2);
-      break;
 #endif
+      break;
   }
 }
 

radio/src/telemetry/crossfire.h

@@ -53,6 +53,14 @@
 #define COMMAND_ID                     0x32
 #define RADIO_ID                       0x3A
 
+#if defined(RTCLOCK)
+// MSP-over-CRSF write to the ExpressLRS backpack, via the video receiver address
+#define MSP_WRITE_ID                   0x7C
+#define VIDEO_RECEIVER_ADDRESS         0x14
+#define MSP_ELRS_BACKPACK_SET_RTC      0x030E
+#define MSP_V2_STATUS_START            0x50  // version 2, start of frame, seq 0
+#endif
+
 #define UART_SYNC                      0xC8
 #define SUBCOMMAND_CRSF                0x10
 #define COMMAND_MODEL_SELECT_ID        0x05

radio/src/tests/crossfire.cpp

@@ -21,11 +21,17 @@
 
 #include "gtest/gtest.h"
 #include "gtests.h"
+#if defined(RTCLOCK)
+#include "rtc.h"
+#endif
 #include "telemetry/telemetry.h"
 
 #if defined(CROSSFIRE)
 
 uint8_t createCrossfireChannelsFrame(uint8_t moduleIdx, uint8_t * frame, int16_t * pulses);
+#if defined(RTCLOCK)
+uint8_t createCrossfireTimeFrame(uint8_t moduleIdx, uint8_t * frame);
+#endif
 TEST(Crossfire, createCrossfireChannelsFrame)
 {
   int16_t pulsesStart[MAX_TRAINER_CHANNELS];
@@ -41,6 +47,38 @@ TEST(Crossfire, createCrossfireChannelsFrame)
   // TODO check
 }
 
+#if defined(RTCLOCK)
+TEST(Crossfire, createCrossfireTimeFrame)
+{
+  struct gtm expected = {56, 34, 12, 1, 0, 126, 0, 0};
+  const gtime_t previousTime = g_rtcTime;
+  g_rtcTime = gmktime(&expected);
+
+  uint8_t frame[CROSSFIRE_FRAME_MAXLEN] = {};
+  const uint8_t length = createCrossfireTimeFrame(EXTERNAL_MODULE, frame);
+
+  // MSPv2 SET_RTC addressed to the video receiver; see ExpressLRS
+  // VideoReceiverEndpoint, which decodes this and relays it to the backpack.
+  const uint8_t expectedFrame[] = {
+    MODULE_ADDRESS, 0x11,
+    MSP_WRITE_ID, VIDEO_RECEIVER_ADDRESS, RADIO_ADDRESS,
+    MSP_V2_STATUS_START, 0x00, 0x0E, 0x03, 0x06, 0x00,
+    126, 0, 1, 12, 34, 56,
+    0x92, 0x35
+  };
+
+  EXPECT_EQ(length, sizeof(expectedFrame));
+  EXPECT_EQ(memcmp(frame, expectedFrame, sizeof(expectedFrame)), 0);
+
+  // Both CRCs are crc8 poly 0xD5: the MSP one covers flags..payload, the CRSF
+  // one covers everything from the type byte to the MSP CRC inclusive.
+  EXPECT_EQ(frame[17], crc8(&frame[6], 11));
+  EXPECT_EQ(frame[18], crc8(&frame[2], 16));
+
+  g_rtcTime = previousTime;
+}
+#endif
+
 TEST(Crossfire, crc8)
 {
   uint8_t frame[] = { 0x00, 0x0C, 0x14, 0x00, 0x00, 0x00, 0x00, 0xFF, 0x01, 0x03, 0x00, 0x00, 0x00, 0xF4 };
@@ -278,4 +316,3 @@ TEST(Crossfire, frameParser_multipleJumboFrames)
 }
 #endif // HARDWARE_EXTERNAL_MODULE
 #endif
-