diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c index b0ee0eb77..16e69d2f6 100644 --- a/src/main/rx/crsf.c +++ b/src/main/rx/crsf.c @@ -69,6 +69,7 @@ static serialPort_t *serialPort; static timeUs_t crsfFrameStartAtUs = 0; static uint8_t telemetryBuf[CRSF_FRAME_SIZE_MAX]; static uint8_t telemetryBufLen = 0; +static uint8_t channelRes = CRSF_SUBSET_RC_RES_CONF_11B; static timeUs_t lastRcFrameTimeUs = 0; @@ -127,13 +128,24 @@ struct crsfPayloadRcChannelsPacked_s { typedef struct crsfPayloadRcChannelsPacked_s crsfPayloadRcChannelsPacked_t; -// first 5 bits in the first byte hold the first channel packed -// remaining bits hold the channel data in 11-bit format -#define CRSF_SUBSET_RC_CHANNELS_PACKED_RESOLUTION 11 -#define CRSF_SUBSET_RC_CHANNELS_PACKED_MASK 0x07FF -#define CRSF_SUBSET_RC_CHANNELS_PACKED_STARTING_CHANNEL_RESOLUTION 5 -#define CRSF_SUBSET_RC_CHANNELS_PACKED_STARTING_CHANNEL_MASK 0x1F -#define CRSF_SUBSET_RC_CHANNELS_PACKED_DIGITAL_SWITCH_RESOLUTION 1 +/* +* SUBSET RC FRAME 0x17 +* +* The structure of 0x17 frame consists of 8-bit configuration data & variable length packed channel data. +* +* definition of the configuration byte +* bits 0-4: number of first channel packed +* bits 5-6: resolution configuration of the channel data (00 -> 10 bits, 01 -> 11 bits, 10 -> 12 bits, 11 -> 13 bits) +* bit 7: reserved + +* data structure of the channel data +* - first channel packed with specified resolution +* - second channel packed with specified resolution +* - third channel packed with specified resolution +* ... +* - last channel packed with specified resolution +*/ + #if defined(USE_CRSF_LINK_STATISTICS) /* @@ -489,7 +501,6 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeState_t *rxRuntimeState) crsfChannelData[14] = rcChannels->chan14; crsfChannelData[15] = rcChannels->chan15; } -#if defined(USE_CRSF_V3) else { // use subset RC frame structure (0x17) uint8_t n; @@ -497,29 +508,61 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeState_t *rxRuntimeState) uint8_t readByteIndex = 0; uint8_t bitsMerged = 0; uint32_t readValue = 0; + uint8_t configByte = 0xFF; uint8_t startChannel = 0xFF; + uint8_t channelBits = 0xFF; + uint16_t channelMask = 0xFF; const uint8_t *payload = crsfChannelDataFrame.frame.payload; - uint8_t numOfChannels = ((crsfChannelDataFrame.frame.frameLength - CRSF_FRAME_LENGTH_TYPE_CRC) * 8 - CRSF_SUBSET_RC_CHANNELS_PACKED_STARTING_CHANNEL_RESOLUTION) / CRSF_SUBSET_RC_CHANNELS_PACKED_RESOLUTION; + uint8_t numOfChannels = 0; + + // get the configuration byte + configByte = payload[readByteIndex++]; + + // get the channel number of start channel + startChannel = configByte & CRSF_SUBSET_RC_STARTING_CHANNEL_MASK; + configByte >>= CRSF_SUBSET_RC_STARTING_CHANNEL_BITS; + + // get the channel resolution settings + channelRes = configByte & CRSF_SUBSET_RC_RES_CONFIGURATION_MASK; + configByte >>= CRSF_SUBSET_RC_RES_CONFIGURATION_BITS; + switch (channelRes) { + case CRSF_SUBSET_RC_RES_CONF_10B: + channelBits = CRSF_SUBSET_RC_RES_BITS_10B; + channelMask = CRSF_SUBSET_RC_RES_MASK_10B; + break; + default: + case CRSF_SUBSET_RC_RES_CONF_11B: + channelBits = CRSF_SUBSET_RC_RES_BITS_11B; + channelMask = CRSF_SUBSET_RC_RES_MASK_11B; + break; + case CRSF_SUBSET_RC_RES_CONF_12B: + channelBits = CRSF_SUBSET_RC_RES_BITS_12B; + channelMask = CRSF_SUBSET_RC_RES_MASK_12B; + break; + case CRSF_SUBSET_RC_RES_CONF_13B: + channelBits = CRSF_SUBSET_RC_RES_BITS_13B; + channelMask = CRSF_SUBSET_RC_RES_MASK_13B; + break; + } + + // do nothing for the reserved configuration bit + configByte >>= CRSF_SUBSET_RC_RESERVED_CONFIGURATION_BITS; + + // calculate the number of channels packed + numOfChannels = ((crsfChannelDataFrame.frame.frameLength - CRSF_FRAME_LENGTH_TYPE_CRC) * 8 - CRSF_SUBSET_RC_STARTING_CHANNEL_BITS) / channelBits; + + // unpack the channel data for (n = 0; n < numOfChannels; n++) { - while (bitsMerged < CRSF_SUBSET_RC_CHANNELS_PACKED_RESOLUTION) { + while (bitsMerged < channelBits) { readByte = payload[readByteIndex++]; - if (startChannel == 0xFF) { - // get the startChannel - startChannel = readByte & CRSF_SUBSET_RC_CHANNELS_PACKED_STARTING_CHANNEL_MASK; - readByte >>= (CRSF_SUBSET_RC_CHANNELS_PACKED_STARTING_CHANNEL_RESOLUTION + CRSF_SUBSET_RC_CHANNELS_PACKED_DIGITAL_SWITCH_RESOLUTION); - readValue |= ((uint32_t) readByte) << bitsMerged; - bitsMerged += 8 - CRSF_SUBSET_RC_CHANNELS_PACKED_STARTING_CHANNEL_RESOLUTION - CRSF_SUBSET_RC_CHANNELS_PACKED_DIGITAL_SWITCH_RESOLUTION; - } else { - readValue |= ((uint32_t) readByte) << bitsMerged; - bitsMerged += 8; - } + readValue |= ((uint32_t) readByte) << bitsMerged; + bitsMerged += 8; } - crsfChannelData[startChannel + n] = readValue & CRSF_SUBSET_RC_CHANNELS_PACKED_MASK; - readValue >>= CRSF_SUBSET_RC_CHANNELS_PACKED_RESOLUTION; - bitsMerged -= CRSF_SUBSET_RC_CHANNELS_PACKED_RESOLUTION; + crsfChannelData[startChannel + n] = readValue & channelMask; + readValue >>= channelBits; + bitsMerged -= channelBits; } } -#endif return RX_FRAME_COMPLETE; } return RX_FRAME_PENDING; @@ -528,15 +571,39 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeState_t *rxRuntimeState) STATIC_UNIT_TESTED float crsfReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan) { UNUSED(rxRuntimeState); - /* conversion from RC value to PWM - * RC PWM - * min 172 -> 988us - * mid 992 -> 1500us - * max 1811 -> 2012us - * scale factor = (2012-988) / (1811-172) = 0.62477120195241 - * offset = 988 - 172 * 0.62477120195241 = 880.53935326418548 - */ - return (0.62477120195241f * (float)crsfChannelData[chan]) + 881; + if (crsfFrame.frame.type == CRSF_FRAMETYPE_RC_CHANNELS_PACKED) { + /* conversion from RC value to PWM + * for 0x16 RC frame + * RC PWM + * min 172 -> 988us + * mid 992 -> 1500us + * max 1811 -> 2012us + * scale factor = (2012-988) / (1811-172) = 0.62477120195241 + * offset = 988 - 172 * 0.62477120195241 = 880.53935326418548 + */ + return (0.62477120195241f * (float)crsfChannelData[chan]) + 881; + } else { + /* conversion from RC value to PWM + * for 0x17 Subset RC frame + */ + float scale; + switch (channelRes) { + case CRSF_SUBSET_RC_RES_CONF_10B: + scale = 1.0f; + break; + default: + case CRSF_SUBSET_RC_RES_CONF_11B: + scale = 0.5f; + break; + case CRSF_SUBSET_RC_RES_CONF_12B: + scale = 0.25f; + break; + case CRSF_SUBSET_RC_RES_CONF_13B: + scale = 0.125f; + break; + } + return (scale * (float)crsfChannelData[chan]) + 988; + } } void crsfRxWriteTelemetryData(const void *data, int len) diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h index 15138ab80..b7e19e89a 100644 --- a/src/main/rx/crsf.h +++ b/src/main/rx/crsf.h @@ -29,6 +29,25 @@ #define CRSF_MAX_CHANNEL 16 #define CRSFV3_MAX_CHANNEL 24 +#define CRSF_SUBSET_RC_STARTING_CHANNEL_BITS 5 +#define CRSF_SUBSET_RC_STARTING_CHANNEL_MASK 0x1F +#define CRSF_SUBSET_RC_RES_CONFIGURATION_BITS 2 +#define CRSF_SUBSET_RC_RES_CONFIGURATION_MASK 0x03 +#define CRSF_SUBSET_RC_RESERVED_CONFIGURATION_BITS 1 + +#define CRSF_SUBSET_RC_RES_CONF_10B 0 +#define CRSF_SUBSET_RC_RES_BITS_10B 10 +#define CRSF_SUBSET_RC_RES_MASK_10B 0x03FF +#define CRSF_SUBSET_RC_RES_CONF_11B 1 +#define CRSF_SUBSET_RC_RES_BITS_11B 11 +#define CRSF_SUBSET_RC_RES_MASK_11B 0x07FF +#define CRSF_SUBSET_RC_RES_CONF_12B 2 +#define CRSF_SUBSET_RC_RES_BITS_12B 12 +#define CRSF_SUBSET_RC_RES_MASK_12B 0x0FFF +#define CRSF_SUBSET_RC_RES_CONF_13B 3 +#define CRSF_SUBSET_RC_RES_BITS_13B 13 +#define CRSF_SUBSET_RC_RES_MASK_13B 0x1FFF + #define CRSF_RSSI_MIN (-130) #define CRSF_RSSI_MAX 0 #define CRSF_SNR_MIN (-30) diff --git a/src/test/unit/rx_crsf_unittest.cc b/src/test/unit/rx_crsf_unittest.cc index 54bf25f8c..dfe9acde5 100644 --- a/src/test/unit/rx_crsf_unittest.cc +++ b/src/test/unit/rx_crsf_unittest.cc @@ -220,6 +220,7 @@ TEST(CrossFireTest, TestCrsfFrameStatusUnpacking) EXPECT_EQ(0, crsfChannelData[15]); } +// example of 0x16 RC frame const uint8_t capturedData[] = { 0x00,0x18,0x16,0xBD,0x08,0x9F,0xF4,0xAE,0xF7,0xBD,0xEF,0x7D,0xEF,0xFB,0xAD,0xFD,0x45,0x2B,0x5A,0x01,0x00,0x00,0x00,0x00,0x00,0x6C, 0x00,0x18,0x16,0xBD,0x08,0x9F,0xF4,0xAA,0xF7,0xBD,0xEF,0x7D,0xEF,0xFB,0xAD,0xFD,0x45,0x2B,0x5A,0x01,0x00,0x00,0x00,0x00,0x00,0x94, @@ -279,6 +280,52 @@ TEST(CrossFireTest, TestCapturedData) EXPECT_EQ(crc, crsfFrame.frame.payload[CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE]); } +// example of 0x17 Subset RC frame +/* Notes of Frame Contents +* frame type = 0x17 Subset RC Frame +* first channel packed = 4, bits 0-4 +* channel resolution = 0x01 = 11-bit, bits 5-6 +* reserved configuration = 0, bit 7 +* first channel packed (Ch4) = 0 = 0x000, 000 0000 0000, bits 8 - 18 +* second channel packed (Ch5) = 820 = 0x334, 011 0011 0100, bits 19 - 29 +* third channel packed (Ch6) = 1959 = 0x7A7, 111 1010 0111, bits 30 - 40 +* fourth channel packed (Ch7) = 2047 = 0x7FF, 111 1111 1111, bits 41 - 51 +*/ +const uint8_t capturedSubsetData[] = { + 0xC8,0x09,0x17,0x24,0x00,0xA0,0xD9,0xE9,0xFF,0x0F,0xD1 +}; + +TEST(CrossFireTest, TestCapturedSubsetData) +{ + crsfFrame = *(const crsfFrame_t*)capturedSubsetData; + crsfFrameDone = true; + memcpy(&crsfChannelDataFrame, &crsfFrame, sizeof(crsfFrame)); + + uint8_t status = crsfFrameStatus(); + EXPECT_EQ(RX_FRAME_COMPLETE, status); + EXPECT_FALSE(crsfFrameDone); + EXPECT_EQ(CRSF_SYNC_BYTE, crsfFrame.frame.deviceAddress); + EXPECT_EQ(CRSF_FRAMETYPE_SUBSET_RC_CHANNELS_PACKED, crsfFrame.frame.type); + + uint8_t crc = crsfFrameCRC(); + uint8_t startChannel = crsfFrame.frame.payload[0] & CRSF_SUBSET_RC_STARTING_CHANNEL_MASK; + uint8_t channelRes = (crsfFrame.frame.payload[0] >> CRSF_SUBSET_RC_STARTING_CHANNEL_BITS) & CRSF_SUBSET_RC_RES_CONFIGURATION_MASK; + uint8_t reservedBit = (crsfFrame.frame.payload[0] >> (CRSF_SUBSET_RC_STARTING_CHANNEL_BITS + CRSF_SUBSET_RC_RES_CONFIGURATION_MASK)) & CRSF_SUBSET_RC_RESERVED_CONFIGURATION_BITS; + EXPECT_EQ(crc, crsfFrame.frame.payload[crsfFrame.frame.frameLength - 2]); + EXPECT_EQ(4, startChannel); + EXPECT_EQ(1, channelRes); + EXPECT_EQ(0, reservedBit); + + EXPECT_EQ(0, crsfChannelData[4]); + EXPECT_EQ(820, crsfChannelData[5]); + EXPECT_EQ(1959, crsfChannelData[6]); + EXPECT_EQ(2047, crsfChannelData[7]); + + EXPECT_EQ(988, (uint16_t)crsfReadRawRC(NULL, 4)); + EXPECT_EQ(1398, (uint16_t)crsfReadRawRC(NULL, 5)); + EXPECT_EQ(1967, (uint16_t)crsfReadRawRC(NULL, 6)); + EXPECT_EQ(2011, (uint16_t)crsfReadRawRC(NULL, 7)); +} TEST(CrossFireTest, TestCrsfDataReceive) {