mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-23 08:15:30 +03:00
Added checksum checking for MSP/SPORT.
This commit is contained in:
parent
1fc6bca61f
commit
083222dcff
1 changed files with 13 additions and 5 deletions
|
@ -166,12 +166,13 @@ typedef struct smartPortFrame_s {
|
||||||
uint8_t frameId;
|
uint8_t frameId;
|
||||||
uint16_t valueId;
|
uint16_t valueId;
|
||||||
uint32_t data;
|
uint32_t data;
|
||||||
} smartPortFrame_t;
|
uint8_t crc;
|
||||||
|
} __attribute__((packed)) smartPortFrame_t;
|
||||||
|
|
||||||
#define SMARTPORT_FRAME_SIZE sizeof(smartPortFrame_t)
|
#define SMARTPORT_FRAME_SIZE sizeof(smartPortFrame_t)
|
||||||
#define SMARTPORT_TX_BUF_SIZE 256
|
#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
|
#define SMARTPORT_PAYLOAD_OFFSET 2
|
||||||
|
|
||||||
static uint8_t smartPortRxBuffer[SMARTPORT_FRAME_SIZE];
|
static uint8_t smartPortRxBuffer[SMARTPORT_FRAME_SIZE];
|
||||||
|
@ -196,6 +197,7 @@ static void smartPortDataReceive(uint16_t c)
|
||||||
{
|
{
|
||||||
static bool skipUntilStart = true;
|
static bool skipUntilStart = true;
|
||||||
static bool byteStuffing = false;
|
static bool byteStuffing = false;
|
||||||
|
static uint16_t checksum = 0;
|
||||||
|
|
||||||
uint32_t now = millis();
|
uint32_t now = millis();
|
||||||
|
|
||||||
|
@ -218,6 +220,7 @@ static void smartPortDataReceive(uint16_t c)
|
||||||
}
|
}
|
||||||
else if (c == FSSP_SENSOR_ID2) {
|
else if (c == FSSP_SENSOR_ID2) {
|
||||||
smartPortRxBuffer[smartPortRxBytes++] = c;
|
smartPortRxBuffer[smartPortRxBytes++] = c;
|
||||||
|
checksum = 0;
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
skipUntilStart = true;
|
skipUntilStart = true;
|
||||||
|
@ -225,8 +228,6 @@ static void smartPortDataReceive(uint16_t c)
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
|
|
||||||
//TODO: add CRC checking
|
|
||||||
|
|
||||||
if (c == FSSP_DLE) {
|
if (c == FSSP_DLE) {
|
||||||
byteStuffing = true;
|
byteStuffing = true;
|
||||||
return;
|
return;
|
||||||
|
@ -240,9 +241,16 @@ static void smartPortDataReceive(uint16_t c)
|
||||||
smartPortRxBuffer[smartPortRxBytes++] = c;
|
smartPortRxBuffer[smartPortRxBytes++] = c;
|
||||||
|
|
||||||
if(smartPortRxBytes == SMARTPORT_FRAME_SIZE) {
|
if(smartPortRxBytes == SMARTPORT_FRAME_SIZE) {
|
||||||
smartPortFrameReceived = true;
|
if (c == (0xFF - checksum)) {
|
||||||
|
smartPortFrameReceived = true;
|
||||||
|
}
|
||||||
skipUntilStart = true;
|
skipUntilStart = true;
|
||||||
}
|
}
|
||||||
|
else if (smartPortRxBytes < SMARTPORT_FRAME_SIZE) {
|
||||||
|
checksum += c;
|
||||||
|
checksum += checksum >> 8;
|
||||||
|
checksum &= 0x00FF;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue