Merge pull request #1339 from PX4/st24

Yuneec ST24 protocol decoding

Tools/tests-host/.gitignore

@@ -3,3 +3,4 @@ mixer_test
 sf0x_test
 sbus2_test
 autodeclination_test
+st24_test

Tools/tests-host/Makefile

@@ -3,7 +3,7 @@ CC=g++
 CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers \
 	-I../../src -I../../src/lib -D__EXPORT="" -Dnullptr="0" -lm
 
-all: mixer_test sbus2_test autodeclination_test sf0x_test
+all: mixer_test sbus2_test autodeclination_test st24_test sf0x_test
 
 MIXER_FILES=../../src/systemcmds/tests/test_mixer.cpp \
 		../../src/systemcmds/tests/test_conv.cpp \
@@ -20,6 +20,10 @@ SBUS2_FILES=../../src/modules/px4iofirmware/sbus.c \
 		hrt.cpp \
 		sbus2_test.cpp
 
+ST24_FILES=../../src/lib/rc/st24.c \
+		hrt.cpp \
+		st24_test.cpp
+
 SF0X_FILES= \
 		hrt.cpp \
 		sf0x_test.cpp \
@@ -41,7 +45,10 @@ sf0x_test: $(SF0X_FILES)
 autodeclination_test: $(SBUS2_FILES)
 	$(CC) -o autodeclination_test $(AUTODECLINATION_FILES) $(CFLAGS)
 
+st24_test: $(ST24_FILES)
+	$(CC) -o st24_test $(ST24_FILES) $(CFLAGS)
+
 .PHONY: clean
 
 clean:
-	rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test sf0x_test
+	rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test st24_test sf0x_test

Tools/tests-host/sbus2_test.cpp

@@ -29,7 +29,8 @@ int main(int argc, char *argv[]) {
 
 	// Trash the first 20 lines
 	for (unsigned i = 0; i < 20; i++) {
-		(void)fscanf(fp, "%f,%x,,", &f, &x);
+		char buf[200];
+		(void)fgets(buf, sizeof(buf), fp);
 	}
 
 	// Init the parser

Tools/tests-host/st24_test.cpp

@@ -0,0 +1,76 @@
+
+#include <stdio.h>
+#include <unistd.h>
+#include <string.h>
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+#include <rc/st24.h>
+#include "../../src/systemcmds/tests/tests.h"
+
+int main(int argc, char *argv[])
+{
+	warnx("ST24 test started");
+
+	if (argc < 2) {
+		errx(1, "Need a filename for the input file");
+	}
+
+	warnx("loading data from: %s", argv[1]);
+
+	FILE *fp;
+
+	fp = fopen(argv[1], "rt");
+
+	if (!fp) {
+		errx(1, "failed opening file");
+	}
+
+	float f;
+	unsigned x;
+	int ret;
+
+	// Trash the first 20 lines
+	for (unsigned i = 0; i < 20; i++) {
+		char buf[200];
+		(void)fgets(buf, sizeof(buf), fp);
+	}
+
+	float last_time = 0;
+
+	while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
+		if (((f - last_time) * 1000 * 1000) > 3000) {
+			// warnx("FRAME RESET\n\n");
+		}
+
+		uint8_t b = static_cast<uint8_t>(x);
+
+		last_time = f;
+
+		// Pipe the data into the parser
+		hrt_abstime now = hrt_absolute_time();
+
+		uint8_t rssi;
+		uint8_t rx_count;
+		uint16_t channel_count;
+		uint16_t channels[20];
+
+
+		if (!st24_decode(b, &rssi, &rx_count, &channel_count, channels, sizeof(channels) / sizeof(channels[0]))) {
+			warnx("decoded: %u channels (converted to PPM range)", (unsigned)channel_count);
+
+			for (unsigned i = 0; i < channel_count; i++) {
+
+				int16_t val = channels[i];
+				warnx("channel %u: %d 0x%03X", i, static_cast<int>(val), static_cast<int>(val));
+			}
+		}
+	}
+
+	if (ret == EOF) {
+		warnx("Test finished, reached end of file");
+
+	} else {
+		warnx("Test aborted, errno: %d", ret);
+	}
+
+}

