1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-23 00:05:33 +03:00

Added checksum checking for MSP/SPORT.

This commit is contained in:
Raphael Coeffic 2016-10-27 12:43:38 +02:00
parent 1fc6bca61f
commit 083222dcff

View file

@ -166,12 +166,13 @@ typedef struct smartPortFrame_s {
uint8_t frameId;
uint16_t valueId;
uint32_t data;
} smartPortFrame_t;
uint8_t crc;
} __attribute__((packed)) smartPortFrame_t;
#define SMARTPORT_FRAME_SIZE sizeof(smartPortFrame_t)
#define SMARTPORT_TX_BUF_SIZE 256
#define SMARTPORT_PAYLOAD_SIZE (SMARTPORT_FRAME_SIZE - 2*sizeof(uint8_t))
#define SMARTPORT_PAYLOAD_SIZE (SMARTPORT_FRAME_SIZE - 3*sizeof(uint8_t))
#define SMARTPORT_PAYLOAD_OFFSET 2
static uint8_t smartPortRxBuffer[SMARTPORT_FRAME_SIZE];
@ -196,6 +197,7 @@ static void smartPortDataReceive(uint16_t c)
{
static bool skipUntilStart = true;
static bool byteStuffing = false;
static uint16_t checksum = 0;
uint32_t now = millis();
@ -218,6 +220,7 @@ static void smartPortDataReceive(uint16_t c)
}
else if (c == FSSP_SENSOR_ID2) {
smartPortRxBuffer[smartPortRxBytes++] = c;
checksum = 0;
}
else {
skipUntilStart = true;
@ -225,8 +228,6 @@ static void smartPortDataReceive(uint16_t c)
}
else {
//TODO: add CRC checking
if (c == FSSP_DLE) {
byteStuffing = true;
return;
@ -240,9 +241,16 @@ static void smartPortDataReceive(uint16_t c)
smartPortRxBuffer[smartPortRxBytes++] = c;
if(smartPortRxBytes == SMARTPORT_FRAME_SIZE) {
smartPortFrameReceived = true;
if (c == (0xFF - checksum)) {
smartPortFrameReceived = true;
}
skipUntilStart = true;
}
else if (smartPortRxBytes < SMARTPORT_FRAME_SIZE) {
checksum += c;
checksum += checksum >> 8;
checksum &= 0x00FF;
}
}
}