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

23
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,14 +252,19 @@ 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 (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) #if defined(USE_TELEMETRY_CRSF) && defined(USE_MSP_OVER_TELEMETRY)
case CRSF_FRAMETYPE_MSP_REQ: case CRSF_FRAMETYPE_MSP_REQ:
case CRSF_FRAMETYPE_MSP_WRITE: { case CRSF_FRAMETYPE_MSP_WRITE: {
@ -299,7 +305,6 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *data)
} }
} }
} }
}
STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeState_t *rxRuntimeState) STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeState_t *rxRuntimeState)
{ {
@ -310,14 +315,9 @@ 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
const uint8_t crc = crsfFrameCRC();
if (crc != crsfFrame.frame.payload[CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE]) {
return RX_FRAME_PENDING;
}
// unpack the RC channels // unpack the RC channels
const crsfPayloadRcChannelsPacked_t* const rcChannels = (crsfPayloadRcChannelsPacked_t*)&crsfFrame.frame.payload; const crsfPayloadRcChannelsPacked_t* const rcChannels = (crsfPayloadRcChannelsPacked_t*)&crsfChannelDataFrame.frame.payload;
crsfChannelData[0] = rcChannels->chan0; crsfChannelData[0] = rcChannels->chan0;
crsfChannelData[1] = rcChannels->chan1; crsfChannelData[1] = rcChannels->chan1;
crsfChannelData[2] = rcChannels->chan2; crsfChannelData[2] = rcChannels->chan2;
@ -336,7 +336,6 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeState_t *rxRuntimeState)
crsfChannelData[15] = rcChannels->chan15; crsfChannelData[15] = rcChannels->chan15;
return RX_FRAME_COMPLETE; 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);