From e8e53ab7f19bc21d6d1b7deffcfa05f401ef629a Mon Sep 17 00:00:00 2001 From: Stefan Grufman Date: Sat, 27 Dec 2014 14:01:40 +0100 Subject: [PATCH] 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 4f569b8aaa..2799cde91e 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 60ebc97263..4c147aa684 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 92ae2eb0f2..5de472a526 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 9125151d77..11ec52075f 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; }