From af4d5c0a3c985007958e512db977832bc93b2553 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Jul 2014 13:14:39 +0200 Subject: [PATCH 01/21] Added initial bits for ST24 decoding --- src/lib/rc/st24.c | 66 ++++++++++++++++++++++ src/lib/rc/st24.h | 140 ++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 206 insertions(+) create mode 100644 src/lib/rc/st24.c create mode 100644 src/lib/rc/st24.h diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c new file mode 100644 index 0000000000..8287a0453e --- /dev/null +++ b/src/lib/rc/st24.c @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * 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 + */ + +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); +} diff --git a/src/lib/rc/st24.h b/src/lib/rc/st24.h new file mode 100644 index 0000000000..1c92bad436 --- /dev/null +++ b/src/lib/rc/st24.h @@ -0,0 +1,140 @@ +/**************************************************************************** + * + * 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 + */ + +#define ST24_DATA_LEN_MAX 64 + +enum { + ST24_PACKET_TYPE_CHANNELDATA12 = 0, + ST24_PACKET_TYPE_CHANNELDATA24, + ST24_PACKET_TYPE_TRANSMITTERGPSDATA +} ST24_PACKET_TYPE; + +#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; /// Date: Thu, 10 Jul 2014 13:21:29 +0200 Subject: [PATCH 02/21] More protocol decoding skeleton code --- src/lib/rc/module.mk | 40 ++++++++++++++++++++++++++++++++++++++++ src/lib/rc/st24.c | 3 +++ src/lib/rc/st24.h | 14 +++++++++++++- 3 files changed, 56 insertions(+), 1 deletion(-) create mode 100644 src/lib/rc/module.mk diff --git a/src/lib/rc/module.mk b/src/lib/rc/module.mk new file mode 100644 index 0000000000..e089c69658 --- /dev/null +++ b/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 diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index 8287a0453e..bae28c85e3 100644 --- a/src/lib/rc/st24.c +++ b/src/lib/rc/st24.c @@ -64,3 +64,6 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len) return (crc); } + + +uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channels, uint16_t max_chan_count) \ No newline at end of file diff --git a/src/lib/rc/st24.h b/src/lib/rc/st24.h index 1c92bad436..3621e8506e 100644 --- a/src/lib/rc/st24.h +++ b/src/lib/rc/st24.h @@ -137,4 +137,16 @@ typedef struct { * @param len number of bytes to accumulate in the checksum * @return the checksum of these bytes over len */ -__EXPORT uint8_t st24_common_crc8(uint8_t *ptr, uint8_t 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), 3 for out of sync, 4 for checksum error + */ +__EXPORT uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channels, uint16_t max_chan_count); From 25a8f2d8056deca5049a8acc27c77e5e17760167 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Jul 2014 14:30:34 +0200 Subject: [PATCH 03/21] ST24 decoding skeleton --- src/lib/rc/st24.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index bae28c85e3..addbcd899d 100644 --- a/src/lib/rc/st24.c +++ b/src/lib/rc/st24.c @@ -66,4 +66,6 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len) } -uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channels, uint16_t max_chan_count) \ No newline at end of file +uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channels, uint16_t max_chan_count) { + +} \ No newline at end of file From 69109b622796dacd6f78ebafa92b10379ba0d7b4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 1 Sep 2014 00:45:41 +0200 Subject: [PATCH 04/21] Compile and link ST24 parser in IO firmware --- src/lib/rc/st24.c | 120 ++++++++++++++++++++++++++- src/lib/rc/st24.h | 48 ++++++----- src/modules/px4iofirmware/controls.c | 27 +++++- src/modules/px4iofirmware/module.mk | 3 +- 4 files changed, 174 insertions(+), 24 deletions(-) diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index addbcd899d..956a72bf35 100644 --- a/src/lib/rc/st24.c +++ b/src/lib/rc/st24.c @@ -39,6 +39,23 @@ * @author Lorenz Meier */ +#include +#include "st24.h" + +enum ST24_DECODE_STATE { + ST24_DECODE_STATE_UNSYNCED, + 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 +}; + +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 ; @@ -66,6 +83,105 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len) } -uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channels, uint16_t max_chan_count) { +uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channels, uint16_t max_chan_count) +{ -} \ No newline at end of file + bool ret = false; + + switch (_decode_state) { + case ST24_DECODE_STATE_UNSYNCED: + if (byte == ST24_STX1) { + _decode_state = ST24_DECODE_STATE_GOT_STX1; + } + 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: + _rxpacket.length = byte; + _rxlen = 0; + _decode_state = ST24_DECODE_STATE_GOT_LEN; + 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: + if (_rxlen < (_rxpacket.length - 1)) { + _rxpacket.st24_data[_rxlen] = byte; + _rxlen++; + } else { + _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), sizeof(_rxpacket.length) + + sizeof(_rxpacket.st24_data) + sizeof(_rxpacket.type)) == _rxpacket.crc8) { + + ret = true; + + /* decode the actual packet */ + + switch (_rxpacket.type) { + + case ST24_PACKET_TYPE_CHANNELDATA12: + { + ChannelData12* d = (ChannelData12*)&_rxpacket; + + *rssi = d->rssi; + *rx_count = d->packet_count; + + for (unsigned i = 0; i < 1; i += 2) { + channels[i] = ((uint16_t)d->channel[i]) << 8; + channels[i] |= (0xF & d->channel[i+1]); + + channels[i+1] = ((uint16_t)(0xF & d->channel[i+1])) << 4; + channels[i+1] |= d->channel[i+2]; + } + } + break; + + case ST24_PACKET_TYPE_CHANNELDATA24: + { + ChannelData24* d = (ChannelData12*)&_rxpacket; + + *rssi = d->rssi; + *rx_count = d->packet_count; + + for (unsigned i = 0; i < 1; i += 2) { + channels[i] = ((uint16_t)d->channel[i]) << 8; + channels[i] |= (0xF & d->channel[i+1]); + + channels[i+1] = ((uint16_t)(0xF & d->channel[i+1])) << 4; + channels[i+1] |= d->channel[i+2]; + } + } + break; + + default: + ret = false; + break; + } + + } else { + /* decoding failed */ + _decode_state = ST24_DECODE_STATE_UNSYNCED; + } + break; + } + + return ret; +} diff --git a/src/lib/rc/st24.h b/src/lib/rc/st24.h index 3621e8506e..24dbf8e51a 100644 --- a/src/lib/rc/st24.h +++ b/src/lib/rc/st24.h @@ -39,22 +39,30 @@ * @author Lorenz Meier */ -#define ST24_DATA_LEN_MAX 64 +#pragma once -enum { +#include + +__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 -} ST24_PACKET_TYPE; +}; #pragma pack(push, 1) typedef struct { - uint8_t header1; ///< 0x55 for a valid packet - uint8_t header2; ///< 0x55 for a valid packet + 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 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 + uint8_t crc8; ///< crc8 checksum, calculated by st24_common_crc8 and including fields length, type and st24_data } ReceiverFcPacket; /** @@ -63,8 +71,8 @@ typedef struct { * This is incoming from the ST24 */ typedef struct { - uint16_t t; ///< packet counter or clock - uint8_t rssi ///< signal strength + 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; @@ -74,8 +82,8 @@ typedef struct { * */ typedef struct { - uint16_t t; ///< packet counter or clock - uint8_t rssi ///< signal strength + 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; @@ -114,17 +122,17 @@ typedef struct { * */ 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) + 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; /// #include #include +#include #include "px4io.h" @@ -51,11 +52,21 @@ #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(void); 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() +{ + /* get data from FD and attempt to parse with DSM and ST24 libs */ + + return false; +} + void controls_init(void) { @@ -65,7 +76,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"); @@ -175,6 +186,18 @@ controls_tick() { } perf_end(c_gather_ppm); + uint8_t st24_rssi, rx_count; + uint8_t st24_maxchans = 18; + bool st24_updated = st24_decode(0, &st24_rssi, &rx_count, r_raw_rc_values, st24_maxchans); + if (st24_updated) { + + rssi = st24_rssi; + + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + } + /* limit number of channels to allowable data size */ if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS) r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS; @@ -191,7 +214,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; diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk index 01869569f6..eb99e8a966 100644 --- a/src/modules/px4iofirmware/module.mk +++ b/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 From 72f6aaca96f7c7936f33cc3e7d7073ddb92d9bfa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 6 Oct 2014 07:53:18 +0200 Subject: [PATCH 05/21] Add ST24 test harness --- Tools/tests-host/.gitignore | 1 + Tools/tests-host/Makefile | 11 +++++-- Tools/tests-host/st24_test.cpp | 58 ++++++++++++++++++++++++++++++++++ 3 files changed, 68 insertions(+), 2 deletions(-) create mode 100644 Tools/tests-host/st24_test.cpp diff --git a/Tools/tests-host/.gitignore b/Tools/tests-host/.gitignore index 87b314c614..b06f998157 100644 --- a/Tools/tests-host/.gitignore +++ b/Tools/tests-host/.gitignore @@ -2,3 +2,4 @@ mixer_test sbus2_test autodeclination_test +st24_test diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile index f0737ef88c..5ee0c94326 100644 --- a/Tools/tests-host/Makefile +++ b/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 +all: mixer_test sbus2_test autodeclination_test st24_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 + AUTODECLINATION_FILES= ../../src/lib/geo/geo_mag_declination.c \ hrt.cpp \ autodeclination_test.cpp @@ -33,7 +37,10 @@ sbus2_test: $(SBUS2_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 \ No newline at end of file + rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test st24_test \ No newline at end of file diff --git a/Tools/tests-host/st24_test.cpp b/Tools/tests-host/st24_test.cpp new file mode 100644 index 0000000000..680791ca8a --- /dev/null +++ b/Tools/tests-host/st24_test.cpp @@ -0,0 +1,58 @@ + +#include +#include +#include +#include +#include +#include +#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++) { + (void)fscanf(fp, "%f,%x,,", &f, &x); + } + + 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"); + } + + warnx("%f: 0x%02x", (double)f, x); + + last_time = f; + + // Pipe the data into the parser + hrt_abstime now = hrt_absolute_time(); + + //if (partial_frame_count % 25 == 0) + //sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, &sbus_failsafe, &sbus_frame_drop, max_channels); + } + + if (ret == EOF) { + warnx("Test finished, reached end of file"); + } else { + warnx("Test aborted, errno: %d", ret); + } + +} From 7b0ac1db8583f3cd553e8dbcb4b338180ea5fb8b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 6 Oct 2014 07:53:32 +0200 Subject: [PATCH 06/21] Fix compile error in parser --- src/lib/rc/st24.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index 956a72bf35..2ae5765328 100644 --- a/src/lib/rc/st24.c +++ b/src/lib/rc/st24.c @@ -156,7 +156,7 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch case ST24_PACKET_TYPE_CHANNELDATA24: { - ChannelData24* d = (ChannelData12*)&_rxpacket; + ChannelData24* d = (ChannelData24*)&_rxpacket; *rssi = d->rssi; *rx_count = d->packet_count; From b4c188cf19097a26c49c4b0235b11ae939554aa0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 6 Oct 2014 13:41:29 +0200 Subject: [PATCH 07/21] Fix file location for st24 test --- Tools/tests-host/Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile index 5ee0c94326..f44f18be68 100644 --- a/Tools/tests-host/Makefile +++ b/Tools/tests-host/Makefile @@ -24,7 +24,7 @@ ST24_FILES=../../src/lib/rc/st24.c \ hrt.cpp \ st24_test.cpp -AUTODECLINATION_FILES= ../../src/lib/geo/geo_mag_declination.c \ +AUTODECLINATION_FILES= ../../src/lib/geo_lookup/geo_mag_declination.c \ hrt.cpp \ autodeclination_test.cpp From 34e75672bbf9a0e0e12d6a27abac5e1fcb3a0339 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 6 Oct 2014 19:20:02 +0200 Subject: [PATCH 08/21] SBUS2 fix --- Tools/tests-host/sbus2_test.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Tools/tests-host/sbus2_test.cpp b/Tools/tests-host/sbus2_test.cpp index d8fcb695d8..e2c18369c0 100644 --- a/Tools/tests-host/sbus2_test.cpp +++ b/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 From 966688d0921e397f7a19c8e9f3c9fd40b9561bba Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 6 Oct 2014 19:20:17 +0200 Subject: [PATCH 09/21] Fixed ST24 test --- Tools/tests-host/st24_test.cpp | 24 +++++++++++++++++++----- 1 file changed, 19 insertions(+), 5 deletions(-) diff --git a/Tools/tests-host/st24_test.cpp b/Tools/tests-host/st24_test.cpp index 680791ca8a..e1134bba63 100644 --- a/Tools/tests-host/st24_test.cpp +++ b/Tools/tests-host/st24_test.cpp @@ -4,7 +4,7 @@ #include #include #include -#include +#include #include "../../src/systemcmds/tests/tests.h" int main(int argc, char *argv[]) { @@ -28,7 +28,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); } float last_time = 0; @@ -38,15 +39,28 @@ int main(int argc, char *argv[]) { warnx("FRAME RESET\n\n"); } - warnx("%f: 0x%02x", (double)f, x); + uint8_t b = static_cast(x); last_time = f; // Pipe the data into the parser hrt_abstime now = hrt_absolute_time(); - //if (partial_frame_count % 25 == 0) - //sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, &sbus_failsafe, &sbus_frame_drop, max_channels); + uint8_t rssi; + uint8_t rx_count; + uint16_t channel_count; + int16_t channels[20]; + + + if (!st24_decode(b, &rssi, &rx_count, &channel_count, channels, sizeof(channels) / sizeof(channels[0]))) + { + //warnx("decoded: %u channels", (unsigned)channel_count); + for (unsigned i = 0; i < channel_count; i++) { + warnx("channel %u: %d", i, static_cast(channels[i])); + } + } + + //warnx("%f: 0x%02x >> RSSI: %u #: %u", (double)f, x, static_cast(rssi), static_cast(rx_count)); } if (ret == EOF) { From debff9e1796fe035975307dfc9d3e4ed081e3ded Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 6 Oct 2014 19:20:39 +0200 Subject: [PATCH 10/21] Updates to ST24 decoding library --- src/lib/rc/st24.c | 86 ++++++++++++++++++++++++++++++++++------------- src/lib/rc/st24.h | 2 +- 2 files changed, 64 insertions(+), 24 deletions(-) diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index 2ae5765328..f43f2de7ba 100644 --- a/src/lib/rc/st24.c +++ b/src/lib/rc/st24.c @@ -40,10 +40,11 @@ */ #include +#include #include "st24.h" enum ST24_DECODE_STATE { - ST24_DECODE_STATE_UNSYNCED, + ST24_DECODE_STATE_UNSYNCED = 0, ST24_DECODE_STATE_GOT_STX1, ST24_DECODE_STATE_GOT_STX2, ST24_DECODE_STATE_GOT_LEN, @@ -51,6 +52,24 @@ enum ST24_DECODE_STATE { 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 8000.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; @@ -83,7 +102,7 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len) } -uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channels, uint16_t max_chan_count) +uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channel_count, int16_t *channels, uint16_t max_chan_count) { bool ret = false; @@ -104,9 +123,14 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch break; case ST24_DECODE_STATE_GOT_STX2: - _rxpacket.length = byte; - _rxlen = 0; - _decode_state = ST24_DECODE_STATE_GOT_LEN; + /* 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: @@ -116,10 +140,9 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch break; case ST24_DECODE_STATE_GOT_TYPE: - if (_rxlen < (_rxpacket.length - 1)) { - _rxpacket.st24_data[_rxlen] = byte; - _rxlen++; - } else { + _rxpacket.st24_data[_rxlen - 1] = byte; + _rxlen++; + if (_rxlen == (_rxpacket.length - 1)) { _decode_state = ST24_DECODE_STATE_GOT_DATA; } break; @@ -128,8 +151,7 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch _rxpacket.crc8 = byte; _rxlen++; - if (st24_common_crc8((uint8_t*)&(_rxpacket.length), sizeof(_rxpacket.length) + - sizeof(_rxpacket.st24_data) + sizeof(_rxpacket.type)) == _rxpacket.crc8) { + if (st24_common_crc8((uint8_t*)&(_rxpacket.length), _rxlen) == _rxpacket.crc8) { ret = true; @@ -143,13 +165,18 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch *rssi = d->rssi; *rx_count = d->packet_count; + *channel_count = 12; - for (unsigned i = 0; i < 1; i += 2) { - channels[i] = ((uint16_t)d->channel[i]) << 8; - channels[i] |= (0xF & d->channel[i+1]); + for (unsigned i = 0; i < *channel_count; i += 2) { + channels[i] = ((uint16_t)d->channel[i]) << 4; + channels[i] |= ((uint16_t)(0xF0 & d->channel[i+1]) >> 4); - channels[i+1] = ((uint16_t)(0xF & d->channel[i+1])) << 4; - channels[i+1] |= d->channel[i+2]; + channels[i+1] = ((uint16_t)d->channel[i+2] << 4); + channels[i+1] |= ((uint16_t)(0x0F & d->channel[i+1])); + + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + // channels[i] = (uint16_t)(channels[i] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET; + // channels[i+1] = (uint16_t)(channels[i+1] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET; } } break; @@ -160,17 +187,29 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch *rssi = d->rssi; *rx_count = d->packet_count; + *channel_count = 24; - for (unsigned i = 0; i < 1; i += 2) { - channels[i] = ((uint16_t)d->channel[i]) << 8; - channels[i] |= (0xF & d->channel[i+1]); + for (unsigned i = 0; i < *channel_count; i += 2) { + channels[i] = ((uint16_t)d->channel[i]) << 4; + channels[i] |= ((uint16_t)(0xF0 & d->channel[i+1]) >> 4); - channels[i+1] = ((uint16_t)(0xF & d->channel[i+1])) << 4; - channels[i+1] |= d->channel[i+2]; + channels[i+1] = ((uint16_t)d->channel[i+2] << 4); + channels[i+1] |= ((uint16_t)(0x0F & d->channel[i+1])); + + // XXX apply scaling } } break; + case ST24_PACKET_TYPE_TRANSMITTERGPSDATA: + { + + ReceiverFcPacket* d = (ReceiverFcPacket*)&_rxpacket; + /* we silently ignore this data for now, as its not classic TX data */ + ret = false; + } + break; + default: ret = false; break; @@ -178,10 +217,11 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch } else { /* decoding failed */ - _decode_state = ST24_DECODE_STATE_UNSYNCED; + } + _decode_state = ST24_DECODE_STATE_UNSYNCED; break; } - return ret; + return !ret; } diff --git a/src/lib/rc/st24.h b/src/lib/rc/st24.h index 24dbf8e51a..45a835f77c 100644 --- a/src/lib/rc/st24.h +++ b/src/lib/rc/st24.h @@ -157,6 +157,6 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len); * @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), 3 for out of sync, 4 for checksum error */ -__EXPORT uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channels, uint16_t max_chan_count); +__EXPORT uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t* channel_count, int16_t *channels, uint16_t max_chan_count); __END_DECLS From 9c89499696baa15f709d125e46866e9f11ad5432 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 07:14:28 +0200 Subject: [PATCH 11/21] Fix up ST24 lib --- Tools/tests-host/st24_test.cpp | 9 +++++-- src/lib/rc/st24.c | 46 +++++++++++++++++++++------------- 2 files changed, 36 insertions(+), 19 deletions(-) diff --git a/Tools/tests-host/st24_test.cpp b/Tools/tests-host/st24_test.cpp index e1134bba63..f143542bd0 100644 --- a/Tools/tests-host/st24_test.cpp +++ b/Tools/tests-host/st24_test.cpp @@ -36,7 +36,7 @@ int main(int argc, char *argv[]) { while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) { if (((f - last_time) * 1000 * 1000) > 3000) { - warnx("FRAME RESET\n\n"); + //warnx("FRAME RESET\n\n"); } uint8_t b = static_cast(x); @@ -56,8 +56,13 @@ int main(int argc, char *argv[]) { { //warnx("decoded: %u channels", (unsigned)channel_count); for (unsigned i = 0; i < channel_count; i++) { - warnx("channel %u: %d", i, static_cast(channels[i])); + + int16_t val = channels[i]; + // if (i == 6) + warnx("channel %u: %d 0x%03X", i, static_cast(val), static_cast(val)); } + // unsigned chan = 1; + // warnx("channel %u: %d", chan, static_cast(channels[chan])); } //warnx("%f: 0x%02x >> RSSI: %u #: %u", (double)f, x, static_cast(rssi), static_cast(rx_count)); diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index f43f2de7ba..5a13649b5a 100644 --- a/src/lib/rc/st24.c +++ b/src/lib/rc/st24.c @@ -61,7 +61,7 @@ const char* decode_states[] = {"UNSYNCED", /* define range mapping here, -+100% -> 1000..2000 */ #define ST24_RANGE_MIN 0.0f -#define ST24_RANGE_MAX 8000.0f +#define ST24_RANGE_MAX 4096.0f #define ST24_TARGET_MIN 1000.0f #define ST24_TARGET_MAX 2000.0f @@ -161,42 +161,54 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch case ST24_PACKET_TYPE_CHANNELDATA12: { - ChannelData12* d = (ChannelData12*)&_rxpacket; + ChannelData12* d = (ChannelData12*)_rxpacket.st24_data; *rssi = d->rssi; *rx_count = d->packet_count; *channel_count = 12; - for (unsigned i = 0; i < *channel_count; i += 2) { - channels[i] = ((uint16_t)d->channel[i]) << 4; - channels[i] |= ((uint16_t)(0xF0 & d->channel[i+1]) >> 4); - - channels[i+1] = ((uint16_t)d->channel[i+2] << 4); - channels[i+1] |= ((uint16_t)(0x0F & d->channel[i+1])); + 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[i] = (uint16_t)(channels[i] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET; - // channels[i+1] = (uint16_t)(channels[i+1] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET; + 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; + ChannelData24* d = (ChannelData24*)&_rxpacket.st24_data; *rssi = d->rssi; *rx_count = d->packet_count; *channel_count = 24; - for (unsigned i = 0; i < *channel_count; i += 2) { - channels[i] = ((uint16_t)d->channel[i]) << 4; - channels[i] |= ((uint16_t)(0xF0 & d->channel[i+1]) >> 4); + unsigned stride_count = (*channel_count * 3) / 2; + unsigned chan_index = 0; - channels[i+1] = ((uint16_t)d->channel[i+2] << 4); - channels[i+1] |= ((uint16_t)(0x0F & d->channel[i+1])); + 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++; - // XXX apply scaling + 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; From cb0cbe479ae4bdaab6b6570857d240940fea713d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 08:05:32 +0200 Subject: [PATCH 12/21] Finalizing ST24 lib --- src/lib/rc/st24.c | 2 +- src/lib/rc/st24.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index 5a13649b5a..31a1e3fd3d 100644 --- a/src/lib/rc/st24.c +++ b/src/lib/rc/st24.c @@ -102,7 +102,7 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len) } -uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channel_count, int16_t *channels, uint16_t max_chan_count) +uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channel_count, uint16_t *channels, uint16_t max_chan_count) { bool ret = false; diff --git a/src/lib/rc/st24.h b/src/lib/rc/st24.h index 45a835f77c..6023a75076 100644 --- a/src/lib/rc/st24.h +++ b/src/lib/rc/st24.h @@ -157,6 +157,6 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len); * @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), 3 for out of sync, 4 for checksum error */ -__EXPORT uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t* channel_count, int16_t *channels, uint16_t max_chan_count); +__EXPORT uint8_t 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 From c2687a777444f68e7a7685ec628848b950f02571 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 08:05:45 +0200 Subject: [PATCH 13/21] ST24 integration in IO firmware --- src/modules/px4iofirmware/controls.c | 73 +++++++++++++++++----------- src/modules/px4iofirmware/dsm.c | 10 +++- src/modules/px4iofirmware/protocol.h | 1 + src/modules/px4iofirmware/px4io.h | 2 +- 4 files changed, 54 insertions(+), 32 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 3aa3d28ff3..1b7be692ad 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -52,7 +52,7 @@ #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(void); +static bool dsm_port_input(uint8_t *rssi); static perf_counter_t c_gather_dsm; static perf_counter_t c_gather_sbus; @@ -60,9 +60,49 @@ static perf_counter_t c_gather_ppm; static int _dsm_fd; -bool dsm_port_input() +bool dsm_port_input(uint8_t *rssi) { + perf_begin(c_gather_dsm); + uint16_t temp_count = r_raw_rc_count; + uint8_t n_bytes = 0; + uint8_t *bytes; + bool 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; + uint8_t st24_maxchans = 18; + + bool st24_updated = false; + + for (unsigned i = 0; i < n_bytes; i++) { + /* set updated flag if one complete packet was parsed */ + st24_updated |= st24_decode(bytes[i], &st24_rssi, &rx_count, + &st24_channel_count, r_raw_rc_values, st24_maxchans); + } + + 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 false; } @@ -127,20 +167,7 @@ controls_tick() { #endif perf_begin(c_gather_dsm); - uint16_t temp_count = r_raw_rc_count; - bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count); - 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); - - } + (void)dsm_port_input(&rssi); perf_end(c_gather_dsm); perf_begin(c_gather_sbus); @@ -186,18 +213,6 @@ controls_tick() { } perf_end(c_gather_ppm); - uint8_t st24_rssi, rx_count; - uint8_t st24_maxchans = 18; - bool st24_updated = st24_decode(0, &st24_rssi, &rx_count, r_raw_rc_values, st24_maxchans); - if (st24_updated) { - - rssi = st24_rssi; - - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; - r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); - r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); - } - /* limit number of channels to allowable data size */ if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS) r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS; @@ -402,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 { diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index d3f3658228..6e57c9ec75 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/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; diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 4739f6e400..e907d29593 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/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) /* SBUS 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 */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index b00a96717e..e32fcc72b6 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/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); From 3fc064882f37919b21e0176a071a7a9430688987 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 09:28:14 +0200 Subject: [PATCH 14/21] ST24 lib: formatting --- src/lib/rc/st24.c | 264 ++++++++++++++++++++++++---------------------- src/lib/rc/st24.h | 3 +- 2 files changed, 138 insertions(+), 129 deletions(-) diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index 31a1e3fd3d..bb42830db9 100644 --- a/src/lib/rc/st24.c +++ b/src/lib/rc/st24.c @@ -52,12 +52,13 @@ enum ST24_DECODE_STATE { ST24_DECODE_STATE_GOT_DATA }; -const char* decode_states[] = {"UNSYNCED", - "GOT_STX1", - "GOT_STX2", - "GOT_LEN", - "GOT_TYPE", - "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 @@ -102,137 +103,144 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len) } -uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channel_count, uint16_t *channels, uint16_t max_chan_count) +uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, uint16_t *channels, + uint16_t max_chan_count) { bool ret = false; switch (_decode_state) { - case ST24_DECODE_STATE_UNSYNCED: - if (byte == ST24_STX1) { - _decode_state = ST24_DECODE_STATE_GOT_STX1; - } - break; + case ST24_DECODE_STATE_UNSYNCED: + if (byte == ST24_STX1) { + _decode_state = ST24_DECODE_STATE_GOT_STX1; + } - 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; + 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_STX1: + if (byte == ST24_STX2) { + _decode_state = ST24_DECODE_STATE_GOT_STX2; - 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 = true; - - /* 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; - *channel_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; - *channel_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; - /* we silently ignore this data for now, as its not classic TX data */ - ret = false; - } - break; - - default: - ret = false; - break; - } - - } else { - /* decoding failed */ - - } + } else { _decode_state = ST24_DECODE_STATE_UNSYNCED; - break; + } + + 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 = true; + + /* 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; + *channel_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; + *channel_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 = false; + } + break; + + default: + ret = false; + break; + } + + } else { + /* decoding failed */ + + } + + _decode_state = ST24_DECODE_STATE_UNSYNCED; + break; } return !ret; diff --git a/src/lib/rc/st24.h b/src/lib/rc/st24.h index 6023a75076..1876eabc5a 100644 --- a/src/lib/rc/st24.h +++ b/src/lib/rc/st24.h @@ -157,6 +157,7 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len); * @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), 3 for out of sync, 4 for checksum error */ -__EXPORT uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t* channel_count, uint16_t *channels, uint16_t max_chan_count); +__EXPORT uint8_t 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 From 35caa8bd996392924ab1d92eb46c3d40e0a91b28 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 09:28:36 +0200 Subject: [PATCH 15/21] PX4IO Controls: compile fixes --- src/modules/px4iofirmware/controls.c | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 1b7be692ad..8670e30c2f 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -52,7 +52,7 @@ #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); +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; @@ -60,14 +60,14 @@ static perf_counter_t c_gather_ppm; static int _dsm_fd; -bool dsm_port_input(uint8_t *rssi) +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; - bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes); - if (dsm_updated) { + *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) @@ -86,15 +86,15 @@ bool dsm_port_input(uint8_t *rssi) uint16_t st24_channel_count; uint8_t st24_maxchans = 18; - bool st24_updated = false; + *st24_updated = false; for (unsigned i = 0; i < n_bytes; i++) { /* set updated flag if one complete packet was parsed */ - st24_updated |= st24_decode(bytes[i], &st24_rssi, &rx_count, + *st24_updated |= st24_decode(bytes[i], &st24_rssi, &rx_count, &st24_channel_count, r_raw_rc_values, st24_maxchans); } - if (st24_updated) { + if (*st24_updated) { *rssi = st24_rssi; r_raw_rc_count = st24_channel_count; @@ -104,7 +104,7 @@ bool dsm_port_input(uint8_t *rssi) r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } - return false; + return (*dsm_updated | *st24_updated); } void @@ -167,7 +167,8 @@ controls_tick() { #endif perf_begin(c_gather_dsm); - (void)dsm_port_input(&rssi); + bool dsm_updated, st24_updated; + (void)dsm_port_input(&rssi, &dsm_updated, &st24_updated); perf_end(c_gather_dsm); perf_begin(c_gather_sbus); From c7e8570f8355cb96b4daf4f1d3283807ae0cbdc5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 09:36:24 +0200 Subject: [PATCH 16/21] PX4IO firmware: Fix comment --- src/modules/px4iofirmware/protocol.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index e907d29593..89a470b44b 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -112,7 +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) /* SBUS input is valid */ +#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 */ From 7921184e4081de18f92874df03a082c04510fa24 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 09:36:42 +0200 Subject: [PATCH 17/21] IO input driver: Output ST24 as receiver type / status --- src/drivers/drv_rc_input.h | 3 ++- src/drivers/px4io/px4io.cpp | 10 +++++++++- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index 47fa8fa59a..b249c2a093 100644 --- a/src/drivers/drv_rc_input.h +++ b/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 }; /** diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index fbb5d4f2e6..d212be7661 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/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; } From 7e5910bdbf10c5c00f438d17d1b0bcd00d6c020d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 09:54:20 +0200 Subject: [PATCH 18/21] Formatted ST24 test code --- Tools/tests-host/st24_test.cpp | 31 +++++++++++++++---------------- 1 file changed, 15 insertions(+), 16 deletions(-) diff --git a/Tools/tests-host/st24_test.cpp b/Tools/tests-host/st24_test.cpp index f143542bd0..25a9355e2e 100644 --- a/Tools/tests-host/st24_test.cpp +++ b/Tools/tests-host/st24_test.cpp @@ -7,20 +7,23 @@ #include #include "../../src/systemcmds/tests/tests.h" -int main(int argc, char *argv[]) { +int main(int argc, char *argv[]) +{ warnx("ST24 test started"); - if (argc < 2) + 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) + fp = fopen(argv[1], "rt"); + + if (!fp) { errx(1, "failed opening file"); + } float f; unsigned x; @@ -36,7 +39,7 @@ int main(int argc, char *argv[]) { while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) { if (((f - last_time) * 1000 * 1000) > 3000) { - //warnx("FRAME RESET\n\n"); + // warnx("FRAME RESET\n\n"); } uint8_t b = static_cast(x); @@ -49,27 +52,23 @@ int main(int argc, char *argv[]) { uint8_t rssi; uint8_t rx_count; uint16_t channel_count; - int16_t channels[20]; + uint16_t channels[20]; - if (!st24_decode(b, &rssi, &rx_count, &channel_count, channels, sizeof(channels) / sizeof(channels[0]))) - { - //warnx("decoded: %u channels", (unsigned)channel_count); + 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]; - // if (i == 6) - warnx("channel %u: %d 0x%03X", i, static_cast(val), static_cast(val)); + warnx("channel %u: %d 0x%03X", i, static_cast(val), static_cast(val)); } - // unsigned chan = 1; - // warnx("channel %u: %d", chan, static_cast(channels[chan])); } - - //warnx("%f: 0x%02x >> RSSI: %u #: %u", (double)f, x, static_cast(rssi), static_cast(rx_count)); } if (ret == EOF) { warnx("Test finished, reached end of file"); + } else { warnx("Test aborted, errno: %d", ret); } From 6436db1a99888aff3802008c91f8a6c151f7da9c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 22:24:32 +0200 Subject: [PATCH 19/21] Fix parser return type handling --- src/modules/px4iofirmware/controls.c | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 8670e30c2f..622f20c7d5 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -83,15 +83,14 @@ bool dsm_port_input(uint8_t *rssi, bool *dsm_updated, bool *st24_updated) /* get data from FD and attempt to parse with DSM and ST24 libs */ uint8_t st24_rssi, rx_count; - uint16_t st24_channel_count; - uint8_t st24_maxchans = 18; + 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 |= st24_decode(bytes[i], &st24_rssi, &rx_count, - &st24_channel_count, r_raw_rc_values, st24_maxchans); + *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) { From 726b10651aa38f1b542e6f3845da9aaf58af72a2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 22:25:03 +0200 Subject: [PATCH 20/21] ST24: Fix parser return values, update docs --- src/lib/rc/st24.c | 16 +++++++++------- src/lib/rc/st24.h | 4 ++-- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index bb42830db9..2b2178f97d 100644 --- a/src/lib/rc/st24.c +++ b/src/lib/rc/st24.c @@ -103,16 +103,18 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len) } -uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, uint16_t *channels, +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) { - bool ret = false; + 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; @@ -163,7 +165,7 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *ch if (st24_common_crc8((uint8_t *) & (_rxpacket.length), _rxlen) == _rxpacket.crc8) { - ret = true; + ret = 0; /* decode the actual packet */ @@ -225,23 +227,23 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *ch // ReceiverFcPacket* d = (ReceiverFcPacket*)&_rxpacket.st24_data; /* we silently ignore this data for now, as it is unused */ - ret = false; + ret = 2; } break; default: - ret = false; + ret = 2; break; } } else { /* decoding failed */ - + ret = 4; } _decode_state = ST24_DECODE_STATE_UNSYNCED; break; } - return !ret; + return ret; } diff --git a/src/lib/rc/st24.h b/src/lib/rc/st24.h index 1876eabc5a..454234601d 100644 --- a/src/lib/rc/st24.h +++ b/src/lib/rc/st24.h @@ -155,9 +155,9 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len); * @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), 3 for out of sync, 4 for checksum error + * @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 uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, +__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 From 8c6c08dcb5ce87e613cbf867571f219a60e1b813 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 22:46:07 +0200 Subject: [PATCH 21/21] Limit channel count effectively --- src/lib/rc/st24.c | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index 2b2178f97d..e8a791b8f6 100644 --- a/src/lib/rc/st24.c +++ b/src/lib/rc/st24.c @@ -176,7 +176,9 @@ int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe *rssi = d->rssi; *rx_count = d->packet_count; - *channel_count = 12; + + /* 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; @@ -202,7 +204,9 @@ int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe *rssi = d->rssi; *rx_count = d->packet_count; - *channel_count = 24; + + /* 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;