diff options
author | Lorenz Meier <lm@qgroundcontrol.org> | 2014-10-07 22:49:41 +0200 |
---|---|---|
committer | Lorenz Meier <lm@qgroundcontrol.org> | 2014-10-07 22:49:41 +0200 |
commit | a06a278a18e9344d06dc3fa3cdd534a793996a08 (patch) | |
tree | 05bb8ef2f9443110c0dfa0e3915ecc391dadf5c6 /src | |
parent | 91b4d85b46f1d4514fe1ce45553753eb011918c1 (diff) | |
parent | 4a8d20e2e562b69f3d2272d3e40db2b7a3e5152a (diff) | |
download | px4-firmware-a06a278a18e9344d06dc3fa3cdd534a793996a08.tar.gz px4-firmware-a06a278a18e9344d06dc3fa3cdd534a793996a08.tar.bz2 px4-firmware-a06a278a18e9344d06dc3fa3cdd534a793996a08.zip |
Merge pull request #1339 from PX4/st24
Yuneec ST24 protocol decoding
Diffstat (limited to 'src')
-rw-r--r-- | src/drivers/drv_rc_input.h | 3 | ||||
-rw-r--r-- | src/drivers/px4io/px4io.cpp | 10 | ||||
-rw-r--r-- | src/lib/rc/module.mk | 40 | ||||
-rw-r--r-- | src/lib/rc/st24.c | 253 | ||||
-rw-r--r-- | src/lib/rc/st24.h | 163 | ||||
-rw-r--r-- | src/modules/px4iofirmware/controls.c | 72 | ||||
-rw-r--r-- | src/modules/px4iofirmware/dsm.c | 10 | ||||
-rw-r--r-- | src/modules/px4iofirmware/module.mk | 3 | ||||
-rw-r--r-- | src/modules/px4iofirmware/protocol.h | 1 | ||||
-rw-r--r-- | src/modules/px4iofirmware/px4io.h | 2 |
10 files changed, 534 insertions, 23 deletions
diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index 47fa8fa59..b249c2a09 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 fbb5d4f2e..d212be766 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; } 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 new file mode 100644 index 000000000..e8a791b8f --- /dev/null +++ b/src/lib/rc/st24.c @@ -0,0 +1,253 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file st24.h + * + * RC protocol implementation for Yuneec ST24 transmitter. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#include <stdbool.h> +#include <stdio.h> +#include "st24.h" + +enum ST24_DECODE_STATE { + ST24_DECODE_STATE_UNSYNCED = 0, + ST24_DECODE_STATE_GOT_STX1, + ST24_DECODE_STATE_GOT_STX2, + ST24_DECODE_STATE_GOT_LEN, + ST24_DECODE_STATE_GOT_TYPE, + ST24_DECODE_STATE_GOT_DATA +}; + +const char *decode_states[] = {"UNSYNCED", + "GOT_STX1", + "GOT_STX2", + "GOT_LEN", + "GOT_TYPE", + "GOT_DATA" + }; + +/* define range mapping here, -+100% -> 1000..2000 */ +#define ST24_RANGE_MIN 0.0f +#define ST24_RANGE_MAX 4096.0f + +#define ST24_TARGET_MIN 1000.0f +#define ST24_TARGET_MAX 2000.0f + +/* pre-calculate the floating point stuff as far as possible at compile time */ +#define ST24_SCALE_FACTOR ((ST24_TARGET_MAX - ST24_TARGET_MIN) / (ST24_RANGE_MAX - ST24_RANGE_MIN)) +#define ST24_SCALE_OFFSET (int)(ST24_TARGET_MIN - (ST24_SCALE_FACTOR * ST24_RANGE_MIN + 0.5f)) + +static enum ST24_DECODE_STATE _decode_state = ST24_DECODE_STATE_UNSYNCED; +static unsigned _rxlen; + +static ReceiverFcPacket _rxpacket; + +uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len) +{ + uint8_t i, crc ; + crc = 0; + + while (len--) { + for (i = 0x80; i != 0; i >>= 1) { + if ((crc & 0x80) != 0) { + crc <<= 1; + crc ^= 0x07; + + } else { + crc <<= 1; + } + + if ((*ptr & i) != 0) { + crc ^= 0x07; + } + } + + ptr++; + } + + return (crc); +} + + +int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, uint16_t *channels, + uint16_t max_chan_count) +{ + + int ret = 1; + + switch (_decode_state) { + case ST24_DECODE_STATE_UNSYNCED: + if (byte == ST24_STX1) { + _decode_state = ST24_DECODE_STATE_GOT_STX1; + } else { + ret = 3; + } + + break; + + case ST24_DECODE_STATE_GOT_STX1: + if (byte == ST24_STX2) { + _decode_state = ST24_DECODE_STATE_GOT_STX2; + + } else { + _decode_state = ST24_DECODE_STATE_UNSYNCED; + } + + break; + + case ST24_DECODE_STATE_GOT_STX2: + + /* ensure no data overflow failure or hack is possible */ + if ((unsigned)byte <= sizeof(_rxpacket.length) + sizeof(_rxpacket.type) + sizeof(_rxpacket.st24_data)) { + _rxpacket.length = byte; + _rxlen = 0; + _decode_state = ST24_DECODE_STATE_GOT_LEN; + + } else { + _decode_state = ST24_DECODE_STATE_UNSYNCED; + } + + break; + + case ST24_DECODE_STATE_GOT_LEN: + _rxpacket.type = byte; + _rxlen++; + _decode_state = ST24_DECODE_STATE_GOT_TYPE; + break; + + case ST24_DECODE_STATE_GOT_TYPE: + _rxpacket.st24_data[_rxlen - 1] = byte; + _rxlen++; + + if (_rxlen == (_rxpacket.length - 1)) { + _decode_state = ST24_DECODE_STATE_GOT_DATA; + } + + break; + + case ST24_DECODE_STATE_GOT_DATA: + _rxpacket.crc8 = byte; + _rxlen++; + + if (st24_common_crc8((uint8_t *) & (_rxpacket.length), _rxlen) == _rxpacket.crc8) { + + ret = 0; + + /* decode the actual packet */ + + switch (_rxpacket.type) { + + case ST24_PACKET_TYPE_CHANNELDATA12: { + ChannelData12 *d = (ChannelData12 *)_rxpacket.st24_data; + + *rssi = d->rssi; + *rx_count = d->packet_count; + + /* this can lead to rounding of the strides */ + *channel_count = (max_chan_count < 12) ? max_chan_count : 12; + + unsigned stride_count = (*channel_count * 3) / 2; + unsigned chan_index = 0; + + for (unsigned i = 0; i < stride_count; i += 3) { + channels[chan_index] = ((uint16_t)d->channel[i] << 4); + channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4); + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET; + chan_index++; + + channels[chan_index] = ((uint16_t)d->channel[i + 2]); + channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8); + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET; + chan_index++; + } + } + break; + + case ST24_PACKET_TYPE_CHANNELDATA24: { + ChannelData24 *d = (ChannelData24 *)&_rxpacket.st24_data; + + *rssi = d->rssi; + *rx_count = d->packet_count; + + /* this can lead to rounding of the strides */ + *channel_count = (max_chan_count < 24) ? max_chan_count : 24; + + unsigned stride_count = (*channel_count * 3) / 2; + unsigned chan_index = 0; + + for (unsigned i = 0; i < stride_count; i += 3) { + channels[chan_index] = ((uint16_t)d->channel[i] << 4); + channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4); + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET; + chan_index++; + + channels[chan_index] = ((uint16_t)d->channel[i + 2]); + channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8); + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET; + chan_index++; + } + } + break; + + case ST24_PACKET_TYPE_TRANSMITTERGPSDATA: { + + // ReceiverFcPacket* d = (ReceiverFcPacket*)&_rxpacket.st24_data; + /* we silently ignore this data for now, as it is unused */ + ret = 2; + } + break; + + default: + ret = 2; + break; + } + + } else { + /* decoding failed */ + ret = 4; + } + + _decode_state = ST24_DECODE_STATE_UNSYNCED; + break; + } + + return ret; +} diff --git a/src/lib/rc/st24.h b/src/lib/rc/st24.h new file mode 100644 index 000000000..454234601 --- /dev/null +++ b/src/lib/rc/st24.h @@ -0,0 +1,163 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file st24.h + * + * RC protocol definition for Yuneec ST24 transmitter + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#pragma once + +#include <stdint.h> + +__BEGIN_DECLS + +#define ST24_DATA_LEN_MAX 64 +#define ST24_STX1 0x55 +#define ST24_STX2 0x55 + +enum ST24_PACKET_TYPE { + ST24_PACKET_TYPE_CHANNELDATA12 = 0, + ST24_PACKET_TYPE_CHANNELDATA24, + ST24_PACKET_TYPE_TRANSMITTERGPSDATA +}; + +#pragma pack(push, 1) +typedef struct { + uint8_t header1; ///< 0x55 for a valid packet + uint8_t header2; ///< 0x55 for a valid packet + uint8_t length; ///< length includes type, data, and crc = sizeof(type)+sizeof(data[payload_len])+sizeof(crc8) + uint8_t type; ///< from enum ST24_PACKET_TYPE + uint8_t st24_data[ST24_DATA_LEN_MAX]; + uint8_t crc8; ///< crc8 checksum, calculated by st24_common_crc8 and including fields length, type and st24_data +} ReceiverFcPacket; + +/** + * RC Channel data (12 channels). + * + * This is incoming from the ST24 + */ +typedef struct { + uint16_t t; ///< packet counter or clock + uint8_t rssi; ///< signal strength + uint8_t packet_count; ///< Number of UART packets sent since reception of last RF frame (this tells something about age / rate) + uint8_t channel[18]; ///< channel data, 12 channels (12 bit numbers) +} ChannelData12; + +/** + * RC Channel data (12 channels). + * + */ +typedef struct { + uint16_t t; ///< packet counter or clock + uint8_t rssi; ///< signal strength + uint8_t packet_count; ///< Number of UART packets sent since reception of last RF frame (this tells something about age / rate) + uint8_t channel[36]; ///< channel data, 24 channels (12 bit numbers) +} ChannelData24; + +/** + * Telemetry packet + * + * This is outgoing to the ST24 + * + * imuStatus: + * 8 bit total + * bits 0-2 for status + * - value 0 is FAILED + * - value 1 is INITIALIZING + * - value 2 is RUNNING + * - values 3 through 7 are reserved + * bits 3-7 are status for sensors (0 or 1) + * - mpu6050 + * - accelerometer + * - primary gyro x + * - primary gyro y + * - primary gyro z + * + * pressCompassStatus + * 8 bit total + * bits 0-3 for compass status + * - value 0 is FAILED + * - value 1 is INITIALIZING + * - value 2 is RUNNING + * - value 3 - 15 are reserved + * bits 4-7 for pressure status + * - value 0 is FAILED + * - value 1 is INITIALIZING + * - value 2 is RUNNING + * - value 3 - 15 are reserved + * + */ +typedef struct { + uint16_t t; ///< packet counter or clock + int32_t lat; ///< lattitude (degrees) +/- 90 deg + int32_t lon; ///< longitude (degrees) +/- 180 deg + int32_t alt; ///< 0.01m resolution, altitude (meters) + int16_t vx, vy, vz; ///< velocity 0.01m res, +/-320.00 North-East- Down + uint8_t nsat; ///<number of satellites + uint8_t voltage; ///< 25.4V voltage = 5 + 255*0.1 = 30.5V, min=5V + uint8_t current; ///< 0.5A resolution + int16_t roll, pitch, yaw; ///< 0.01 degree resolution + uint8_t motorStatus; ///< 1 bit per motor for status 1=good, 0= fail + uint8_t imuStatus; ///< inertial measurement unit status + uint8_t pressCompassStatus; ///< baro / compass status +} TelemetryData; + +#pragma pack(pop) + +/** + * CRC8 implementation for ST24 protocol + * + * @param prt Pointer to the data to CRC + * @param len number of bytes to accumulate in the checksum + * @return the checksum of these bytes over len + */ +uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len); + +/** + * Decoder for ST24 protocol + * + * @param byte current char to read + * @param rssi pointer to a byte where the RSSI value is written back to + * @param rx_count pointer to a byte where the receive count of packets signce last wireless frame is written back to + * @param channels pointer to a datastructure of size max_chan_count where channel values (12 bit) are written back to + * @param max_chan_count maximum channels to decode - if more channels are decoded, the last n are skipped and success (0) is returned + * @return 0 for success (a decoded packet), 1 for no packet yet (accumulating), 2 for unknown packet, 3 for out of sync, 4 for checksum error + */ +__EXPORT int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, + uint16_t *channels, uint16_t max_chan_count); + +__END_DECLS diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 185cb20dd..622f20c7d 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -43,6 +43,7 @@ #include <drivers/drv_hrt.h> #include <systemlib/perf_counter.h> #include <systemlib/ppm_decode.h> +#include <rc/st24.h> #include "px4io.h" @@ -51,11 +52,60 @@ #define RC_CHANNEL_LOW_THRESH -8000 /* 10% threshold */ static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len); +static bool dsm_port_input(uint8_t *rssi, bool *dsm_updated, bool *st24_updated); static perf_counter_t c_gather_dsm; static perf_counter_t c_gather_sbus; static perf_counter_t c_gather_ppm; +static int _dsm_fd; + +bool dsm_port_input(uint8_t *rssi, bool *dsm_updated, bool *st24_updated) +{ + perf_begin(c_gather_dsm); + uint16_t temp_count = r_raw_rc_count; + uint8_t n_bytes = 0; + uint8_t *bytes; + *dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes); + if (*dsm_updated) { + r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; + r_raw_rc_count = temp_count & 0x7fff; + if (temp_count & 0x8000) + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11; + else + r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11; + + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + + } + perf_end(c_gather_dsm); + + /* get data from FD and attempt to parse with DSM and ST24 libs */ + uint8_t st24_rssi, rx_count; + uint16_t st24_channel_count = 0; + + *st24_updated = false; + + for (unsigned i = 0; i < n_bytes; i++) { + /* set updated flag if one complete packet was parsed */ + *st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &rx_count, + &st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS)); + } + + if (*st24_updated) { + + *rssi = st24_rssi; + r_raw_rc_count = st24_channel_count; + + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24; + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + } + + return (*dsm_updated | *st24_updated); +} + void controls_init(void) { @@ -65,7 +115,7 @@ controls_init(void) system_state.rc_channels_timestamp_valid = 0; /* DSM input (USART1) */ - dsm_init("/dev/ttyS0"); + _dsm_fd = dsm_init("/dev/ttyS0"); /* S.bus input (USART3) */ sbus_init("/dev/ttyS2"); @@ -116,20 +166,8 @@ controls_tick() { #endif perf_begin(c_gather_dsm); - uint16_t temp_count = r_raw_rc_count; - bool dsm_updated = 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); - - } + bool dsm_updated, st24_updated; + (void)dsm_port_input(&rssi, &dsm_updated, &st24_updated); perf_end(c_gather_dsm); perf_begin(c_gather_sbus); @@ -191,7 +229,7 @@ controls_tick() { /* * If we received a new frame from any of the RC sources, process it. */ - if (dsm_updated || sbus_updated || ppm_updated) { + if (dsm_updated || sbus_updated || ppm_updated || st24_updated) { /* record a bitmask of channels assigned */ unsigned assigned_channels = 0; @@ -379,7 +417,7 @@ controls_tick() { r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; /* mix new RC input control values to servos */ - if (dsm_updated || sbus_updated || ppm_updated) + if (dsm_updated || sbus_updated || ppm_updated || st24_updated) mixer_tick(); } else { diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index d3f365822..6e57c9ec7 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/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 diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 4739f6e40..89a470b44 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) /* 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 */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index b00a96717..e32fcc72b 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); |