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

Refactored the frame unpack to be used by both XBUS protocols.

This commit is contained in:
Stefan Grufman 2015-01-25 13:47:45 +01:00
parent 6376f9a8a8
commit 5c96d36442
1 changed files with 10 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
src/main/rx/xbus.c
View File

@ -42,6 +42,7 @@
#define XBUS_RJ01_FRAME_SIZE 33 #define XBUS_RJ01_FRAME_SIZE 33
#define XBUS_RJ01_MESSAGE_LENGTH 30 #define XBUS_RJ01_MESSAGE_LENGTH 30
#define XBUS_RJ01_OFFSET_BYTES 3
#define XBUS_CRC_AND_VALUE 0x8000 #define XBUS_CRC_AND_VALUE 0x8000
#define XBUS_CRC_POLY 0x1021 #define XBUS_CRC_POLY 0x1021
@ -171,7 +172,7 @@ uint8_t xBusRj01CRC8(uint8_t inData, uint8_t seed)
} }
static void xBusUnpackModeBFrame(void) static void xBusUnpackModeBFrame(uint8_t offsetBytes)
{ {
// Calculate the CRC of the incoming frame // Calculate the CRC of the incoming frame
uint16_t crc = 0; uint16_t crc = 0;
@ -181,19 +182,19 @@ static void xBusUnpackModeBFrame(void)
uint8_t frameAddr; uint8_t frameAddr;
// Calculate on all bytes except the final two CRC bytes // Calculate on all bytes except the final two CRC bytes
for (i = 0; i < xBusFrameLength - 2; i++) { for (i = 0; i < XBUS_FRAME_SIZE - 2; i++) {
inCrc = xBusCRC16(inCrc, xBusFrame[i]); inCrc = xBusCRC16(inCrc, xBusFrame[i+offsetBytes]);
} }
// Get the received CRC // Get the received CRC
crc = ((uint16_t)xBusFrame[xBusFrameLength-2]) << 8; crc = ((uint16_t)xBusFrame[offsetBytes + XBUS_FRAME_SIZE - 2]) << 8;
crc = crc + ((uint16_t)xBusFrame[xBusFrameLength-1]); crc = crc + ((uint16_t)xBusFrame[offsetBytes + XBUS_FRAME_SIZE - 1]);
if (crc == inCrc) { if (crc == inCrc) {
// Unpack the data, we have a valid frame // Unpack the data, we have a valid frame
for (i = 0; i < xBusChannelCount; i++) { for (i = 0; i < xBusChannelCount; i++) {
frameAddr = 1 + i * 2; frameAddr = offsetBytes + 1 + i * 2;
value = ((uint16_t)xBusFrame[frameAddr]) << 8; value = ((uint16_t)xBusFrame[frameAddr]) << 8;
value = value + ((uint16_t)xBusFrame[frameAddr + 1]); value = value + ((uint16_t)xBusFrame[frameAddr + 1]);
@ -210,11 +211,7 @@ static void xBusUnpackRJ01Frame(void)
{ {
// Calculate the CRC of the incoming frame // Calculate the CRC of the incoming frame
uint8_t outerCrc = 0; uint8_t outerCrc = 0;
uint16_t crc = 0;
uint16_t inCrc = 0;
uint8_t i = 0; uint8_t i = 0;
uint16_t value;
uint8_t frameAddr;
// When using the Align RJ01 receiver with // When using the Align RJ01 receiver with
// a MODE B setting in the radio (XG14 tested) // a MODE B setting in the radio (XG14 tested)
@ -253,32 +250,8 @@ static void xBusUnpackRJ01Frame(void)
return; return;
} }
// // Now unpack the "embedded MODE B frame"
// Calculate CRC bytes of the "embedded MODE B frame" xBusUnpackModeBFrame(XBUS_RJ01_OFFSET_BYTES);
//
for (i = 3; i < xBusFrameLength - 5; i++) {
inCrc = xBusCRC16(inCrc, xBusFrame[i]);
}
// Get the received CRC (of the "embedded MODE B frame")
crc = ((uint16_t)xBusFrame[xBusFrameLength-5]) << 8;
crc = crc + ((uint16_t)xBusFrame[xBusFrameLength-4]);
if (crc == inCrc) {
// Unpack the data, we have a valid frame
for (i = 0; i < xBusChannelCount; i++) {
frameAddr = 4 + i * 2;
value = ((uint16_t)xBusFrame[frameAddr]) << 8;
value = value + ((uint16_t)xBusFrame[frameAddr + 1]);
// Convert to internal format
xBusChannelData[i] = XBUS_CONVERT_TO_USEC(value);
}
xBusFrameReceived = true;
}
} }
// Receive ISR callback // Receive ISR callback
@ -312,7 +285,7 @@ static void xBusDataReceive(uint16_t c)
if (xBusFramePosition == xBusFrameLength) { if (xBusFramePosition == xBusFrameLength) {
switch (xBusProvider) { switch (xBusProvider) {
case SERIALRX_XBUS_MODE_B: case SERIALRX_XBUS_MODE_B:
xBusUnpackModeBFrame(); xBusUnpackModeBFrame(0);
case SERIALRX_XBUS_MODE_B_RJ01: case SERIALRX_XBUS_MODE_B_RJ01:
xBusUnpackRJ01Frame(); xBusUnpackRJ01Frame();
} }