src/drivers/drv_rc_input.h

@@ -83,7 +83,8 @@ enum RC_INPUT_SOURCE {
 	RC_INPUT_SOURCE_PX4FMU_PPM,
 	RC_INPUT_SOURCE_PX4IO_PPM,
 	RC_INPUT_SOURCE_PX4IO_SPEKTRUM,
-	RC_INPUT_SOURCE_PX4IO_SBUS
+	RC_INPUT_SOURCE_PX4IO_SBUS,
+	RC_INPUT_SOURCE_PX4IO_ST24
 };
 
 /**

src/drivers/px4io/px4io.cpp

@@ -1617,6 +1617,9 @@ PX4IO::io_publish_raw_rc()
 	} else if (_status & PX4IO_P_STATUS_FLAGS_RC_SBUS) {
 		rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SBUS;
 
+	} else if (_status & PX4IO_P_STATUS_FLAGS_RC_ST24) {
+		rc_val.input_source = RC_INPUT_SOURCE_PX4IO_ST24;
+
 	} else {
 		rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN;
 
@@ -1934,13 +1937,15 @@ PX4IO::print_status(bool extended_status)
 	       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM));
 	uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS);
 	uint16_t io_status_flags = flags;
-	printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n",
+	printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s%s%s\n",
 	       flags,
 	       ((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""),
 	       ((flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? " SAFETY_OFF" : " SAFETY_SAFE"),
 	       ((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""),
 	       ((flags & PX4IO_P_STATUS_FLAGS_RC_OK)    ? " RC_OK" : " RC_FAIL"),
 	       ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)   ? " PPM" : ""),
+	       ((flags & PX4IO_P_STATUS_FLAGS_RC_DSM)   ? " DSM" : ""),
+	       ((flags & PX4IO_P_STATUS_FLAGS_RC_ST24)   ? " ST24" : ""),
 	       ((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)  ? " SBUS" : ""),
 	       ((flags & PX4IO_P_STATUS_FLAGS_FMU_OK)   ? " FMU_OK" : " FMU_FAIL"),
 	       ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)  ? " RAW_PWM_PASSTHROUGH" : ""),
@@ -2465,6 +2470,9 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
 			} else if (status & PX4IO_P_STATUS_FLAGS_RC_SBUS) {
 				rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SBUS;
 
+			} else if (status & PX4IO_P_STATUS_FLAGS_RC_ST24) {
+				rc_val->input_source = RC_INPUT_SOURCE_PX4IO_ST24;
+
 			} else {
 				rc_val->input_source = RC_INPUT_SOURCE_UNKNOWN;
 			}

src/lib/rc/module.mk

@@ -0,0 +1,40 @@
+############################################################################
+#
+#   Copyright (c) 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.
+#
+############################################################################
+
+#
+# Yuntec ST24 transmitter protocol decoder
+#
+
+SRCS		 =	st24.c
+
+MAXOPTIMIZATION	 = -Os

src/lib/rc/st24.c

@@ -0,0 +1,253 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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 st24.h
+ *
+ * RC protocol implementation for Yuneec ST24 transmitter.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#include <stdbool.h>
+#include <stdio.h>
+#include "st24.h"
+
+enum ST24_DECODE_STATE {
+	ST24_DECODE_STATE_UNSYNCED = 0,
+	ST24_DECODE_STATE_GOT_STX1,
+	ST24_DECODE_STATE_GOT_STX2,
+	ST24_DECODE_STATE_GOT_LEN,
+	ST24_DECODE_STATE_GOT_TYPE,
+	ST24_DECODE_STATE_GOT_DATA
+};
+
+const char *decode_states[] = {"UNSYNCED",
+			       "GOT_STX1",
+			       "GOT_STX2",
+			       "GOT_LEN",
+			       "GOT_TYPE",
+			       "GOT_DATA"
+			      };
+
+/* define range mapping here, -+100% -> 1000..2000 */
+#define ST24_RANGE_MIN 0.0f
+#define ST24_RANGE_MAX 4096.0f
+
+#define ST24_TARGET_MIN 1000.0f
+#define ST24_TARGET_MAX 2000.0f
+
+/* pre-calculate the floating point stuff as far as possible at compile time */
+#define ST24_SCALE_FACTOR ((ST24_TARGET_MAX - ST24_TARGET_MIN) / (ST24_RANGE_MAX - ST24_RANGE_MIN))
+#define ST24_SCALE_OFFSET (int)(ST24_TARGET_MIN - (ST24_SCALE_FACTOR * ST24_RANGE_MIN + 0.5f))
+
+static enum ST24_DECODE_STATE _decode_state = ST24_DECODE_STATE_UNSYNCED;
+static unsigned _rxlen;
+
+static ReceiverFcPacket _rxpacket;
+
+uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len)
+{
+	uint8_t i, crc ;
+	crc = 0;
+
+	while (len--) {
+		for (i = 0x80; i != 0; i >>= 1) {
+			if ((crc & 0x80) != 0) {
+				crc <<= 1;
+				crc ^= 0x07;
+
+			} else {
+				crc <<= 1;
+			}
+
+			if ((*ptr & i) != 0) {
+				crc ^= 0x07;
+			}
+		}
+
+		ptr++;
+	}
+
+	return (crc);
+}
+
+
+int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, uint16_t *channels,
+		    uint16_t max_chan_count)
+{
+
+	int ret = 1;
+
+	switch (_decode_state) {
+	case ST24_DECODE_STATE_UNSYNCED:
+		if (byte == ST24_STX1) {
+			_decode_state = ST24_DECODE_STATE_GOT_STX1;
+		} else {
+			ret = 3;
+		}
+
+		break;
+
+	case ST24_DECODE_STATE_GOT_STX1:
+		if (byte == ST24_STX2) {
+			_decode_state = ST24_DECODE_STATE_GOT_STX2;
+
+		} else {
+			_decode_state = ST24_DECODE_STATE_UNSYNCED;
+		}
+
+		break;
+
+	case ST24_DECODE_STATE_GOT_STX2:
+
+		/* ensure no data overflow failure or hack is possible */
+		if ((unsigned)byte <= sizeof(_rxpacket.length) + sizeof(_rxpacket.type) + sizeof(_rxpacket.st24_data)) {
+			_rxpacket.length = byte;
+			_rxlen = 0;
+			_decode_state = ST24_DECODE_STATE_GOT_LEN;
+
+		} else {
+			_decode_state = ST24_DECODE_STATE_UNSYNCED;
+		}
+
+		break;
+
+	case ST24_DECODE_STATE_GOT_LEN:
+		_rxpacket.type = byte;
+		_rxlen++;
+		_decode_state = ST24_DECODE_STATE_GOT_TYPE;
+		break;
+
+	case ST24_DECODE_STATE_GOT_TYPE:
+		_rxpacket.st24_data[_rxlen - 1] = byte;
+		_rxlen++;
+
+		if (_rxlen == (_rxpacket.length - 1)) {
+			_decode_state = ST24_DECODE_STATE_GOT_DATA;
+		}
+
+		break;
+
+	case ST24_DECODE_STATE_GOT_DATA:
+		_rxpacket.crc8 = byte;
+		_rxlen++;
+
+		if (st24_common_crc8((uint8_t *) & (_rxpacket.length), _rxlen) == _rxpacket.crc8) {
+
+			ret = 0;
+
+			/* decode the actual packet */
+
+			switch (_rxpacket.type) {
+
+			case ST24_PACKET_TYPE_CHANNELDATA12: {
+					ChannelData12 *d = (ChannelData12 *)_rxpacket.st24_data;
+
+					*rssi = d->rssi;
+					*rx_count = d->packet_count;
+
+					/* this can lead to rounding of the strides */
+					*channel_count = (max_chan_count < 12) ? max_chan_count : 12;
+
+					unsigned stride_count = (*channel_count * 3) / 2;
+					unsigned chan_index = 0;
+
+					for (unsigned i = 0; i < stride_count; i += 3) {
+						channels[chan_index] = ((uint16_t)d->channel[i] << 4);
+						channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4);
+						/* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
+						channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET;
+						chan_index++;
+
+						channels[chan_index] = ((uint16_t)d->channel[i + 2]);
+						channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8);
+						/* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
+						channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET;
+						chan_index++;
+					}
+				}
+				break;
+
+			case ST24_PACKET_TYPE_CHANNELDATA24: {
+					ChannelData24 *d = (ChannelData24 *)&_rxpacket.st24_data;
+
+					*rssi = d->rssi;
+					*rx_count = d->packet_count;
+
+					/* this can lead to rounding of the strides */
+					*channel_count = (max_chan_count < 24) ? max_chan_count : 24;
+
+					unsigned stride_count = (*channel_count * 3) / 2;
+					unsigned chan_index = 0;
+
+					for (unsigned i = 0; i < stride_count; i += 3) {
+						channels[chan_index] = ((uint16_t)d->channel[i] << 4);
+						channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4);
+						/* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
+						channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET;
+						chan_index++;
+
+						channels[chan_index] = ((uint16_t)d->channel[i + 2]);
+						channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8);
+						/* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
+						channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET;
+						chan_index++;
+					}
+				}
+				break;
+
+			case ST24_PACKET_TYPE_TRANSMITTERGPSDATA: {
+
+					// ReceiverFcPacket* d = (ReceiverFcPacket*)&_rxpacket.st24_data;
+					/* we silently ignore this data for now, as it is unused */
+					ret = 2;
+				}
+				break;
+
+			default:
+				ret = 2;
+				break;
+			}
+
+		} else {
+			/* decoding failed */
+			ret = 4;
+		}
+
+		_decode_state = ST24_DECODE_STATE_UNSYNCED;
+		break;
+	}
+
+	return ret;
+}

src/lib/rc/st24.h

@@ -0,0 +1,163 @@
+/****************************************************************************
+ *
+ *	Copyright (c) 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 st24.h
+ *
+ * RC protocol definition for Yuneec ST24 transmitter
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+__BEGIN_DECLS
+
+#define ST24_DATA_LEN_MAX	64
+#define ST24_STX1		0x55
+#define ST24_STX2		0x55
+
+enum ST24_PACKET_TYPE {
+	ST24_PACKET_TYPE_CHANNELDATA12 = 0,
+	ST24_PACKET_TYPE_CHANNELDATA24,
+	ST24_PACKET_TYPE_TRANSMITTERGPSDATA
+};
+
+#pragma pack(push, 1)
+typedef struct {
+	uint8_t	header1;			///< 0x55 for a valid packet
+	uint8_t	header2;			///< 0x55 for a valid packet
+	uint8_t	length;				///< length includes type, data, and crc = sizeof(type)+sizeof(data[payload_len])+sizeof(crc8)
+	uint8_t	type;				///< from enum ST24_PACKET_TYPE
+	uint8_t	st24_data[ST24_DATA_LEN_MAX];
+	uint8_t	crc8;				///< crc8 checksum, calculated by st24_common_crc8 and including fields length, type and st24_data
+} ReceiverFcPacket;
+
+/**
+ * RC Channel data (12 channels).
+ *
+ * This is incoming from the ST24
+ */
+typedef struct {
+	uint16_t t;			///< packet counter or clock
+	uint8_t	rssi;			///< signal strength
+	uint8_t	packet_count;		///< Number of UART packets sent since reception of last RF frame (this tells something about age / rate)
+	uint8_t	channel[18];		///< channel data, 12 channels (12 bit numbers)
+} ChannelData12;
+
+/**
+ * RC Channel data (12 channels).
+ *
+ */
+typedef struct {
+	uint16_t t;			///< packet counter or clock
+	uint8_t	rssi;			///< signal strength
+	uint8_t	packet_count;		///< Number of UART packets sent since reception of last RF frame (this tells something about age / rate)
+	uint8_t	channel[36];		///< channel data, 24 channels (12 bit numbers)
+} ChannelData24;
+
+/**
+ * Telemetry packet
+ *
+ * This is outgoing to the ST24
+ *
+ * imuStatus:
+ * 8 bit total
+ * bits 0-2 for status
+ * - value 0 is FAILED
+ * - value 1 is INITIALIZING
+ * - value 2 is RUNNING
+ * - values 3 through 7 are reserved
+ * bits 3-7 are status for sensors (0 or 1)
+ * - mpu6050
+ * - accelerometer
+ * - primary gyro x
+ * - primary gyro y
+ * - primary gyro z
+ *
+ * pressCompassStatus
+ * 8 bit total
+ * bits 0-3 for compass status
+ * - value 0 is FAILED
+ * - value 1 is INITIALIZING
+ * - value 2 is RUNNING
+ * - value 3 - 15 are reserved
+ * bits 4-7 for pressure status
+ * - value 0 is FAILED
+ * - value 1 is INITIALIZING
+ * - value 2 is RUNNING
+ * - value 3 - 15 are reserved
+ *
+ */
+typedef struct {
+	uint16_t t;			///< packet counter or clock
+	int32_t	lat;			///< lattitude (degrees)	+/- 90 deg
+	int32_t	lon;			///< longitude (degrees)	+/- 180 deg
+	int32_t	alt;			///< 0.01m resolution, altitude (meters)
+	int16_t	vx, vy, vz; 		///< velocity 0.01m res, +/-320.00 North-East- Down
+	uint8_t	nsat;			///<number of satellites
+	uint8_t	voltage; 		///< 25.4V	voltage = 5 + 255*0.1 = 30.5V, min=5V
+	uint8_t	current; 		///< 0.5A resolution
+	int16_t	roll, pitch, yaw;	///< 0.01 degree resolution
+	uint8_t	motorStatus;		///< 1 bit per motor for status 1=good, 0= fail
+	uint8_t	imuStatus;		///< inertial measurement unit status
+	uint8_t	pressCompassStatus;	///< baro / compass status
+} TelemetryData;
+
+#pragma pack(pop)
+
+/**
+ * CRC8 implementation for ST24 protocol
+ *
+ * @param prt Pointer to the data to CRC
+ * @param len number of bytes to accumulate in the checksum
+ * @return the checksum of these bytes over len
+ */
+uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len);
+
+/**
+ * Decoder for ST24 protocol
+ *
+ * @param byte current char to read
+ * @param rssi pointer to a byte where the RSSI value is written back to
+ * @param rx_count pointer to a byte where the receive count of packets signce last wireless frame is written back to
+ * @param channels pointer to a datastructure of size max_chan_count where channel values (12 bit) are written back to
+ * @param max_chan_count maximum channels to decode - if more channels are decoded, the last n are skipped and success (0) is returned
+ * @return 0 for success (a decoded packet), 1 for no packet yet (accumulating), 2 for unknown packet, 3 for out of sync, 4 for checksum error
+ */
+__EXPORT int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count,
+			     uint16_t *channels, uint16_t max_chan_count);
+
+__END_DECLS

