diff --git a/radio/src/pulses/crossfire.cpp b/radio/src/pulses/crossfire.cpp index 36cbe64fa..1cd9259c4 100644 --- a/radio/src/pulses/crossfire.cpp +++ b/radio/src/pulses/crossfire.cpp @@ -96,20 +96,20 @@ uint8_t createCrossfireModelIDFrame(uint8_t moduleIdx, uint8_t * frame) uint8_t createCrossfireChannelsFrame(uint8_t moduleIdx, uint8_t * frame, int16_t * pulses) { // - // sends channel data and also communicates commanded armed status in arming mode Switch. - // frame len 24 -> arming mode CH5: module will use channel 5 - // frame len 25 -> arming mode Switch: send commanded armed status in extra byte after channel data - // - ModuleData *md = &g_model.moduleData[moduleIdx]; - - uint8_t armingMode = md->crsf.crsfArmingMode; // 0 = Channel mode, 1 = Switch mode - uint8_t lenAdjust = (armingMode == ARMING_MODE_SWITCH) ? 1 : 0; - + // sends channel data and also communicates status information in status byte: + // - arming status in Switch mode (bit 0) + // - arming mode Switch or CH5 (bit 1) + // - bits 2-7 spare + // uint8_t * buf = frame; *buf++ = MODULE_ADDRESS; - *buf++ = 24 + lenAdjust; // 1(ID) + 22(channel data) + (+1 extra byte if Switch mode) + 1(CRC) + *buf++ = 25; // 1(ID) + 22(channel data) + 1(extra status byte) + 1(CRC) uint8_t * crc_start = buf; *buf++ = CHANNELS_ID; + + // + // assemble channel data + // uint32_t bits = 0; uint8_t bitsavailable = 0; for (int i=0; icrsf.crsfArmingMode == ARMING_MODE_SWITCH) { swsrc_t sw = md->crsf.crsfArmingTrigger; - *buf++ = (sw != SWSRC_NONE) && getSwitch(sw, 0); // commanded armed status in Switch mode + *buf = (sw != SWSRC_NONE) && getSwitch(sw, 0); // commanded armed status in Switch mode + } else { + *buf = 0x02; // flag arming mode CH5 } + + buf++; - *buf++ = crc8(crc_start, 23 + lenAdjust); + // + // add crc + // + *buf++ = crc8(crc_start, 24); + return buf - frame; } diff --git a/radio/src/tests/crossfire.cpp b/radio/src/tests/crossfire.cpp index 0c5a52010..b904a454f 100644 --- a/radio/src/tests/crossfire.cpp +++ b/radio/src/tests/crossfire.cpp @@ -22,12 +22,20 @@ #include "gtest/gtest.h" #include "gtests.h" #include "telemetry/telemetry.h" +#include "telemetry/crossfire.h" +#include "crc.h" #if defined(CROSSFIRE) uint8_t createCrossfireChannelsFrame(uint8_t moduleIdx, uint8_t * frame, int16_t * pulses); + +// Spec-defined part of the frame (0x16 RC Channels Packed): sync byte, type, +// 16 x 11-bit channel packing. Expected bytes computed independently, not +// derived from createCrossfireChannelsFrame() itself. TEST(Crossfire, createCrossfireChannelsFrame) { + MODEL_RESET(); + int16_t pulsesStart[MAX_TRAINER_CHANNELS]; uint8_t crossfire[CROSSFIRE_FRAME_MAXLEN]; @@ -38,7 +46,71 @@ TEST(Crossfire, createCrossfireChannelsFrame) createCrossfireChannelsFrame(EXTERNAL_MODULE, crossfire, pulsesStart); - // TODO check + ASSERT_EQ(crossfire[0], MODULE_ADDRESS); + ASSERT_EQ(crossfire[2], CHANNELS_ID); + + const uint8_t expectedChannelData[22] = { + 0xAD, 0xA0, 0x88, 0x5E, 0xC0, 0x73, 0xA4, 0x56, 0x51, 0x4C, 0x6F, + 0xE0, 0x33, 0x22, 0x2B, 0x27, 0x9A, 0x57, 0xF0, 0x1A, 0x99, 0xD5 + }; + ASSERT_EQ(memcmp(&crossfire[3], expectedChannelData, sizeof(expectedChannelData)), 0); +} + +// Status byte after the 0x16 payload is an ExpressLRS extension, not TBS CRSF +// spec (semantics per ExpressLRS's TXModuleEndpoint.cpp / crsf_protocol.h). +// Frame is always 25 bytes (1 ID + 22 channel data + 1 status + 1 CRC); +// bit 0 = commanded armed status (Switch mode only), bit 1 = arming mode is CH5. +TEST(Crossfire, ExpressLRSArmingExtension_CH5Mode) +{ + MODEL_RESET(); + + int16_t pulsesStart[MAX_TRAINER_CHANNELS]; + uint8_t crossfire[CROSSFIRE_FRAME_MAXLEN]; + + memset(crossfire, 0, sizeof(crossfire)); + for (int i=0; i not armed, bit 1 clear (Switch mode) + + uint8_t crc = crc8(&crossfire[2], 24); + ASSERT_EQ(crossfire[26], crc); } TEST(Crossfire, crc8)