From e8e53ab7f19bc21d6d1b7deffcfa05f401ef629a Mon Sep 17 00:00:00 2001 From: Stefan Grufman Date: Sat, 27 Dec 2014 14:01:40 +0100 Subject: [PATCH 1/5] Added initial suport for Align JR01 DMSS (i.e. satellite receiver for JR systems) support. --- src/main/io/serial.c | 6 +-- src/main/rx/rx.c | 3 ++ src/main/rx/rx.h | 3 +- src/main/rx/xbus.c | 115 +++++++++++++++++++++++++++++++++++++------ 4 files changed, 109 insertions(+), 18 deletions(-) diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 4f569b8aa..2799cde91 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -119,8 +119,8 @@ static serialPortFunction_t serialPortFunctions[SERIAL_PORT_COUNT] = { }; static const serialPortConstraint_t serialPortConstraints[SERIAL_PORT_COUNT] = { - {SERIAL_PORT_USART1, 9600, 115200, SPF_NONE | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, - {SERIAL_PORT_USART2, 9600, 115200, SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, + {SERIAL_PORT_USART1, 9600, 250000, SPF_NONE | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, + {SERIAL_PORT_USART2, 9600, 250000, SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE}, #if (SERIAL_PORT_COUNT > 2) {SERIAL_PORT_SOFTSERIAL1, 9600, 19200, SPF_SUPPORTS_CALLBACK | SPF_IS_SOFTWARE_INVERTABLE}, {SERIAL_PORT_SOFTSERIAL2, 9600, 19200, SPF_SUPPORTS_CALLBACK | SPF_IS_SOFTWARE_INVERTABLE} @@ -134,7 +134,7 @@ const functionConstraint_t functionConstraints[] = { { FUNCTION_GPS, 9600, 115200, AUTOBAUD, SPF_NONE }, { FUNCTION_GPS_PASSTHROUGH, 9600, 115200, NO_AUTOBAUD, SPF_NONE }, { FUNCTION_MSP, 9600, 115200, NO_AUTOBAUD, SPF_NONE }, - { FUNCTION_SERIAL_RX, 9600, 115200, NO_AUTOBAUD, SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_CALLBACK }, + { FUNCTION_SERIAL_RX, 9600, 250000, NO_AUTOBAUD, SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_CALLBACK }, { FUNCTION_TELEMETRY, 9600, 19200, NO_AUTOBAUD, SPF_NONE }, { FUNCTION_SMARTPORT_TELEMETRY, 57600, 57600, NO_AUTOBAUD, SPF_SUPPORTS_BIDIR_MODE } }; diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 60ebc9726..4c147aa68 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -102,6 +102,7 @@ void updateSerialRxFunctionConstraint(functionConstraint_t *functionConstraintTo sumhUpdateSerialRxFunctionConstraint(functionConstraintToUpdate); break; case SERIALRX_XBUS_MODE_B: + case SERIALRX_XBUS_MODE_B_JR01: xBusUpdateSerialRxFunctionConstraint(functionConstraintToUpdate); break; } @@ -158,6 +159,7 @@ void serialRxInit(rxConfig_t *rxConfig) enabled = sumhInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); break; case SERIALRX_XBUS_MODE_B: + case SERIALRX_XBUS_MODE_B_JR01: enabled = xBusInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); break; } @@ -182,6 +184,7 @@ bool isSerialRxFrameComplete(rxConfig_t *rxConfig) case SERIALRX_SUMH: return sumhFrameComplete(); case SERIALRX_XBUS_MODE_B: + case SERIALRX_XBUS_MODE_B_JR01: return xBusFrameComplete(); } return false; diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 92ae2eb0f..5de472a52 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -33,7 +33,8 @@ typedef enum { SERIALRX_SUMD = 3, SERIALRX_SUMH = 4, SERIALRX_XBUS_MODE_B = 5, - SERIALRX_PROVIDER_MAX = SERIALRX_XBUS_MODE_B + SERIALRX_XBUS_MODE_B_JR01 = 6, + SERIALRX_PROVIDER_MAX = SERIALRX_XBUS_MODE_B_JR01 } SerialRXType; #define SERIALRX_PROVIDER_COUNT (SERIALRX_PROVIDER_MAX + 1) diff --git a/src/main/rx/xbus.c b/src/main/rx/xbus.c index 9125151d7..11ec52075 100644 --- a/src/main/rx/xbus.c +++ b/src/main/rx/xbus.c @@ -35,17 +35,19 @@ // #define XBUS_CHANNEL_COUNT 12 +#define XBUS_JR01_CHANNEL_COUNT 12 // Frame is: ID(1 byte) + 12*channel(2 bytes) + CRC(2 bytes) = 27 #define XBUS_FRAME_SIZE 27 -#define XBUS_CRC_BYTE_1 25 -#define XBUS_CRC_BYTE_2 26 + +#define XBUS_JR01_FRAME_SIZE 33 #define XBUS_CRC_AND_VALUE 0x8000 #define XBUS_CRC_POLY 0x1021 #define XBUS_BAUDRATE 115200 -#define XBUS_MAX_FRAME_TIME 5000 +#define XBUS_JR01_BAUDRATE 250000 +#define XBUS_MAX_FRAME_TIME 8000 // NOTE! // This is actually based on ID+LENGTH (nibble each) @@ -68,9 +70,14 @@ static bool xBusFrameReceived = false; static bool xBusDataIncoming = false; static uint8_t xBusFramePosition; +static uint8_t xBusFrameLength; +static uint8_t xBusChannelCount; +static uint8_t xBusProvider; -static volatile uint8_t xBusFrame[XBUS_FRAME_SIZE]; -static uint16_t xBusChannelData[XBUS_CHANNEL_COUNT]; + +// Use max values for ram areas +static volatile uint8_t xBusFrame[XBUS_JR01_FRAME_SIZE]; +static uint16_t xBusChannelData[XBUS_JR01_CHANNEL_COUNT]; static void xBusDataReceive(uint16_t c); static uint16_t xBusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); @@ -80,25 +87,41 @@ static serialPort_t *xBusPort; void xBusUpdateSerialRxFunctionConstraint(functionConstraint_t *functionConstraint) { functionConstraint->minBaudRate = XBUS_BAUDRATE; - functionConstraint->maxBaudRate = XBUS_BAUDRATE; + functionConstraint->maxBaudRate = XBUS_JR01_BAUDRATE; functionConstraint->requiredSerialPortFeatures = SPF_SUPPORTS_CALLBACK; } bool xBusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback) { + uint32_t baudRate; + switch (rxConfig->serialrx_provider) { case SERIALRX_XBUS_MODE_B: rxRuntimeConfig->channelCount = XBUS_CHANNEL_COUNT; xBusFrameReceived = false; xBusDataIncoming = false; xBusFramePosition = 0; + baudRate = XBUS_BAUDRATE; + xBusFrameLength = XBUS_FRAME_SIZE; + xBusChannelCount = XBUS_CHANNEL_COUNT; + xBusProvider = SERIALRX_XBUS_MODE_B; + break; + case SERIALRX_XBUS_MODE_B_JR01: + rxRuntimeConfig->channelCount = XBUS_JR01_CHANNEL_COUNT; + xBusFrameReceived = false; + xBusDataIncoming = false; + xBusFramePosition = 0; + baudRate = XBUS_JR01_BAUDRATE; + xBusFrameLength = XBUS_JR01_FRAME_SIZE; + xBusChannelCount = XBUS_JR01_CHANNEL_COUNT; + xBusProvider = SERIALRX_XBUS_MODE_B_JR01; break; default: return false; break; } - xBusPort = openSerialPort(FUNCTION_SERIAL_RX, xBusDataReceive, XBUS_BAUDRATE, MODE_RX, SERIAL_NOT_INVERTED); + xBusPort = openSerialPort(FUNCTION_SERIAL_RX, xBusDataReceive, baudRate, MODE_RX, SERIAL_NOT_INVERTED); if (callback) { *callback = xBusReadRawRC; } @@ -123,7 +146,7 @@ static uint16_t xBusCRC16(uint16_t crc, uint8_t value) return crc; } -static void xBusUnpackFrame(void) +static void xBusUnpackModeBFrame(void) { // Calculate the CRC of the incoming frame uint16_t crc = 0; @@ -133,17 +156,17 @@ static void xBusUnpackFrame(void) uint8_t frameAddr; // Calculate on all bytes except the final two CRC bytes - for (i = 0; i < XBUS_FRAME_SIZE - 2; i++) { + for (i = 0; i < xBusFrameLength - 2; i++) { inCrc = xBusCRC16(inCrc, xBusFrame[i]); } // Get the received CRC - crc = ((uint16_t)xBusFrame[XBUS_CRC_BYTE_1]) << 8; - crc = crc + ((uint16_t)xBusFrame[XBUS_CRC_BYTE_2]); + crc = ((uint16_t)xBusFrame[xBusFrameLength-2]) << 8; + crc = crc + ((uint16_t)xBusFrame[xBusFrameLength-1]); if (crc == inCrc) { // Unpack the data, we have a valid frame - for (i = 0; i < XBUS_CHANNEL_COUNT; i++) { + for (i = 0; i < xBusChannelCount; i++) { frameAddr = 1 + i * 2; value = ((uint16_t)xBusFrame[frameAddr]) << 8; @@ -158,9 +181,68 @@ static void xBusUnpackFrame(void) } +static void xBusUnpackJr01Frame(void) +{ + // Calculate the CRC of the incoming frame + uint16_t crc = 0; + uint16_t inCrc = 0; + uint8_t i = 0; + uint16_t value; + uint8_t frameAddr; + + // When using the Align JR01 receiver with + // a MODE B setting in the radio (XG14 tested) + // the MODE_B -frame is packed within some + // at the moment unknown bytes before and after: + // 0xA1 __ __ 0xA1 12*(High + Low) CRC1 CRC2 + __ __ __ + // Compared to a standard MODE B frame that only + // contains the "middle" package + // Hence, at the moment, the unknown header and footer + // of the JR01 MODEB packages are discarded. This is + // ok as the CRC-checksum of the embedded package works + // out nicely. + + // Calculate CRC bytes of the "embedded MODE B frame" + 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 static void xBusDataReceive(uint16_t c) { + uint32_t now; + static uint32_t xBusTimeLast, xBusTimeInterval; + + // Check if we shall reset frame position due to time + now = micros(); + xBusTimeInterval = now - xBusTimeLast; + xBusTimeLast = now; + if (xBusTimeInterval > XBUS_MAX_FRAME_TIME) { + xBusFramePosition = 0; + xBusDataIncoming = false; + } + // Check if we shall start a frame? if ((xBusFramePosition == 0) && (c == XBUS_START_OF_FRAME_BYTE)) { xBusDataIncoming = true; @@ -174,8 +256,13 @@ static void xBusDataReceive(uint16_t c) } // Done? - if (xBusFramePosition == XBUS_FRAME_SIZE) { - xBusUnpackFrame(); + if (xBusFramePosition == xBusFrameLength) { + switch (xBusProvider) { + case SERIALRX_XBUS_MODE_B: + xBusUnpackModeBFrame(); + case SERIALRX_XBUS_MODE_B_JR01: + xBusUnpackJr01Frame(); + } xBusDataIncoming = false; xBusFramePosition = 0; } From d3b21f8e36afeaf5974455088f0776eece91f573 Mon Sep 17 00:00:00 2001 From: Stefan Grufman Date: Sat, 27 Dec 2014 14:52:06 +0100 Subject: [PATCH 2/5] Messed up the numbers, now changed to correct RJ01. --- src/main/rx/rx.c | 6 +++--- src/main/rx/rx.h | 4 ++-- src/main/rx/xbus.c | 34 +++++++++++++++++----------------- 3 files changed, 22 insertions(+), 22 deletions(-) diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 4c147aa68..094766ab6 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -102,7 +102,7 @@ void updateSerialRxFunctionConstraint(functionConstraint_t *functionConstraintTo sumhUpdateSerialRxFunctionConstraint(functionConstraintToUpdate); break; case SERIALRX_XBUS_MODE_B: - case SERIALRX_XBUS_MODE_B_JR01: + case SERIALRX_XBUS_MODE_B_RJ01: xBusUpdateSerialRxFunctionConstraint(functionConstraintToUpdate); break; } @@ -159,7 +159,7 @@ void serialRxInit(rxConfig_t *rxConfig) enabled = sumhInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); break; case SERIALRX_XBUS_MODE_B: - case SERIALRX_XBUS_MODE_B_JR01: + case SERIALRX_XBUS_MODE_B_RJ01: enabled = xBusInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); break; } @@ -184,7 +184,7 @@ bool isSerialRxFrameComplete(rxConfig_t *rxConfig) case SERIALRX_SUMH: return sumhFrameComplete(); case SERIALRX_XBUS_MODE_B: - case SERIALRX_XBUS_MODE_B_JR01: + case SERIALRX_XBUS_MODE_B_RJ01: return xBusFrameComplete(); } return false; diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 5de472a52..b88e0c590 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -33,8 +33,8 @@ typedef enum { SERIALRX_SUMD = 3, SERIALRX_SUMH = 4, SERIALRX_XBUS_MODE_B = 5, - SERIALRX_XBUS_MODE_B_JR01 = 6, - SERIALRX_PROVIDER_MAX = SERIALRX_XBUS_MODE_B_JR01 + SERIALRX_XBUS_MODE_B_RJ01 = 6, + SERIALRX_PROVIDER_MAX = SERIALRX_XBUS_MODE_B_RJ01 } SerialRXType; #define SERIALRX_PROVIDER_COUNT (SERIALRX_PROVIDER_MAX + 1) diff --git a/src/main/rx/xbus.c b/src/main/rx/xbus.c index 11ec52075..ed3015f6a 100644 --- a/src/main/rx/xbus.c +++ b/src/main/rx/xbus.c @@ -35,18 +35,18 @@ // #define XBUS_CHANNEL_COUNT 12 -#define XBUS_JR01_CHANNEL_COUNT 12 +#define XBUS_RJ01_CHANNEL_COUNT 12 // Frame is: ID(1 byte) + 12*channel(2 bytes) + CRC(2 bytes) = 27 #define XBUS_FRAME_SIZE 27 -#define XBUS_JR01_FRAME_SIZE 33 +#define XBUS_RJ01_FRAME_SIZE 33 #define XBUS_CRC_AND_VALUE 0x8000 #define XBUS_CRC_POLY 0x1021 #define XBUS_BAUDRATE 115200 -#define XBUS_JR01_BAUDRATE 250000 +#define XBUS_RJ01_BAUDRATE 250000 #define XBUS_MAX_FRAME_TIME 8000 // NOTE! @@ -76,8 +76,8 @@ static uint8_t xBusProvider; // Use max values for ram areas -static volatile uint8_t xBusFrame[XBUS_JR01_FRAME_SIZE]; -static uint16_t xBusChannelData[XBUS_JR01_CHANNEL_COUNT]; +static volatile uint8_t xBusFrame[XBUS_RJ01_FRAME_SIZE]; +static uint16_t xBusChannelData[XBUS_RJ01_CHANNEL_COUNT]; static void xBusDataReceive(uint16_t c); static uint16_t xBusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); @@ -87,7 +87,7 @@ static serialPort_t *xBusPort; void xBusUpdateSerialRxFunctionConstraint(functionConstraint_t *functionConstraint) { functionConstraint->minBaudRate = XBUS_BAUDRATE; - functionConstraint->maxBaudRate = XBUS_JR01_BAUDRATE; + functionConstraint->maxBaudRate = XBUS_RJ01_BAUDRATE; functionConstraint->requiredSerialPortFeatures = SPF_SUPPORTS_CALLBACK; } @@ -106,15 +106,15 @@ bool xBusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRa xBusChannelCount = XBUS_CHANNEL_COUNT; xBusProvider = SERIALRX_XBUS_MODE_B; break; - case SERIALRX_XBUS_MODE_B_JR01: - rxRuntimeConfig->channelCount = XBUS_JR01_CHANNEL_COUNT; + case SERIALRX_XBUS_MODE_B_RJ01: + rxRuntimeConfig->channelCount = XBUS_RJ01_CHANNEL_COUNT; xBusFrameReceived = false; xBusDataIncoming = false; xBusFramePosition = 0; - baudRate = XBUS_JR01_BAUDRATE; - xBusFrameLength = XBUS_JR01_FRAME_SIZE; - xBusChannelCount = XBUS_JR01_CHANNEL_COUNT; - xBusProvider = SERIALRX_XBUS_MODE_B_JR01; + baudRate = XBUS_RJ01_BAUDRATE; + xBusFrameLength = XBUS_RJ01_FRAME_SIZE; + xBusChannelCount = XBUS_RJ01_CHANNEL_COUNT; + xBusProvider = SERIALRX_XBUS_MODE_B_RJ01; break; default: return false; @@ -181,7 +181,7 @@ static void xBusUnpackModeBFrame(void) } -static void xBusUnpackJr01Frame(void) +static void xBusUnpackRJ01Frame(void) { // Calculate the CRC of the incoming frame uint16_t crc = 0; @@ -190,7 +190,7 @@ static void xBusUnpackJr01Frame(void) uint16_t value; uint8_t frameAddr; - // When using the Align JR01 receiver with + // When using the Align RJ01 receiver with // a MODE B setting in the radio (XG14 tested) // the MODE_B -frame is packed within some // at the moment unknown bytes before and after: @@ -198,7 +198,7 @@ static void xBusUnpackJr01Frame(void) // Compared to a standard MODE B frame that only // contains the "middle" package // Hence, at the moment, the unknown header and footer - // of the JR01 MODEB packages are discarded. This is + // of the RJ01 MODEB packages are discarded. This is // ok as the CRC-checksum of the embedded package works // out nicely. @@ -260,8 +260,8 @@ static void xBusDataReceive(uint16_t c) switch (xBusProvider) { case SERIALRX_XBUS_MODE_B: xBusUnpackModeBFrame(); - case SERIALRX_XBUS_MODE_B_JR01: - xBusUnpackJr01Frame(); + case SERIALRX_XBUS_MODE_B_RJ01: + xBusUnpackRJ01Frame(); } xBusDataIncoming = false; xBusFramePosition = 0; From 51dd9a43678f8ec886001c7730a7a8397c0bd5b0 Mon Sep 17 00:00:00 2001 From: Stefan Grufman Date: Thu, 15 Jan 2015 20:27:00 +0100 Subject: [PATCH 3/5] Added documentation for Align DMSS RJ01 receiver support. --- docs/Rx.md | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/docs/Rx.md b/docs/Rx.md index 98b2cee6b..0a4285e07 100644 --- a/docs/Rx.md +++ b/docs/Rx.md @@ -86,6 +86,11 @@ These receivers are reported working: XG14 14ch DMSS System w/RG731BX XBus Receiver http://www.jramericas.com/233794/JRP00631/ +There exist a remote receiver made for small BNF-models like the Align T-Rex 150 helicopter. The code also supports using the Align DMSS RJ01 receiver directly with the cleanflight software. +To use this receiver you must power it with 3V from the hardware, and then connect the serial line as other serial RX receivers. +In order for this receiver to work, you need to specify the XBUS_MODE_B_RJ01 for serialrx_provider. Note that you need to set your radio mode for XBUS "MODE B" also for this receiver to work. +Receiver name: Align DMSS RJ01 (HER15001) + ### SUMD 16 channels via serial currently supported. @@ -135,7 +140,7 @@ For Serial RX enable `RX_SERIAL` and set the `serialrx_provider` CLI setting as | SUMD | 3 | | SUMH | 4 | | XBUS_MODE_B | 5 | - +| XBUS_MODE_B_RJ01 | 6 | ### PPM/PWM input filtering. From 6376f9a8a87c6e24e70031a099469107f501a594 Mon Sep 17 00:00:00 2001 From: Stefan Grufman Date: Sun, 25 Jan 2015 10:51:07 +0100 Subject: [PATCH 4/5] Enabled the outer package CRC check. --- src/main/rx/xbus.c | 63 ++++++++++++++++++++++++++++++++++++++++++---- 1 file changed, 58 insertions(+), 5 deletions(-) diff --git a/src/main/rx/xbus.c b/src/main/rx/xbus.c index ed3015f6a..c373e293e 100644 --- a/src/main/rx/xbus.c +++ b/src/main/rx/xbus.c @@ -41,6 +41,7 @@ #define XBUS_FRAME_SIZE 27 #define XBUS_RJ01_FRAME_SIZE 33 +#define XBUS_RJ01_MESSAGE_LENGTH 30 #define XBUS_CRC_AND_VALUE 0x8000 #define XBUS_CRC_POLY 0x1021 @@ -146,6 +147,30 @@ static uint16_t xBusCRC16(uint16_t crc, uint8_t value) return crc; } +// Full RJ01 message CRC calculations +uint8_t xBusRj01CRC8(uint8_t inData, uint8_t seed) +{ + uint8_t bitsLeft; + uint8_t temp; + + for (bitsLeft = 8; bitsLeft > 0; bitsLeft--) { + temp = ((seed ^ inData) & 0x01); + + if (temp == 0) { + seed >>= 1; + } else { + seed ^= 0x18; + seed >>= 1; + seed |= 0x80; + } + + inData >>= 1; + } + + return seed; +} + + static void xBusUnpackModeBFrame(void) { // Calculate the CRC of the incoming frame @@ -184,6 +209,7 @@ static void xBusUnpackModeBFrame(void) static void xBusUnpackRJ01Frame(void) { // Calculate the CRC of the incoming frame + uint8_t outerCrc = 0; uint16_t crc = 0; uint16_t inCrc = 0; uint8_t i = 0; @@ -194,15 +220,42 @@ static void xBusUnpackRJ01Frame(void) // a MODE B setting in the radio (XG14 tested) // the MODE_B -frame is packed within some // at the moment unknown bytes before and after: - // 0xA1 __ __ 0xA1 12*(High + Low) CRC1 CRC2 + __ __ __ + // 0xA1 LEN __ 0xA1 12*(High + Low) CRC1 CRC2 + __ __ CRC_OUTER // Compared to a standard MODE B frame that only - // contains the "middle" package + // contains the "middle" package. // Hence, at the moment, the unknown header and footer - // of the RJ01 MODEB packages are discarded. This is - // ok as the CRC-checksum of the embedded package works - // out nicely. + // of the RJ01 MODEB packages are discarded. + // However, the LAST byte (CRC_OUTER) is infact an 8-bit + // CRC for the whole package, using the Dallas-One-Wire CRC + // method. + // So, we check both these values as well as the provided length + // of the outer/full message (LEN) + + // + // Check we have correct length of message + // + if (xBusFrame[1] != XBUS_RJ01_MESSAGE_LENGTH) + { + // Unknown package as length is not ok + return; + } + + // + // CRC calculation & check for full message + // + for (i = 0; i < xBusFrameLength - 1; i++) { + outerCrc = xBusRj01CRC8(outerCrc, xBusFrame[i]); + } + + if (outerCrc != xBusFrame[xBusFrameLength - 1]) + { + // CRC does not match, skip this frame + return; + } + // // Calculate CRC bytes of the "embedded MODE B frame" + // for (i = 3; i < xBusFrameLength - 5; i++) { inCrc = xBusCRC16(inCrc, xBusFrame[i]); } From 5c96d36442c269ad605a4b72e09f15e0c406c22c Mon Sep 17 00:00:00 2001 From: Stefan Grufman Date: Sun, 25 Jan 2015 13:47:45 +0100 Subject: [PATCH 5/5] Refactored the frame unpack to be used by both XBUS protocols. --- src/main/rx/xbus.c | 47 ++++++++++------------------------------------ 1 file changed, 10 insertions(+), 37 deletions(-) diff --git a/src/main/rx/xbus.c b/src/main/rx/xbus.c index c373e293e..9730e26b2 100644 --- a/src/main/rx/xbus.c +++ b/src/main/rx/xbus.c @@ -42,6 +42,7 @@ #define XBUS_RJ01_FRAME_SIZE 33 #define XBUS_RJ01_MESSAGE_LENGTH 30 +#define XBUS_RJ01_OFFSET_BYTES 3 #define XBUS_CRC_AND_VALUE 0x8000 #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 uint16_t crc = 0; @@ -181,19 +182,19 @@ static void xBusUnpackModeBFrame(void) uint8_t frameAddr; // Calculate on all bytes except the final two CRC bytes - for (i = 0; i < xBusFrameLength - 2; i++) { - inCrc = xBusCRC16(inCrc, xBusFrame[i]); + for (i = 0; i < XBUS_FRAME_SIZE - 2; i++) { + inCrc = xBusCRC16(inCrc, xBusFrame[i+offsetBytes]); } // Get the received CRC - crc = ((uint16_t)xBusFrame[xBusFrameLength-2]) << 8; - crc = crc + ((uint16_t)xBusFrame[xBusFrameLength-1]); + crc = ((uint16_t)xBusFrame[offsetBytes + XBUS_FRAME_SIZE - 2]) << 8; + crc = crc + ((uint16_t)xBusFrame[offsetBytes + XBUS_FRAME_SIZE - 1]); if (crc == inCrc) { // Unpack the data, we have a valid frame for (i = 0; i < xBusChannelCount; i++) { - frameAddr = 1 + i * 2; + frameAddr = offsetBytes + 1 + i * 2; value = ((uint16_t)xBusFrame[frameAddr]) << 8; value = value + ((uint16_t)xBusFrame[frameAddr + 1]); @@ -210,11 +211,7 @@ static void xBusUnpackRJ01Frame(void) { // Calculate the CRC of the incoming frame uint8_t outerCrc = 0; - uint16_t crc = 0; - uint16_t inCrc = 0; uint8_t i = 0; - uint16_t value; - uint8_t frameAddr; // When using the Align RJ01 receiver with // a MODE B setting in the radio (XG14 tested) @@ -253,32 +250,8 @@ static void xBusUnpackRJ01Frame(void) return; } - // - // Calculate CRC bytes of the "embedded MODE B frame" - // - 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; - } - + // Now unpack the "embedded MODE B frame" + xBusUnpackModeBFrame(XBUS_RJ01_OFFSET_BYTES); } // Receive ISR callback @@ -312,7 +285,7 @@ static void xBusDataReceive(uint16_t c) if (xBusFramePosition == xBusFrameLength) { switch (xBusProvider) { case SERIALRX_XBUS_MODE_B: - xBusUnpackModeBFrame(); + xBusUnpackModeBFrame(0); case SERIALRX_XBUS_MODE_B_RJ01: xBusUnpackRJ01Frame(); }