rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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:
Bruce Luckcuck 2019-11-17 10:37:41 -05:00
parent dd0b45dc87
commit 09406ad9d9
2 changed files with 66 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

123
src/main/rx/crsf.c
View File

@ -60,6 +60,7 @@
STATIC_UNIT_TESTED bool crsfFrameDone = false; STATIC_UNIT_TESTED bool crsfFrameDone = false;
STATIC_UNIT_TESTED crsfFrame_t crsfFrame; STATIC_UNIT_TESTED crsfFrame_t crsfFrame;
STATIC_UNIT_TESTED crsfFrame_t crsfChannelDataFrame;
STATIC_UNIT_TESTED uint32_t crsfChannelData[CRSF_MAX_CHANNEL]; STATIC_UNIT_TESTED uint32_t crsfChannelData[CRSF_MAX_CHANNEL];
static serialPort_t *serialPort; static serialPort_t *serialPort;
@ -251,50 +252,54 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *data)
if (crsfFramePosition < fullFrameLength) { if (crsfFramePosition < fullFrameLength) {
crsfFrame.bytes[crsfFramePosition++] = (uint8_t)c; crsfFrame.bytes[crsfFramePosition++] = (uint8_t)c;
crsfFrameDone = crsfFramePosition < fullFrameLength ? false : true; if (crsfFramePosition >= fullFrameLength) {
if (crsfFrameDone) {
crsfFramePosition = 0; crsfFramePosition = 0;
if (crsfFrame.frame.type != CRSF_FRAMETYPE_RC_CHANNELS_PACKED) { const uint8_t crc = crsfFrameCRC();
const uint8_t crc = crsfFrameCRC(); if (crc == crsfFrame.bytes[fullFrameLength - 1]) {
if (crc == crsfFrame.bytes[fullFrameLength - 1]) { switch (crsfFrame.frame.type)
switch (crsfFrame.frame.type) {
{ case CRSF_FRAMETYPE_RC_CHANNELS_PACKED:
#if defined(USE_TELEMETRY_CRSF) && defined(USE_MSP_OVER_TELEMETRY) if (crsfFrame.frame.deviceAddress == CRSF_ADDRESS_FLIGHT_CONTROLLER) {
case CRSF_FRAMETYPE_MSP_REQ: crsfFrameDone = true;
case CRSF_FRAMETYPE_MSP_WRITE: { memcpy(&crsfChannelDataFrame, &crsfFrame, sizeof(crsfFrame));
uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE;
if (bufferCrsfMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE)) {
crsfScheduleMspResponse();
}
break;
} }
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 #endif
#if defined(USE_CRSF_CMS_TELEMETRY) #if defined(USE_CRSF_CMS_TELEMETRY)
case CRSF_FRAMETYPE_DEVICE_PING: case CRSF_FRAMETYPE_DEVICE_PING:
crsfScheduleDeviceInfoResponse(); crsfScheduleDeviceInfoResponse();
break; break;
case CRSF_FRAMETYPE_DISPLAYPORT_CMD: { case CRSF_FRAMETYPE_DISPLAYPORT_CMD: {
uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE; uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE;
crsfProcessDisplayPortCmd(frameStart); crsfProcessDisplayPortCmd(frameStart);
break; break;
} }
#endif #endif
#if defined(USE_CRSF_LINK_STATISTICS) #if defined(USE_CRSF_LINK_STATISTICS)
case CRSF_FRAMETYPE_LINK_STATISTICS: { case CRSF_FRAMETYPE_LINK_STATISTICS: {
// if to FC and 10 bytes + CRSF_FRAME_ORIGIN_DEST_SIZE // if to FC and 10 bytes + CRSF_FRAME_ORIGIN_DEST_SIZE
if ((rssiSource == RSSI_SOURCE_RX_PROTOCOL_CRSF) && if ((rssiSource == RSSI_SOURCE_RX_PROTOCOL_CRSF) &&
(crsfFrame.frame.deviceAddress == CRSF_ADDRESS_FLIGHT_CONTROLLER) && (crsfFrame.frame.deviceAddress == CRSF_ADDRESS_FLIGHT_CONTROLLER) &&
(crsfFrame.frame.frameLength == CRSF_FRAME_ORIGIN_DEST_SIZE + CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE)) { (crsfFrame.frame.frameLength == CRSF_FRAME_ORIGIN_DEST_SIZE + CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE)) {
const crsfLinkStatistics_t* statsFrame = (const crsfLinkStatistics_t*)&crsfFrame.frame.payload; const crsfLinkStatistics_t* statsFrame = (const crsfLinkStatistics_t*)&crsfFrame.frame.payload;
handleCrsfLinkStatisticsFrame(statsFrame, currentTimeUs); handleCrsfLinkStatisticsFrame(statsFrame, currentTimeUs);
} }
break; break;
}
#endif
default:
break;
} }
#endif
default:
break;
} }
} }
} }
@ -310,32 +315,26 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeState_t *rxRuntimeState)
#endif #endif
if (crsfFrameDone) { if (crsfFrameDone) {
crsfFrameDone = false; crsfFrameDone = false;
if (crsfFrame.frame.type == CRSF_FRAMETYPE_RC_CHANNELS_PACKED) {
// CRC includes type and payload of each frame // unpack the RC channels
const uint8_t crc = crsfFrameCRC(); const crsfPayloadRcChannelsPacked_t* const rcChannels = (crsfPayloadRcChannelsPacked_t*)&crsfChannelDataFrame.frame.payload;
if (crc != crsfFrame.frame.payload[CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE]) { crsfChannelData[0] = rcChannels->chan0;
return RX_FRAME_PENDING; crsfChannelData[1] = rcChannels->chan1;
} crsfChannelData[2] = rcChannels->chan2;
// unpack the RC channels crsfChannelData[3] = rcChannels->chan3;
const crsfPayloadRcChannelsPacked_t* const rcChannels = (crsfPayloadRcChannelsPacked_t*)&crsfFrame.frame.payload; crsfChannelData[4] = rcChannels->chan4;
crsfChannelData[0] = rcChannels->chan0; crsfChannelData[5] = rcChannels->chan5;
crsfChannelData[1] = rcChannels->chan1; crsfChannelData[6] = rcChannels->chan6;
crsfChannelData[2] = rcChannels->chan2; crsfChannelData[7] = rcChannels->chan7;
crsfChannelData[3] = rcChannels->chan3; crsfChannelData[8] = rcChannels->chan8;
crsfChannelData[4] = rcChannels->chan4; crsfChannelData[9] = rcChannels->chan9;
crsfChannelData[5] = rcChannels->chan5; crsfChannelData[10] = rcChannels->chan10;
crsfChannelData[6] = rcChannels->chan6; crsfChannelData[11] = rcChannels->chan11;
crsfChannelData[7] = rcChannels->chan7; crsfChannelData[12] = rcChannels->chan12;
crsfChannelData[8] = rcChannels->chan8; crsfChannelData[13] = rcChannels->chan13;
crsfChannelData[9] = rcChannels->chan9; crsfChannelData[14] = rcChannels->chan14;
crsfChannelData[10] = rcChannels->chan10; crsfChannelData[15] = rcChannels->chan15;
crsfChannelData[11] = rcChannels->chan11; return RX_FRAME_COMPLETE;
crsfChannelData[12] = rcChannels->chan12;
crsfChannelData[13] = rcChannels->chan13;
crsfChannelData[14] = rcChannels->chan14;
crsfChannelData[15] = rcChannels->chan15;
return RX_FRAME_COMPLETE;
}
} }
return RX_FRAME_PENDING; return RX_FRAME_PENDING;
} }

6
src/test/unit/rx_crsf_unittest.cc
View File

@ -50,6 +50,7 @@ extern "C" {
extern bool crsfFrameDone; extern bool crsfFrameDone;
extern crsfFrame_t crsfFrame; extern crsfFrame_t crsfFrame;
extern crsfFrame_t crsfChannelDataFrame;
extern uint32_t crsfChannelData[CRSF_MAX_CHANNEL]; extern uint32_t crsfChannelData[CRSF_MAX_CHANNEL];
uint32_t dummyTimeUs; uint32_t dummyTimeUs;
@ -178,6 +179,7 @@ TEST(CrossFireTest, TestCrsfFrameStatusUnpacking)
const uint8_t crc = crsfFrameCRC(); const uint8_t crc = crsfFrameCRC();
crsfFrame.frame.payload[CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE] = crc; crsfFrame.frame.payload[CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE] = crc;
memcpy(&crsfChannelDataFrame, &crsfFrame, sizeof(crsfFrame));
const uint8_t status = crsfFrameStatus(); const uint8_t status = crsfFrameStatus();
EXPECT_EQ(RX_FRAME_COMPLETE, status); EXPECT_EQ(RX_FRAME_COMPLETE, status);
EXPECT_EQ(false, crsfFrameDone); EXPECT_EQ(false, crsfFrameDone);
@ -222,6 +224,7 @@ TEST(CrossFireTest, TestCapturedData)
const crsfRcChannelsFrame_t *framePtr = (const crsfRcChannelsFrame_t*)capturedData; const crsfRcChannelsFrame_t *framePtr = (const crsfRcChannelsFrame_t*)capturedData;
crsfFrame = *(const crsfFrame_t*)framePtr; crsfFrame = *(const crsfFrame_t*)framePtr;
crsfFrameDone = true; crsfFrameDone = true;
memcpy(&crsfChannelDataFrame, &crsfFrame, sizeof(crsfFrame));
uint8_t status = crsfFrameStatus(); uint8_t status = crsfFrameStatus();
EXPECT_EQ(RX_FRAME_COMPLETE, status); EXPECT_EQ(RX_FRAME_COMPLETE, status);
EXPECT_EQ(false, crsfFrameDone); EXPECT_EQ(false, crsfFrameDone);
@ -244,6 +247,7 @@ TEST(CrossFireTest, TestCapturedData)
++framePtr; ++framePtr;
crsfFrame = *(const crsfFrame_t*)framePtr; crsfFrame = *(const crsfFrame_t*)framePtr;
crsfFrameDone = true; crsfFrameDone = true;
memcpy(&crsfChannelDataFrame, &crsfFrame, sizeof(crsfFrame));
status = crsfFrameStatus(); status = crsfFrameStatus();
EXPECT_EQ(RX_FRAME_COMPLETE, status); EXPECT_EQ(RX_FRAME_COMPLETE, status);
EXPECT_EQ(false, crsfFrameDone); EXPECT_EQ(false, crsfFrameDone);
@ -268,7 +272,7 @@ TEST(CrossFireTest, TestCrsfDataReceive)
for (unsigned int ii = 0; ii < sizeof(crsfRcChannelsFrame_t); ++ii) { for (unsigned int ii = 0; ii < sizeof(crsfRcChannelsFrame_t); ++ii) {
crsfDataReceive(*pData++); 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_ADDRESS_BROADCAST, crsfFrame.frame.deviceAddress);
EXPECT_EQ(CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC, crsfFrame.frame.frameLength); 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); EXPECT_EQ(CRSF_FRAMETYPE_RC_CHANNELS_PACKED, crsfFrame.frame.type);