From 509f73853dcf2672413f0b861dd6d4c92fa393f6 Mon Sep 17 00:00:00 2001 From: Stefan Grufman Date: Mon, 15 Dec 2014 22:15:47 +0100 Subject: [PATCH 1/4] Added initial version of JRPropo XBUS Mode B support. --- Makefile | 1 + src/main/rx/rx.c | 11 +++ src/main/rx/rx.h | 3 +- src/main/rx/xbus.c | 196 +++++++++++++++++++++++++++++++++++++++++++++ src/main/rx/xbus.h | 24 ++++++ 5 files changed, 234 insertions(+), 1 deletion(-) create mode 100644 src/main/rx/xbus.c create mode 100644 src/main/rx/xbus.h diff --git a/Makefile b/Makefile index b47766f74..b70a6b344 100644 --- a/Makefile +++ b/Makefile @@ -190,6 +190,7 @@ COMMON_SRC = build_config.c \ rx/sumd.c \ rx/sumh.c \ rx/spektrum.c \ + rx/xbus.c \ sensors/acceleration.c \ sensors/battery.c \ sensors/boardalignment.c \ diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 4836bd211..2bd593d17 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -42,6 +42,7 @@ #include "rx/sumd.h" #include "rx/sumh.h" #include "rx/msp.h" +#include "rx/xbus.h" #include "rx/rx.h" @@ -100,6 +101,9 @@ void updateSerialRxFunctionConstraint(functionConstraint_t *functionConstraintTo case SERIALRX_SUMH: sumhUpdateSerialRxFunctionConstraint(functionConstraintToUpdate); break; + case SERIALRX_XBUS_MODE_B: + xbusUpdateSerialRxFunctionConstraint(functionConstraintToUpdate); + break; } } #endif @@ -109,6 +113,7 @@ void updateSerialRxFunctionConstraint(functionConstraint_t *functionConstraintTo void rxInit(rxConfig_t *rxConfig, failsafe_t *initialFailsafe) { uint8_t i; + useRxConfig(rxConfig); for (i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) { @@ -152,6 +157,9 @@ void serialRxInit(rxConfig_t *rxConfig) case SERIALRX_SUMH: enabled = sumhInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); break; + case SERIALRX_XBUS_MODE_B: + enabled = xbusInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); + break; } if (!enabled) { @@ -162,6 +170,7 @@ void serialRxInit(rxConfig_t *rxConfig) bool isSerialRxFrameComplete(rxConfig_t *rxConfig) { + switch (rxConfig->serialrx_provider) { case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM2048: @@ -172,6 +181,8 @@ bool isSerialRxFrameComplete(rxConfig_t *rxConfig) return sumdFrameComplete(); case SERIALRX_SUMH: return sumhFrameComplete(); + case SERIALRX_XBUS_MODE_B: + return xbusFrameComplete(); } return false; } diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 2c6807c3d..dd4a821d6 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -32,7 +32,8 @@ typedef enum { SERIALRX_SBUS = 2, SERIALRX_SUMD = 3, SERIALRX_SUMH = 4, - SERIALRX_PROVIDER_MAX = SERIALRX_SUMD + SERIALRX_XBUS_MODE_B = 5, + SERIALRX_PROVIDER_MAX = SERIALRX_XBUS_MODE_B } SerialRXType; #define SERIALRX_PROVIDER_COUNT (SERIALRX_PROVIDER_MAX + 1) diff --git a/src/main/rx/xbus.c b/src/main/rx/xbus.c new file mode 100644 index 000000000..9c8d23e2b --- /dev/null +++ b/src/main/rx/xbus.c @@ -0,0 +1,196 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#include "drivers/system.h" + +#include "drivers/serial.h" +#include "drivers/serial_uart.h" +#include "io/serial.h" + +#include "rx/rx.h" +#include "rx/xbus.h" + +// driver for xbus mode B receiver + +#define XBUS_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_BAUDRATE 115200 +#define XBUS_MAX_FRAME_TIME 5000 + +#define XBUS_START_OF_FRAME_BYTE (0xA1) + +static bool xbusFrameReceived = false; +static bool xbusDataIncoming = false; +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); + +static serialPort_t *xbusPort; + +void xbusUpdateSerialRxFunctionConstraint(functionConstraint_t *functionConstraint) +{ + functionConstraint->minBaudRate = XBUS_BAUDRATE; + functionConstraint->maxBaudRate = XBUS_BAUDRATE; + functionConstraint->requiredSerialPortFeatures = SPF_SUPPORTS_CALLBACK; +} + +bool xbusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback) +{ + switch (rxConfig->serialrx_provider) { + case SERIALRX_XBUS_MODE_B: + rxRuntimeConfig->channelCount = XBUS_CHANNEL_COUNT; + xbusFrameReceived = false; + xbusDataIncoming = false; + xbusFramePosition = 0; + break; + default: + return false; + break; + } + + xbusPort = openSerialPort(FUNCTION_SERIAL_RX, xbusDataReceive, XBUS_BAUDRATE, MODE_RX, SERIAL_NOT_INVERTED); + if (callback) { + *callback = xbusReadRawRC; + } + + return xbusPort != NULL; +} + +// 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; +} + +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; + } + +} + +// 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++; + } + + // Done? + if (xbusFramePosition == XBUS_FRAME_SIZE) { + xbusUnpackFrame(); + xbusDataIncoming = false; + xbusFramePosition = 0; + } +} + +// Indicate time to read a frame from the data... +bool xbusFrameComplete(void) +{ + return xbusFrameReceived; +} + +static uint16_t xbusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) +{ + uint16_t data; + + // Mark frame as read + if (xbusFrameReceived) { + xbusFrameReceived = false; + } + + // Deliver the data wanted + if (chan >= rxRuntimeConfig->channelCount) { + return 0; + } + + data = xbusChannelData[chan]; + + return data; +} diff --git a/src/main/rx/xbus.h b/src/main/rx/xbus.h new file mode 100644 index 000000000..8275e9094 --- /dev/null +++ b/src/main/rx/xbus.h @@ -0,0 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#include "rx/rx.h" + +bool xbusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); +bool xbusFrameComplete(void); +void xbusUpdateSerialRxFunctionConstraint(functionConstraint_t *functionConstraint); From 67cce3b9a0f0f358112bca953d25c72a950519a5 Mon Sep 17 00:00:00 2001 From: Stefan Grufman Date: Tue, 16 Dec 2014 18:36:43 +0100 Subject: [PATCH 2/4] Cleanup of code (whitespaces/defines etc). Added initial text for docs about XBus and its configurations. --- docs/Rx.md | 17 +++++ src/main/rx/xbus.c | 156 ++++++++++++++++++++++++--------------------- 2 files changed, 102 insertions(+), 71 deletions(-) diff --git a/docs/Rx.md b/docs/Rx.md index f61a6fa8c..23ce0540b 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 9c8d23e2b..89eb77c16 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; } From 981adf51d940eb3b72104d1b0f8790bbf2206486 Mon Sep 17 00:00:00 2001 From: Stefan Grufman Date: Sun, 21 Dec 2014 17:01:14 +0100 Subject: [PATCH 3/4] Cleanup for coding standard. --- src/main/rx/rx.c | 6 +-- src/main/rx/xbus.c | 93 ++++++++++++++++++++++------------------------ src/main/rx/xbus.h | 6 +-- 3 files changed, 51 insertions(+), 54 deletions(-) diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 2bd593d17..60ebc9726 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: - xbusUpdateSerialRxFunctionConstraint(functionConstraintToUpdate); + xBusUpdateSerialRxFunctionConstraint(functionConstraintToUpdate); break; } } @@ -158,7 +158,7 @@ void serialRxInit(rxConfig_t *rxConfig) enabled = sumhInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); break; case SERIALRX_XBUS_MODE_B: - enabled = xbusInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); + enabled = xBusInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); break; } @@ -182,7 +182,7 @@ bool isSerialRxFrameComplete(rxConfig_t *rxConfig) case SERIALRX_SUMH: return sumhFrameComplete(); case SERIALRX_XBUS_MODE_B: - return xbusFrameComplete(); + return xBusFrameComplete(); } return false; } diff --git a/src/main/rx/xbus.c b/src/main/rx/xbus.c index 89eb77c16..b8747ad77 100644 --- a/src/main/rx/xbus.c +++ b/src/main/rx/xbus.c @@ -65,55 +65,55 @@ // 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; +static bool xBusFrameReceived = false; +static bool xBusDataIncoming = false; +static uint8_t xBusFramePosition; -static volatile uint8_t xbusFrame[XBUS_FRAME_SIZE]; -static uint16_t xbusChannelData[XBUS_CHANNEL_COUNT]; +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); +static void xBusDataReceive(uint16_t c); +static uint16_t xBusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); -static serialPort_t *xbusPort; +static serialPort_t *xBusPort; -void xbusUpdateSerialRxFunctionConstraint(functionConstraint_t *functionConstraint) +void xBusUpdateSerialRxFunctionConstraint(functionConstraint_t *functionConstraint) { functionConstraint->minBaudRate = XBUS_BAUDRATE; functionConstraint->maxBaudRate = XBUS_BAUDRATE; functionConstraint->requiredSerialPortFeatures = SPF_SUPPORTS_CALLBACK; } -bool xbusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback) +bool xBusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback) { switch (rxConfig->serialrx_provider) { case SERIALRX_XBUS_MODE_B: rxRuntimeConfig->channelCount = XBUS_CHANNEL_COUNT; - xbusFrameReceived = false; - xbusDataIncoming = false; - xbusFramePosition = 0; + xBusFrameReceived = false; + xBusDataIncoming = false; + xBusFramePosition = 0; break; default: return false; break; } - xbusPort = openSerialPort(FUNCTION_SERIAL_RX, xbusDataReceive, XBUS_BAUDRATE, MODE_RX, SERIAL_NOT_INVERTED); + xBusPort = openSerialPort(FUNCTION_SERIAL_RX, xBusDataReceive, XBUS_BAUDRATE, MODE_RX, SERIAL_NOT_INVERTED); if (callback) { - *callback = xbusReadRawRC; + *callback = xBusReadRawRC; } - return xbusPort != NULL; + return xBusPort != NULL; } // The xbus mode B CRC calculations -static uint16_t xbusCRC16(uint16_t crc, uint8_t value) +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++) - { + for (i = 0; i < 8; i++) { if (crc & XBUS_CRC_AND_VALUE) { crc = crc << 1 ^ XBUS_CRC_XOR_VALUE; } else { @@ -123,7 +123,7 @@ static uint16_t xbusCRC16(uint16_t crc, uint8_t value) return crc; } -static void xbusUnpackFrame(void) +static void xBusUnpackFrame(void) { // Calculate the CRC of the incoming frame uint16_t crc = 0; @@ -133,70 +133,67 @@ 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++) - { - inCrc = xbusCRC16(inCrc, xbusFrame[i]); + 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]); + crc = ((uint16_t)xBusFrame[XBUS_CRC_BYTE_1]) << 8; + crc = crc + ((uint16_t)xBusFrame[XBUS_CRC_BYTE_2]); - if (crc == inCrc) - { + 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]); + value = ((uint16_t)xBusFrame[frameAddr]) << 8; + value = value + ((uint16_t)xBusFrame[frameAddr + 1]); // Convert to internal format - xbusChannelData[i] = XBUS_CONVERT_TO_USEC(value); + xBusChannelData[i] = XBUS_CONVERT_TO_USEC(value); } - xbusFrameReceived = true; + xBusFrameReceived = true; } } // Receive ISR callback -static void xbusDataReceive(uint16_t c) +static void xBusDataReceive(uint16_t c) { - // Check if we shall start a frame? - if ((xbusFramePosition == 0) && (c == XBUS_START_OF_FRAME_BYTE)) { - xbusDataIncoming = true; + if ((xBusFramePosition == 0) && (c == XBUS_START_OF_FRAME_BYTE)) { + xBusDataIncoming = true; } // Only do this if we are receiving to a frame - if (xbusDataIncoming == true) { + if (xBusDataIncoming == true) { // Store in frame copy - xbusFrame[xbusFramePosition] = (uint8_t)c; - xbusFramePosition++; + xBusFrame[xBusFramePosition] = (uint8_t)c; + xBusFramePosition++; } // Done? - if (xbusFramePosition == XBUS_FRAME_SIZE) { - xbusUnpackFrame(); - xbusDataIncoming = false; - xbusFramePosition = 0; + if (xBusFramePosition == XBUS_FRAME_SIZE) { + xBusUnpackFrame(); + xBusDataIncoming = false; + xBusFramePosition = 0; } } // Indicate time to read a frame from the data... -bool xbusFrameComplete(void) +bool xBusFrameComplete(void) { - return xbusFrameReceived; + return xBusFrameReceived; } -static uint16_t xbusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) +static uint16_t xBusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) { uint16_t data; // Mark frame as read - if (xbusFrameReceived) { - xbusFrameReceived = false; + if (xBusFrameReceived) { + xBusFrameReceived = false; } // Deliver the data wanted @@ -204,7 +201,7 @@ static uint16_t xbusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) return 0; } - data = xbusChannelData[chan]; + data = xBusChannelData[chan]; return data; } diff --git a/src/main/rx/xbus.h b/src/main/rx/xbus.h index 8275e9094..bc06e5e6e 100644 --- a/src/main/rx/xbus.h +++ b/src/main/rx/xbus.h @@ -19,6 +19,6 @@ #include "rx/rx.h" -bool xbusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); -bool xbusFrameComplete(void); -void xbusUpdateSerialRxFunctionConstraint(functionConstraint_t *functionConstraint); +bool xBusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); +bool xBusFrameComplete(void); +void xBusUpdateSerialRxFunctionConstraint(functionConstraint_t *functionConstraint); From 19bfabbce46fff076b9f1777a8eaee018fa79770 Mon Sep 17 00:00:00 2001 From: Stefan Grufman Date: Sun, 21 Dec 2014 22:13:23 +0100 Subject: [PATCH 4/4] Changed name of define to better name. --- src/main/rx/xbus.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/rx/xbus.c b/src/main/rx/xbus.c index b8747ad77..9125151d7 100644 --- a/src/main/rx/xbus.c +++ b/src/main/rx/xbus.c @@ -42,7 +42,7 @@ #define XBUS_CRC_BYTE_2 26 #define XBUS_CRC_AND_VALUE 0x8000 -#define XBUS_CRC_XOR_VALUE 0x1021 +#define XBUS_CRC_POLY 0x1021 #define XBUS_BAUDRATE 115200 #define XBUS_MAX_FRAME_TIME 5000 @@ -115,7 +115,7 @@ static uint16_t xBusCRC16(uint16_t crc, uint8_t value) for (i = 0; i < 8; i++) { if (crc & XBUS_CRC_AND_VALUE) { - crc = crc << 1 ^ XBUS_CRC_XOR_VALUE; + crc = crc << 1 ^ XBUS_CRC_POLY; } else { crc = crc << 1; }