src/modules/px4iofirmware/controls.c

@@ -43,6 +43,7 @@
 #include <drivers/drv_hrt.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/ppm_decode.h>
+#include <rc/st24.h>
 
 #include "px4io.h"
 
@@ -51,11 +52,60 @@
 #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(uint8_t *rssi, bool *dsm_updated, bool *st24_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;
+
+bool dsm_port_input(uint8_t *rssi, bool *dsm_updated, bool *st24_updated)
+{
+	perf_begin(c_gather_dsm);
+	uint16_t temp_count = r_raw_rc_count;
+	uint8_t n_bytes = 0;
+	uint8_t *bytes;
+	*dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes);
+	if (*dsm_updated) {
+		r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
+		r_raw_rc_count = temp_count & 0x7fff;
+		if (temp_count & 0x8000)
+			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
+		else
+			r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
+
+		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
+		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
+
+	}
+	perf_end(c_gather_dsm);
+
+	/* get data from FD and attempt to parse with DSM and ST24 libs */
+	uint8_t st24_rssi, rx_count;
+	uint16_t st24_channel_count = 0;
+
+	*st24_updated = false;
+
+	for (unsigned i = 0; i < n_bytes; i++) {
+		/* set updated flag if one complete packet was parsed */
+		*st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &rx_count,
+					&st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
+	}
+
+	if (*st24_updated) {
+
+		*rssi = st24_rssi;
+		r_raw_rc_count = st24_channel_count;
+
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24;
+		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);
+}
+
 void
 controls_init(void)
 {
@@ -65,7 +115,7 @@ controls_init(void)
 	system_state.rc_channels_timestamp_valid = 0;
 
 	/* DSM input (USART1) */
-	dsm_init("/dev/ttyS0");
+	_dsm_fd = dsm_init("/dev/ttyS0");
 
 	/* S.bus input (USART3) */
 	sbus_init("/dev/ttyS2");
@@ -116,20 +166,8 @@ controls_tick() {
 #endif
 
 	perf_begin(c_gather_dsm);
-	uint16_t temp_count = r_raw_rc_count;
+	bool dsm_updated, st24_updated;
-	bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count);
+	(void)dsm_port_input(&rssi, &dsm_updated, &st24_updated);
-	if (dsm_updated) {
-		r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
-		r_raw_rc_count = temp_count & 0x7fff;
-		if (temp_count & 0x8000)
-			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
-		else
-			r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
-
-		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
-		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
-
-	}
 	perf_end(c_gather_dsm);
 
 	perf_begin(c_gather_sbus);
@@ -191,7 +229,7 @@ controls_tick() {
 	/*
 	 * If we received a new frame from any of the RC sources, process it.
 	 */
-	if (dsm_updated || sbus_updated || ppm_updated) {
+	if (dsm_updated || sbus_updated || ppm_updated || st24_updated) {
 
 		/* record a bitmask of channels assigned */
 		unsigned assigned_channels = 0;
@@ -379,7 +417,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)
+			if (dsm_updated || sbus_updated || ppm_updated || st24_updated)
 				mixer_tick();
 
 		} else {

src/modules/px4iofirmware/dsm.c

@@ -452,10 +452,12 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
  *
  * @param[out] values pointer to per channel array of decoded values
  * @param[out] num_values pointer to number of raw channel values returned, high order bit 0:10 bit data, 1:11 bit data
+ * @param[out] n_butes number of bytes read
+ * @param[out] bytes pointer to the buffer of read bytes
  * @return true=decoded raw channel values updated, false=no update
  */
 bool
-dsm_input(uint16_t *values, uint16_t *num_values)
+dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes)
 {
 	ssize_t		ret;
 	hrt_abstime	now;
@@ -478,8 +480,12 @@ dsm_input(uint16_t *values, uint16_t *num_values)
 	ret = read(dsm_fd, &dsm_frame[dsm_partial_frame_count], DSM_FRAME_SIZE - dsm_partial_frame_count);
 
 	/* if the read failed for any reason, just give up here */
-	if (ret < 1)
+	if (ret < 1) {
 		return false;
+	} else {
+		*n_bytes = ret;
+		*bytes = &dsm_frame[dsm_partial_frame_count];
+	}
 
 	dsm_last_rx_time = now;
 

src/modules/px4iofirmware/module.mk

@@ -14,7 +14,8 @@ SRCS		= adc.c \
 		  ../systemlib/mixer/mixer_group.cpp \
 		  ../systemlib/mixer/mixer_multirotor.cpp \
 		  ../systemlib/mixer/mixer_simple.cpp \
-		  ../systemlib/pwm_limit/pwm_limit.c
+		  ../systemlib/pwm_limit/pwm_limit.c \
+		  ../../lib/rc/st24.c
 
 ifeq ($(BOARD),px4io-v1)
 SRCS		+= i2c.c

src/modules/px4iofirmware/protocol.h

@@ -112,6 +112,7 @@
 #define PX4IO_P_STATUS_FLAGS_FAILSAFE		(1 << 11) /* failsafe is active */
 #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_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

@@ -215,7 +215,7 @@ extern uint16_t	adc_measure(unsigned channel);
 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);
+extern bool	dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes);
 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);