rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'rx_sumd_added_crc_check' of https://github.com/ProDrone/cleanflight into ProDrone-rx_sumd_added_crc_check 

Conflicts:
	src/main/rx/sumd.c
This commit is contained in:
Dominic Clifton 2015-08-12 01:10:31 +01:00
parent f510fe88b7 f26af1d844
commit 94c243c279
1 changed files with 31 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
src/main/rx/sumd.c
View File

@ -44,6 +44,7 @@
static bool sumdFrameDone = false;
static uint16_t sumdChannels[SUMD_MAX_CHANNEL];
static uint16_t crc;
static void sumdDataReceive(uint16_t c);
static uint16_t sumdReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan);
@ -67,6 +68,22 @@ bool sumdInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRa
return sumdPort != NULL;
}
#define CRC_POLYNOME 0x1021
// CRC calculation, adds a 8 bit unsigned to 16 bit crc
static void CRC16(uint8_t value)
{
uint8_t i;
crc = crc ^ (int16_t)value << 8;
for (i = 0; i < 8; i++) {
if (crc & 0x8000)
crc = (crc << 1) ^ CRC_POLYNOME;
else
crc = (crc << 1);
}
}
static uint8_t sumd[SUMD_BUFFSIZE] = { 0, };
static uint8_t sumdChannelCount;
@ -86,17 +103,23 @@ static void sumdDataReceive(uint16_t c)
if (c != SUMD_SYNCBYTE)
return;
else
{
sumdFrameDone = false; // lazy main loop didnt fetch the stuff
crc = 0;
}
}
if (sumdIndex == 2)
sumdChannelCount = (uint8_t)c;
if (sumdIndex < SUMD_BUFFSIZE)
sumd[sumdIndex] = (uint8_t)c;
sumdIndex++;
if (sumdIndex == sumdChannelCount * 2 + 5) {
sumdIndex = 0;
sumdFrameDone = true;
}
if (sumdIndex < sumdChannelCount * 2 + 4)
CRC16((uint8_t)c);
else
if (sumdIndex == sumdChannelCount * 2 + 5) {
sumdIndex = 0;
sumdFrameDone = true;
}
}
#define SUMD_OFFSET_CHANNEL_1_HIGH 3
@ -119,6 +142,10 @@ uint8_t sumdFrameStatus(void)
sumdFrameDone = false;
// verify CRC
if (crc != ((sumd[SUMD_BYTES_PER_CHANNEL * sumdChannelCount + SUMD_OFFSET_CHANNEL_1_HIGH] << 8) |
(sumd[SUMD_BYTES_PER_CHANNEL * sumdChannelCount + SUMD_OFFSET_CHANNEL_1_LOW])))
return frameStatus;
switch (sumd[1]) {
case SUMD_FRAME_STATE_FAILSAFE: