PX4IO: Added support for JR XBus Mode B

JR XBus Mode B uses the same serial port parameters as the DSM port (3.3V logic, 115200, 8n1), this update adds support for decoding it.

XBus Mode B is effectively the SRXL protocol, using the Multiplex 12-channel format (start charachter 0xA1)

Decoding of XBus packets only occurs if a valid checksum is received, so this should not interfere with DSM packets.

- RC Failsafe is now disabled when XBus data is bein received

Author: Owen McAree

src/modules/px4iofirmware/controls.c

@@ -53,17 +53,19 @@
 #define RC_CHANNEL_LOW_THRESH		-8000	/* 10% threshold */
 
 static bool	ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len);
-static bool	dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated);
+static bool	dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *xbus_updated);
+
 
 static perf_counter_t c_gather_dsm;
 static perf_counter_t c_gather_sbus;
 static perf_counter_t c_gather_ppm;
 
+
 static int _dsm_fd;
 
 static uint16_t rc_value_override = 0;
 
-bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated)
+bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *xbus_updated)
 {
 	perf_begin(c_gather_dsm);
 	uint16_t temp_count = r_raw_rc_count;
@@ -106,19 +108,30 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated)
 		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
 	}
 
-	return (*dsm_updated | *st24_updated);
+    /* Attempt to parse serial data as XBus Mode B
+     *  Will only override DSM if valid XBus checksum found
+     */
+    *xbus_updated = xbus_input(bytes, n_bytes, r_raw_rc_values, &r_raw_rc_count);
+    if (*xbus_updated) {
+        r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_XBUS;
+		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
+		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
+    }
+
+	return (*dsm_updated | *st24_updated | *xbus_updated);
 }
 
 void
 controls_init(void)
 {
+    
 	/* no channels */
 	r_raw_rc_count = 0;
 	system_state.rc_channels_timestamp_received = 0;
 	system_state.rc_channels_timestamp_valid = 0;
 
-	/* DSM input (USART1) */
-	_dsm_fd = dsm_init("/dev/ttyS0");
+    /* DSM input (USART1) */
+    _dsm_fd = dsm_init("/dev/ttyS0");
 
 	/* S.bus input (USART3) */
 	sbus_init("/dev/ttyS2");
@@ -135,8 +148,8 @@ controls_init(void)
 		r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_ASSIGNMENT] = i;
 		r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS]    = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
 	}
-
-	c_gather_dsm = perf_alloc(PC_ELAPSED, "c_gather_dsm");
+    
+    c_gather_dsm = perf_alloc(PC_ELAPSED, "c_gather_dsm");
 	c_gather_sbus = perf_alloc(PC_ELAPSED, "c_gather_sbus");
 	c_gather_ppm = perf_alloc(PC_ELAPSED, "c_gather_ppm");
 }
@@ -168,16 +181,21 @@ controls_tick() {
 	}
 #endif
 
-	perf_begin(c_gather_dsm);
-	bool dsm_updated, st24_updated;
-	(void)dsm_port_input(&rssi, &dsm_updated, &st24_updated);
-	if (dsm_updated) {
-		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
-	}
-	if (st24_updated) {
-		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24;
-	}
-	perf_end(c_gather_dsm);
+    bool dsm_updated, st24_updated, xbus_updated;
+
+    perf_begin(c_gather_dsm);
+    
+    (void)dsm_port_input(&rssi, &dsm_updated, &st24_updated, &xbus_updated);
+    if (dsm_updated) {
+        r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
+    }
+    if (st24_updated) {
+        r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24;
+    }
+    if (xbus_updated) {
+        r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_XBUS;
+    }
+    perf_end(c_gather_dsm);
 
 	perf_begin(c_gather_sbus);
 
@@ -238,7 +256,7 @@ controls_tick() {
 	/*
 	 * If we received a new frame from any of the RC sources, process it.
 	 */
-	if (dsm_updated || sbus_updated || ppm_updated || st24_updated) {
+	if (dsm_updated || xbus_updated || sbus_updated || ppm_updated || st24_updated) {
 
 		/* record a bitmask of channels assigned */
 		unsigned assigned_channels = 0;
@@ -367,6 +385,7 @@ controls_tick() {
 		r_status_flags &= ~(
 			PX4IO_P_STATUS_FLAGS_RC_PPM |
 			PX4IO_P_STATUS_FLAGS_RC_DSM |
+            PX4IO_P_STATUS_FLAGS_RC_XBUS |
 			PX4IO_P_STATUS_FLAGS_RC_SBUS);
 
 	}
@@ -438,7 +457,7 @@ controls_tick() {
 			r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
 
 			/* mix new RC input control values to servos */
-			if (dsm_updated || sbus_updated || ppm_updated || st24_updated)
+			if (dsm_updated || xbus_updated || sbus_updated || ppm_updated || st24_updated)
 				mixer_tick();
 
 		} else {

src/modules/px4iofirmware/module.mk

@@ -2,6 +2,7 @@
 SRCS		= adc.c \
 		  controls.c \
 		  dsm.c \
+		  xbus.c \
 		  px4io.c \
 		  registers.c \
 		  safety.c \
@@ -26,4 +27,4 @@ endif
 
 SELF_DIR := $(dir $(lastword $(MAKEFILE_LIST)))
 include $(SELF_DIR)../systemlib/mixer/multi_tables.mk
-	
+	

src/modules/px4iofirmware/protocol.h

@@ -115,6 +115,7 @@
 #define PX4IO_P_STATUS_FLAGS_SAFETY_OFF		(1 << 12) /* safety is off */
 #define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED	(1 << 13) /* FMU was initialized and OK once */
 #define PX4IO_P_STATUS_FLAGS_RC_ST24		(1 << 14) /* ST24 input is valid */
+#define PX4IO_P_STATUS_FLAGS_RC_XBUS		(1 << 15) /* XBus input is valid */
 
 #define PX4IO_P_STATUS_ALARMS			3	 /* alarm flags - alarms latch, write 1 to a bit to clear it */
 #define PX4IO_P_STATUS_ALARMS_VBATT_LOW		(1 << 0) /* [1] VBatt is very close to regulator dropout */

src/modules/px4iofirmware/px4io.h

@@ -217,6 +217,7 @@ extern void	controls_init(void);
 extern void	controls_tick(void);
 extern int	dsm_init(const char *device);
 extern bool	dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes);
+extern bool	xbus_input(uint8_t *bytes, uint16_t num_bytes, uint16_t *values, uint16_t *num_values);
 extern void	dsm_bind(uint16_t cmd, int pulses);
 extern int	sbus_init(const char *device);
 extern bool	sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels);

src/modules/px4iofirmware/xbus.c

@@ -0,0 +1,152 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file dsm.c
+ *
+ * Serial protocol decoder for the JR XBus protocol.
+ * 
+ * Uses XBus Mode B which is the Multiplex SRXL protocol, details here
+ * http://www.multiplex-rc.de/en/service/downloads/interface-descriptions.html?eID=dam_frontend_push&docID=4233
+ *
+ * Decodes into the global PPM buffer and updates accordingly.
+ */
+
+#include <nuttx/config.h>
+#include <nuttx/arch.h>
+
+#include <fcntl.h>
+#include <unistd.h>
+#include <termios.h>
+#include <string.h>
+
+#include <drivers/drv_hrt.h>
+
+#include "px4io.h"
+
+#define XBUS_NUM_CHANNELS	12		/**<Number of XBus channels*/
+#define XBUS_PACKET_LENGTH     3 + XBUS_NUM_CHANNELS*2
+
+static int bytesReceived = 0;       /**<Number of bytes received since start character */
+static uint8_t xbus_buffer[XBUS_PACKET_LENGTH];
+
+uint16_t CRC16(uint16_t crc, uint8_t value);
+
+uint16_t CRC16(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;
+}
+
+/**
+ * Called periodically to check for input data from the XBus UART
+ * 
+ * 
+ *
+ * @param[in] bytes received through DSM serial port
+ * @param[in] number of bytes received through DSM serial port
+ * @param[out] values pointer to per channel array of decoded values
+ * @param[out] num_values pointer to number of raw channel values returned
+
+ * @return true=decoded raw channel values updated, false=no update
+ */
+bool
+xbus_input(uint8_t *bytes, uint16_t num_bytes, uint16_t *values, uint16_t *num_values)
+{
+    /* If we have no new data, return here */
+    if (num_bytes < 1)
+        return false;
+    
+    /* Copy new bytes in to our buffer */
+    memcpy(xbus_buffer + bytesReceived, bytes, num_bytes);
+    
+    /* If start character is not correct, reset everything */
+    if (xbus_buffer[0] != 0xA1) {
+        memset(xbus_buffer, '\0', XBUS_PACKET_LENGTH);
+        bytesReceived = 0;
+        return false;
+    }
+    
+    /* Increment buffer length */
+    bytesReceived += num_bytes;
+    
+    /* If we don't have a full packet, return here */
+    if (bytesReceived < XBUS_PACKET_LENGTH)
+        return false;
+    
+    /* Reset counter ready for next packet */
+    bytesReceived = 0;
+    
+    /* Check CRC of packet */
+    uint16_t crc_calc = 0;
+    for (int i=0; i< XBUS_PACKET_LENGTH-2; i++) {
+        crc_calc = CRC16(crc_calc, xbus_buffer[i]);
+    }
+    uint16_t crc_buffer = ((uint16_t)(xbus_buffer[XBUS_PACKET_LENGTH - 2]))<<8 | (uint16_t)(xbus_buffer[XBUS_PACKET_LENGTH - 1]);
+    if (crc_calc != crc_buffer)
+        return false;
+    
+    
+    /* Apply this channel mapping to get correct order 
+     *  JR XBus Mode B order is:
+     *      - Aileron
+     *      - Elevator
+     *      - Rudder
+     *      - Aux 2 (Flap)
+     *      - Throttle
+     *      - Aux 1 (Gear)
+     *      - Aux 3
+     *      - Aux 4
+     *      - etc...
+     **/
+    uint8_t chMap[] = {1, 2, 3, 5, 0, 4, 6, 7, 8, 9, 10, 11};
+    *num_values = XBUS_NUM_CHANNELS;
+    
+    /* Channel scaling is linear between
+     * - 0x000: 800us
+     * - 0xfff: 2200us
+     **/
+    for (int channel = 0; channel<XBUS_NUM_CHANNELS; channel++) {
+        uint32_t value = (((uint16_t)xbus_buffer[2*channel+1])<<8) | ((uint16_t)xbus_buffer[2*channel+2]);
+        double dblVal = value*(2200.0-800.0)/4095.0 + 800.0;
+        values[chMap[channel]] = (uint16_t)dblVal;
+    }
+    return true;
+}