diff --git a/docs/Rx.md b/docs/Rx.md index f61a6fa8c3..23ce0540b9 100644 --- a/docs/Rx.md +++ b/docs/Rx.md @@ -22,6 +22,21 @@ Allows you to use MSP commands as the RC input. Only 8 channel support to maint 16 channels via serial currently supported. +## XBus + +The firmware currently supports the MODE B version of the XBus protocol. +Make sure to set your TX to use "MODE B" for XBUS in the TX menus! +See here for info on JR's XBus protocol: http://www.jrpropo.com/english/propo/XBus/ + +Tested hardware: JR XG14 + RG731BX with NAZE32 (rev4) +With the current CLI configuration: + `set serialrx_provider=5` + `set serial_port_2_scenario=3` + `feature RX_SERIAL` + +This will set the FW to use serial RX, with XBUS_MODE_B as provider and finally the scenario to be used for serial port 2. +Please note that your config may vary depending on hw used. + ### OpenTX configuration If using OpenTX set the transmitter module to D16 mode and select CH1-16 on the transmitter before binding to allow reception @@ -53,6 +68,8 @@ For Serial RX enable `RX_SERIAL` and set the `serialrx_provider` cli setting as | SBUS | 2 | | SUMD | 3 | | SUMH | 4 | +| XBUS_MODE_B | 5 | + #### PPM/PWM input filtering. diff --git a/src/main/rx/xbus.c b/src/main/rx/xbus.c index 9c8d23e2b5..89eb77c167 100644 --- a/src/main/rx/xbus.c +++ b/src/main/rx/xbus.c @@ -30,7 +30,9 @@ #include "rx/rx.h" #include "rx/xbus.h" -// driver for xbus mode B receiver +// +// Serial driver for JR's XBus (MODE B) receiver +// #define XBUS_CHANNEL_COUNT 12 @@ -39,12 +41,30 @@ #define XBUS_CRC_BYTE_1 25 #define XBUS_CRC_BYTE_2 26 +#define XBUS_CRC_AND_VALUE 0x8000 +#define XBUS_CRC_XOR_VALUE 0x1021 #define XBUS_BAUDRATE 115200 #define XBUS_MAX_FRAME_TIME 5000 +// NOTE! +// This is actually based on ID+LENGTH (nibble each) +// 0xA - Multiplex ID (also used by JR, no idea why) +// 0x1 - 12 channels +// 0x2 - 16 channels +// However, the JR XG14 that is used for test at the moment +// does only use 0xA1 as its output. This is why the implementation +// is based on these numbers only. Maybe update this in the future? #define XBUS_START_OF_FRAME_BYTE (0xA1) +// Pulse length convertion from [0...4095] to µs: +// 800µs -> 0x000 +// 1500µs -> 0x800 +// 2200µs -> 0xFFF +// Total range is: 2200 - 800 = 1400 <==> 4095 +// Use formula: 800 + value * 1400 / 4096 (i.e. a shift by 12) +#define XBUS_CONVERT_TO_USEC(V) (800 + ((V * 1400) >> 12)) + static bool xbusFrameReceived = false; static bool xbusDataIncoming = false; static uint8_t xbusFramePosition; @@ -52,7 +72,6 @@ static uint8_t xbusFramePosition; static volatile uint8_t xbusFrame[XBUS_FRAME_SIZE]; static uint16_t xbusChannelData[XBUS_CHANNEL_COUNT]; - static void xbusDataReceive(uint16_t c); static uint16_t xbusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); @@ -75,14 +94,14 @@ bool xbusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRa xbusFramePosition = 0; break; default: - return false; + return false; break; } xbusPort = openSerialPort(FUNCTION_SERIAL_RX, xbusDataReceive, XBUS_BAUDRATE, MODE_RX, SERIAL_NOT_INVERTED); if (callback) { *callback = xbusReadRawRC; - } + } return xbusPort != NULL; } @@ -90,81 +109,76 @@ bool xbusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRa // The xbus mode B CRC calculations static uint16_t xbusCRC16(uint16_t crc, uint8_t value) { - uint8_t i; - crc = crc ^ (int16_t)value<<8; - - for (i = 0; i < 8; i++) - { - if(crc & 0x8000) { - crc = crc << 1^0x1021; - } else { - crc = crc << 1; - } - } - return crc; + uint8_t i; + crc = crc ^ (int16_t)value << 8; + + for (i = 0; i < 8; i++) + { + if (crc & XBUS_CRC_AND_VALUE) { + crc = crc << 1 ^ XBUS_CRC_XOR_VALUE; + } else { + crc = crc << 1; + } + } + return crc; } static void xbusUnpackFrame(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; - - // Calculate on all bytes except the final two CRC bytes - for (i=0; i 4095 - // 4095 / 3 = 1365 (close enough) - xbusChannelData[i] = 818 + value/3; - } - - xbusFrameReceived = true; - } - + // 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; + + // Calculate on all bytes except the final two CRC bytes + for (i = 0; i < XBUS_FRAME_SIZE - 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]); + + if (crc == inCrc) + { + // Unpack the data, we have a valid frame + for (i = 0; i < XBUS_CHANNEL_COUNT; i++) { + + frameAddr = 1 + 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) -{ - - // Check if we shall start a frame? - if ((xbusFramePosition == 0) && (c == XBUS_START_OF_FRAME_BYTE)) { - xbusDataIncoming = true; - } - - // Only do this if we are receiving to a frame - if (xbusDataIncoming == true) { - // Store in frame copy - xbusFrame[xbusFramePosition] = (uint8_t)c; - xbusFramePosition++; - } +{ + + // Check if we shall start a frame? + if ((xbusFramePosition == 0) && (c == XBUS_START_OF_FRAME_BYTE)) { + xbusDataIncoming = true; + } + + // Only do this if we are receiving to a frame + if (xbusDataIncoming == true) { + // Store in frame copy + xbusFrame[xbusFramePosition] = (uint8_t)c; + xbusFramePosition++; + } // Done? if (xbusFramePosition == XBUS_FRAME_SIZE) { - xbusUnpackFrame(); + xbusUnpackFrame(); xbusDataIncoming = false; xbusFramePosition = 0; } @@ -180,12 +194,12 @@ static uint16_t xbusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) { uint16_t data; - // Mark frame as read - if (xbusFrameReceived) { + // Mark frame as read + if (xbusFrameReceived) { xbusFrameReceived = false; } - // Deliver the data wanted + // Deliver the data wanted if (chan >= rxRuntimeConfig->channelCount) { return 0; }