diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c index afdda5c8d..c4eeaef2a 100644 --- a/src/main/rx/crsf.c +++ b/src/main/rx/crsf.c @@ -60,6 +60,7 @@ STATIC_UNIT_TESTED bool crsfFrameDone = false; STATIC_UNIT_TESTED crsfFrame_t crsfFrame; +STATIC_UNIT_TESTED crsfFrame_t crsfChannelDataFrame; STATIC_UNIT_TESTED uint32_t crsfChannelData[CRSF_MAX_CHANNEL]; static serialPort_t *serialPort; @@ -251,50 +252,54 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *data) if (crsfFramePosition < fullFrameLength) { crsfFrame.bytes[crsfFramePosition++] = (uint8_t)c; - crsfFrameDone = crsfFramePosition < fullFrameLength ? false : true; - if (crsfFrameDone) { + if (crsfFramePosition >= fullFrameLength) { crsfFramePosition = 0; - if (crsfFrame.frame.type != CRSF_FRAMETYPE_RC_CHANNELS_PACKED) { - const uint8_t crc = crsfFrameCRC(); - if (crc == crsfFrame.bytes[fullFrameLength - 1]) { - switch (crsfFrame.frame.type) - { -#if defined(USE_TELEMETRY_CRSF) && defined(USE_MSP_OVER_TELEMETRY) - case CRSF_FRAMETYPE_MSP_REQ: - case CRSF_FRAMETYPE_MSP_WRITE: { - uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE; - if (bufferCrsfMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE)) { - crsfScheduleMspResponse(); - } - break; + const uint8_t crc = crsfFrameCRC(); + if (crc == crsfFrame.bytes[fullFrameLength - 1]) { + switch (crsfFrame.frame.type) + { + case CRSF_FRAMETYPE_RC_CHANNELS_PACKED: + if (crsfFrame.frame.deviceAddress == CRSF_ADDRESS_FLIGHT_CONTROLLER) { + crsfFrameDone = true; + memcpy(&crsfChannelDataFrame, &crsfFrame, sizeof(crsfFrame)); } + break; + +#if defined(USE_TELEMETRY_CRSF) && defined(USE_MSP_OVER_TELEMETRY) + case CRSF_FRAMETYPE_MSP_REQ: + case CRSF_FRAMETYPE_MSP_WRITE: { + uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE; + if (bufferCrsfMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE)) { + crsfScheduleMspResponse(); + } + break; + } #endif #if defined(USE_CRSF_CMS_TELEMETRY) - case CRSF_FRAMETYPE_DEVICE_PING: - crsfScheduleDeviceInfoResponse(); - break; - case CRSF_FRAMETYPE_DISPLAYPORT_CMD: { - uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE; - crsfProcessDisplayPortCmd(frameStart); - break; - } + case CRSF_FRAMETYPE_DEVICE_PING: + crsfScheduleDeviceInfoResponse(); + break; + case CRSF_FRAMETYPE_DISPLAYPORT_CMD: { + uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE; + crsfProcessDisplayPortCmd(frameStart); + break; + } #endif #if defined(USE_CRSF_LINK_STATISTICS) - case CRSF_FRAMETYPE_LINK_STATISTICS: { - // if to FC and 10 bytes + CRSF_FRAME_ORIGIN_DEST_SIZE - if ((rssiSource == RSSI_SOURCE_RX_PROTOCOL_CRSF) && - (crsfFrame.frame.deviceAddress == CRSF_ADDRESS_FLIGHT_CONTROLLER) && - (crsfFrame.frame.frameLength == CRSF_FRAME_ORIGIN_DEST_SIZE + CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE)) { - const crsfLinkStatistics_t* statsFrame = (const crsfLinkStatistics_t*)&crsfFrame.frame.payload; - handleCrsfLinkStatisticsFrame(statsFrame, currentTimeUs); - } - break; - } -#endif - default: - break; + case CRSF_FRAMETYPE_LINK_STATISTICS: { + // if to FC and 10 bytes + CRSF_FRAME_ORIGIN_DEST_SIZE + if ((rssiSource == RSSI_SOURCE_RX_PROTOCOL_CRSF) && + (crsfFrame.frame.deviceAddress == CRSF_ADDRESS_FLIGHT_CONTROLLER) && + (crsfFrame.frame.frameLength == CRSF_FRAME_ORIGIN_DEST_SIZE + CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE)) { + const crsfLinkStatistics_t* statsFrame = (const crsfLinkStatistics_t*)&crsfFrame.frame.payload; + handleCrsfLinkStatisticsFrame(statsFrame, currentTimeUs); + } + break; } +#endif + default: + break; } } } @@ -310,32 +315,26 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeState_t *rxRuntimeState) #endif if (crsfFrameDone) { crsfFrameDone = false; - if (crsfFrame.frame.type == CRSF_FRAMETYPE_RC_CHANNELS_PACKED) { - // CRC includes type and payload of each frame - const uint8_t crc = crsfFrameCRC(); - if (crc != crsfFrame.frame.payload[CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE]) { - return RX_FRAME_PENDING; - } - // unpack the RC channels - const crsfPayloadRcChannelsPacked_t* const rcChannels = (crsfPayloadRcChannelsPacked_t*)&crsfFrame.frame.payload; - crsfChannelData[0] = rcChannels->chan0; - crsfChannelData[1] = rcChannels->chan1; - crsfChannelData[2] = rcChannels->chan2; - crsfChannelData[3] = rcChannels->chan3; - crsfChannelData[4] = rcChannels->chan4; - crsfChannelData[5] = rcChannels->chan5; - crsfChannelData[6] = rcChannels->chan6; - crsfChannelData[7] = rcChannels->chan7; - crsfChannelData[8] = rcChannels->chan8; - crsfChannelData[9] = rcChannels->chan9; - crsfChannelData[10] = rcChannels->chan10; - crsfChannelData[11] = rcChannels->chan11; - crsfChannelData[12] = rcChannels->chan12; - crsfChannelData[13] = rcChannels->chan13; - crsfChannelData[14] = rcChannels->chan14; - crsfChannelData[15] = rcChannels->chan15; - return RX_FRAME_COMPLETE; - } + + // unpack the RC channels + const crsfPayloadRcChannelsPacked_t* const rcChannels = (crsfPayloadRcChannelsPacked_t*)&crsfChannelDataFrame.frame.payload; + crsfChannelData[0] = rcChannels->chan0; + crsfChannelData[1] = rcChannels->chan1; + crsfChannelData[2] = rcChannels->chan2; + crsfChannelData[3] = rcChannels->chan3; + crsfChannelData[4] = rcChannels->chan4; + crsfChannelData[5] = rcChannels->chan5; + crsfChannelData[6] = rcChannels->chan6; + crsfChannelData[7] = rcChannels->chan7; + crsfChannelData[8] = rcChannels->chan8; + crsfChannelData[9] = rcChannels->chan9; + crsfChannelData[10] = rcChannels->chan10; + crsfChannelData[11] = rcChannels->chan11; + crsfChannelData[12] = rcChannels->chan12; + crsfChannelData[13] = rcChannels->chan13; + crsfChannelData[14] = rcChannels->chan14; + crsfChannelData[15] = rcChannels->chan15; + return RX_FRAME_COMPLETE; } return RX_FRAME_PENDING; } diff --git a/src/test/unit/rx_crsf_unittest.cc b/src/test/unit/rx_crsf_unittest.cc index 7476c5297..ba4cd54e4 100644 --- a/src/test/unit/rx_crsf_unittest.cc +++ b/src/test/unit/rx_crsf_unittest.cc @@ -50,6 +50,7 @@ extern "C" { extern bool crsfFrameDone; extern crsfFrame_t crsfFrame; + extern crsfFrame_t crsfChannelDataFrame; extern uint32_t crsfChannelData[CRSF_MAX_CHANNEL]; uint32_t dummyTimeUs; @@ -178,6 +179,7 @@ TEST(CrossFireTest, TestCrsfFrameStatusUnpacking) const uint8_t crc = crsfFrameCRC(); crsfFrame.frame.payload[CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE] = crc; + memcpy(&crsfChannelDataFrame, &crsfFrame, sizeof(crsfFrame)); const uint8_t status = crsfFrameStatus(); EXPECT_EQ(RX_FRAME_COMPLETE, status); EXPECT_EQ(false, crsfFrameDone); @@ -222,6 +224,7 @@ TEST(CrossFireTest, TestCapturedData) const crsfRcChannelsFrame_t *framePtr = (const crsfRcChannelsFrame_t*)capturedData; crsfFrame = *(const crsfFrame_t*)framePtr; crsfFrameDone = true; + memcpy(&crsfChannelDataFrame, &crsfFrame, sizeof(crsfFrame)); uint8_t status = crsfFrameStatus(); EXPECT_EQ(RX_FRAME_COMPLETE, status); EXPECT_EQ(false, crsfFrameDone); @@ -244,6 +247,7 @@ TEST(CrossFireTest, TestCapturedData) ++framePtr; crsfFrame = *(const crsfFrame_t*)framePtr; crsfFrameDone = true; + memcpy(&crsfChannelDataFrame, &crsfFrame, sizeof(crsfFrame)); status = crsfFrameStatus(); EXPECT_EQ(RX_FRAME_COMPLETE, status); EXPECT_EQ(false, crsfFrameDone); @@ -268,7 +272,7 @@ TEST(CrossFireTest, TestCrsfDataReceive) for (unsigned int ii = 0; ii < sizeof(crsfRcChannelsFrame_t); ++ii) { crsfDataReceive(*pData++); } - EXPECT_EQ(true, crsfFrameDone); + EXPECT_EQ(false, crsfFrameDone); // data is not a valid rc channels frame so don't expect crsfFrameDone to be true EXPECT_EQ(CRSF_ADDRESS_BROADCAST, crsfFrame.frame.deviceAddress); EXPECT_EQ(CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC, crsfFrame.frame.frameLength); EXPECT_EQ(CRSF_FRAMETYPE_RC_CHANNELS_PACKED, crsfFrame.frame.type);