From af4d5c0a3c985007958e512db977832bc93b2553 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Jul 2014 13:14:39 +0200 Subject: Added initial bits for ST24 decoding --- src/lib/rc/st24.h | 140 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 140 insertions(+) create mode 100644 src/lib/rc/st24.h (limited to 'src/lib/rc/st24.h') diff --git a/src/lib/rc/st24.h b/src/lib/rc/st24.h new file mode 100644 index 000000000..1c92bad43 --- /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: 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 (limited to 'src/lib/rc/st24.h') diff --git a/src/lib/rc/module.mk b/src/lib/rc/module.mk new file mode 100644 index 000000000..e089c6965 --- /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 8287a0453..bae28c85e 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 1c92bad43..3621e8506 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); -- cgit v1.2.3 From 69109b622796dacd6f78ebafa92b10379ba0d7b4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 1 Sep 2014 00:45:41 +0200 Subject: Compile and link ST24 parser in IO firmware --- src/lib/rc/st24.c | 120 ++++++++++++++++++++++++++++++++++- src/lib/rc/st24.h | 46 ++++++++------ src/modules/px4iofirmware/controls.c | 27 +++++++- src/modules/px4iofirmware/module.mk | 3 +- 4 files changed, 173 insertions(+), 23 deletions(-) (limited to 'src/lib/rc/st24.h') diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index addbcd899..956a72bf3 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) +{ + + 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) { -} \ No newline at end of file + 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 3621e8506..24dbf8e51 100644 --- a/src/lib/rc/st24.h +++ b/src/lib/rc/st24.h @@ -39,22 +39,30 @@ * @author Lorenz Meier */ +#pragma once + +#include + +__BEGIN_DECLS + #define ST24_DATA_LEN_MAX 64 +#define ST24_STX1 0x55 +#define ST24_STX2 0x55 -enum { +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 01869569f..eb99e8a96 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 -- cgit v1.2.3 From debff9e1796fe035975307dfc9d3e4ed081e3ded Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 6 Oct 2014 19:20:39 +0200 Subject: Updates to ST24 decoding library --- src/lib/rc/st24.c | 86 ++++++++++++++++++++++++++++++++++++++++--------------- src/lib/rc/st24.h | 2 +- 2 files changed, 64 insertions(+), 24 deletions(-) (limited to 'src/lib/rc/st24.h') diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index 2ae576532..f43f2de7b 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 < *channel_count; i += 2) { + channels[i] = ((uint16_t)d->channel[i]) << 4; + channels[i] |= ((uint16_t)(0xF0 & d->channel[i+1]) >> 4); - 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)d->channel[i+2] << 4); + channels[i+1] |= ((uint16_t)(0x0F & d->channel[i+1])); - channels[i+1] = ((uint16_t)(0xF & d->channel[i+1])) << 4; - channels[i+1] |= d->channel[i+2]; + // 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 24dbf8e51..45a835f77 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 -- cgit v1.2.3 From cb0cbe479ae4bdaab6b6570857d240940fea713d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 08:05:32 +0200 Subject: Finalizing ST24 lib --- src/lib/rc/st24.c | 2 +- src/lib/rc/st24.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) (limited to 'src/lib/rc/st24.h') diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index 5a13649b5..31a1e3fd3 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 45a835f77..6023a7507 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 -- cgit v1.2.3 From 3fc064882f37919b21e0176a071a7a9430688987 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 09:28:14 +0200 Subject: ST24 lib: formatting --- src/lib/rc/st24.c | 264 ++++++++++++++++++++++++++++-------------------------- src/lib/rc/st24.h | 3 +- 2 files changed, 138 insertions(+), 129 deletions(-) (limited to 'src/lib/rc/st24.h') diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index 31a1e3fd3..bb42830db 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; - - 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; - /* we silently ignore this data for now, as its not classic TX data */ - ret = false; - } - break; - - default: - ret = false; - break; + 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 = 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; - } else { - /* decoding failed */ - + 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; } - _decode_state = ST24_DECODE_STATE_UNSYNCED; - 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 6023a7507..1876eabc5 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 -- cgit v1.2.3 From 726b10651aa38f1b542e6f3845da9aaf58af72a2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 22:25:03 +0200 Subject: 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(-) (limited to 'src/lib/rc/st24.h') diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index bb42830db..2b2178f97 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 1876eabc5..454234601 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 -- cgit v1.2.3