diff --git a/radio/src/pulses/crossfire.cpp b/radio/src/pulses/crossfire.cpp index 36cbe64fa06..cc241d7f1e6 100644 --- a/radio/src/pulses/crossfire.cpp +++ b/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)); diff --git a/radio/src/telemetry/crossfire.cpp b/radio/src/telemetry/crossfire.cpp index 0ec18573bbd..47f55d4b924 100644 --- a/radio/src/telemetry/crossfire.cpp +++ b/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; } } diff --git a/radio/src/telemetry/crossfire.h b/radio/src/telemetry/crossfire.h index bdfc4956180..52a5001f764 100644 --- a/radio/src/telemetry/crossfire.h +++ b/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 diff --git a/radio/src/tests/crossfire.cpp b/radio/src/tests/crossfire.cpp index 0c5a52010a6..9c030118e21 100644 --- a/radio/src/tests/crossfire.cpp +++ b/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 -