Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 14:25:20 +03:00
Code Issues Wiki Activity

Restored SmartPort MSP frame handling to how it was before the addition of MSP over CRSF.

Browse source
This commit is contained in:
mikeller 2017-09-11 03:01:51 +12:00
parent 6bf755d0bd
commit aa1edbd978
7 changed files with 23 additions and 20 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/rx/crsf.c
View file

@ -194,7 +194,7 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(void)
// TODO: CRC CHECK // TODO: CRC CHECK
uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2; uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2;
uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + 2 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE; uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + 2 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE;
if(handleMspFrame(frameStart, frameEnd)) { if(handleMspFrame(frameStart, frameEnd, MSP_FRAME_HANDLING_NORMAL)) {
scheduleMspResponse(); scheduleMspResponse();
} }
return RX_FRAME_COMPLETE; return RX_FRAME_COMPLETE;

4
src/main/telemetry/msp_shared.c
View file

@ -86,12 +86,12 @@ void sendMspErrorResponse(uint8_t error, int16_t cmd)
sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer); sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer);
} }
bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd) bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd, mspFrameHandling_t handling)
{ {
static uint8_t mspStarted = 0; static uint8_t mspStarted = 0;
static uint8_t lastSeq = 0; static uint8_t lastSeq = 0;
if (sbufBytesRemaining(&mspPackage.responsePacket->buf) > 0) { if (handling != MSP_FRAME_HANDLING_FORCED && sbufBytesRemaining(&mspPackage.responsePacket->buf) > 0) {
return false; return false;
} }

7
src/main/telemetry/msp_shared.h
View file

@ -24,6 +24,11 @@ typedef union mspTxBuffer_u {
uint8_t crsfMspTxBuffer[CRSF_MSP_TX_BUF_SIZE]; uint8_t crsfMspTxBuffer[CRSF_MSP_TX_BUF_SIZE];
} mspTxBuffer_t; } mspTxBuffer_t;
typedef enum mspFrameHandling_e {
MSP_FRAME_HANDLING_NORMAL,
MSP_FRAME_HANDLING_FORCED
} mspFrameHandling_t;
void initSharedMsp(); void initSharedMsp();
bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd); bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd, mspFrameHandling_t handling);
bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn); bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn);

10
src/main/telemetry/smartport.c
View file

@ -353,13 +353,11 @@ void handleSmartPortTelemetry(void)
// do not check the physical ID here again // do not check the physical ID here again
// unless we start receiving other sensors' packets // unless we start receiving other sensors' packets
if (smartPortRxBuffer.frameId == FSSP_MSPC_FRAME) { if (smartPortRxBuffer.frameId == FSSP_MSPC_FRAME) {
// Pass only the payload: skip sensorId & frameId // Pass only the payload: skip sensorId & frameId
if (!smartPortMspReplyPending) { uint8_t *frameStart = (uint8_t *)&smartPortRxBuffer + SMARTPORT_PAYLOAD_OFFSET;
uint8_t *frameStart = (uint8_t *)&smartPortRxBuffer + SMARTPORT_PAYLOAD_OFFSET; uint8_t *frameEnd = (uint8_t *)&smartPortRxBuffer + SMARTPORT_PAYLOAD_OFFSET + SMARTPORT_PAYLOAD_SIZE;
uint8_t *frameEnd = (uint8_t *)&smartPortRxBuffer + SMARTPORT_PAYLOAD_OFFSET + SMARTPORT_PAYLOAD_SIZE;
smartPortMspReplyPending = handleMspFrame(frameStart, frameEnd); smartPortMspReplyPending = handleMspFrame(frameStart, frameEnd, MSP_FRAME_HANDLING_FORCED);
}
} }
} }

2
src/test/unit/rx_crsf_unittest.cc
View file

@ -284,5 +284,5 @@ bool telemetryCheckRxPortShared(const serialPortConfig_t *) {return false;}
serialPort_t *telemetrySharedPort = NULL; serialPort_t *telemetrySharedPort = NULL;
void scheduleDeviceInfoResponse(void) {}; void scheduleDeviceInfoResponse(void) {};
void scheduleMspResponse(mspPackage_t *package) { UNUSED(package); }; void scheduleMspResponse(mspPackage_t *package) { UNUSED(package); };
bool handleMspFrame(uint8_t *, uint8_t *) { return false; } bool handleMspFrame(uint8_t *, uint8_t *, mspFrameHandling_t) { return false; }
} }

16
src/test/unit/telemetry_crsf_msp_unittest.cc
View file

@ -60,7 +60,7 @@ extern "C" {
#include "telemetry/msp_shared.h" #include "telemetry/msp_shared.h"
#include "telemetry/smartport.h" #include "telemetry/smartport.h"
bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd); bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd, mspFrameHandling_t handling);
bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn); bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn);
uint8_t sbufReadU8(sbuf_t *src); uint8_t sbufReadU8(sbuf_t *src);
int sbufBytesRemaining(sbuf_t *buf); int sbufBytesRemaining(sbuf_t *buf);
@ -130,7 +130,7 @@ TEST(CrossFireMSPTest, ResponsePacketTest)
crsfFrameDone = true; crsfFrameDone = true;
uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2; uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2;
uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2; uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2;
handleMspFrame(frameStart, frameEnd); handleMspFrame(frameStart, frameEnd, MSP_FRAME_HANDLING_NORMAL);
for (unsigned int ii=1; ii<30; ii++) { for (unsigned int ii=1; ii<30; ii++) {
EXPECT_EQ(ii, sbufReadU8(&mspPackage.responsePacket->buf)); EXPECT_EQ(ii, sbufReadU8(&mspPackage.responsePacket->buf));
} }
@ -151,7 +151,7 @@ TEST(CrossFireMSPTest, WriteResponseTest)
crsfFrameDone = true; crsfFrameDone = true;
uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2; uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2;
uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2; uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2;
bool pending1 = handleMspFrame(frameStart, frameEnd); bool pending1 = handleMspFrame(frameStart, frameEnd, MSP_FRAME_HANDLING_NORMAL);
EXPECT_FALSE(pending1); // not done yet*/ EXPECT_FALSE(pending1); // not done yet*/
EXPECT_EQ(0x29, mspPackage.requestBuffer[0]); EXPECT_EQ(0x29, mspPackage.requestBuffer[0]);
EXPECT_EQ(0x28, mspPackage.requestBuffer[1]); EXPECT_EQ(0x28, mspPackage.requestBuffer[1]);
@ -164,7 +164,7 @@ TEST(CrossFireMSPTest, WriteResponseTest)
crsfFrameDone = true; crsfFrameDone = true;
uint8_t *frameStart2 = (uint8_t *)&crsfFrame.frame.payload + 2; uint8_t *frameStart2 = (uint8_t *)&crsfFrame.frame.payload + 2;
uint8_t *frameEnd2 = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2; uint8_t *frameEnd2 = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2;
bool pending2 = handleMspFrame(frameStart2, frameEnd2); bool pending2 = handleMspFrame(frameStart2, frameEnd2, MSP_FRAME_HANDLING_NORMAL);
EXPECT_FALSE(pending2); // not done yet EXPECT_FALSE(pending2); // not done yet
EXPECT_EQ(0x23, mspPackage.requestBuffer[5]); EXPECT_EQ(0x23, mspPackage.requestBuffer[5]);
EXPECT_EQ(0x46, mspPackage.requestBuffer[6]); EXPECT_EQ(0x46, mspPackage.requestBuffer[6]);
@ -179,7 +179,7 @@ TEST(CrossFireMSPTest, WriteResponseTest)
crsfFrameDone = true; crsfFrameDone = true;
uint8_t *frameStart3 = (uint8_t *)&crsfFrame.frame.payload + 2; uint8_t *frameStart3 = (uint8_t *)&crsfFrame.frame.payload + 2;
uint8_t *frameEnd3 = frameStart3 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE; uint8_t *frameEnd3 = frameStart3 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE;
bool pending3 = handleMspFrame(frameStart3, frameEnd3); bool pending3 = handleMspFrame(frameStart3, frameEnd3, MSP_FRAME_HANDLING_NORMAL);
EXPECT_FALSE(pending3); // not done yet EXPECT_FALSE(pending3); // not done yet
EXPECT_EQ(0x0F, mspPackage.requestBuffer[12]); EXPECT_EQ(0x0F, mspPackage.requestBuffer[12]);
EXPECT_EQ(0x00, mspPackage.requestBuffer[13]); EXPECT_EQ(0x00, mspPackage.requestBuffer[13]);
@ -194,7 +194,7 @@ TEST(CrossFireMSPTest, WriteResponseTest)
crsfFrameDone = true; crsfFrameDone = true;
uint8_t *frameStart4 = (uint8_t *)&crsfFrame.frame.payload + 2; uint8_t *frameStart4 = (uint8_t *)&crsfFrame.frame.payload + 2;
uint8_t *frameEnd4 = frameStart4 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE; uint8_t *frameEnd4 = frameStart4 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE;
bool pending4 = handleMspFrame(frameStart4, frameEnd4); bool pending4 = handleMspFrame(frameStart4, frameEnd4, MSP_FRAME_HANDLING_NORMAL);
EXPECT_FALSE(pending4); // not done yet EXPECT_FALSE(pending4); // not done yet
EXPECT_EQ(0x21, mspPackage.requestBuffer[19]); EXPECT_EQ(0x21, mspPackage.requestBuffer[19]);
EXPECT_EQ(0x53, mspPackage.requestBuffer[20]); EXPECT_EQ(0x53, mspPackage.requestBuffer[20]);
@ -210,7 +210,7 @@ TEST(CrossFireMSPTest, WriteResponseTest)
crsfFrameDone = true; crsfFrameDone = true;
uint8_t *frameStart5 = (uint8_t *)&crsfFrame.frame.payload + 2; uint8_t *frameStart5 = (uint8_t *)&crsfFrame.frame.payload + 2;
uint8_t *frameEnd5 = frameStart2 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE; uint8_t *frameEnd5 = frameStart2 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE;
bool pending5 = handleMspFrame(frameStart5, frameEnd5); bool pending5 = handleMspFrame(frameStart5, frameEnd5, MSP_FRAME_HANDLING_NORMAL);
EXPECT_TRUE(pending5); // not done yet EXPECT_TRUE(pending5); // not done yet
EXPECT_EQ(0x00, mspPackage.requestBuffer[26]); EXPECT_EQ(0x00, mspPackage.requestBuffer[26]);
EXPECT_EQ(0x37, mspPackage.requestBuffer[27]); EXPECT_EQ(0x37, mspPackage.requestBuffer[27]);
@ -233,7 +233,7 @@ TEST(CrossFireMSPTest, SendMspReply) {
crsfFrameDone = true; crsfFrameDone = true;
uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2; uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2;
uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2; uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2;
bool handled = handleMspFrame(frameStart, frameEnd); bool handled = handleMspFrame(frameStart, frameEnd, MSP_FRAME_HANDLING_NORMAL);
EXPECT_TRUE(handled); EXPECT_TRUE(handled);
bool replyPending = sendMspReply(64, &testSendMspResponse); bool replyPending = sendMspReply(64, &testSendMspResponse);
EXPECT_FALSE(replyPending); EXPECT_FALSE(replyPending);

2
src/test/unit/telemetry_crsf_unittest.cc
View file

@ -330,6 +330,6 @@ int32_t getMAhDrawn(void){
} }
bool sendMspReply(uint8_t, mspResponseFnPtr) { return false; } bool sendMspReply(uint8_t, mspResponseFnPtr) { return false; }
bool handleMspFrame(uint8_t *, uint8_t *) { return false; } bool handleMspFrame(uint8_t *, uint8_t *, mspFrameHandling_t) { return false; }
} }