Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-24 00:35:39 +03:00
Code Issues Wiki Activity

Added checksum checking for MSP/SPORT.

Browse source
This commit is contained in:
Raphael Coeffic 2016-10-27 12:43:38 +02:00
parent 1fc6bca61f
commit 083222dcff
1 changed files with 13 additions and 5 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -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) {
if (c == (0xFF - checksum)) {
smartPortFrameReceived = true; smartPortFrameReceived = true;
}
skipUntilStart = true; skipUntilStart = true;
} }
else if (smartPortRxBytes < SMARTPORT_FRAME_SIZE) {
checksum += c;
checksum += checksum >> 8;
checksum &= 0x00FF;
}
} }
} }