Restructure CRSF frame processing and RC channel handling
Prevents channel data packet corruption if other packets are receved before processing can complete.
This commit is contained in:
parent
dd0b45dc87
commit
09406ad9d9
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
|
|
Loading…
Reference in New Issue