aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/drivers/drv_rc_input.h3
-rw-r--r--src/drivers/px4io/px4io.cpp10
-rw-r--r--src/lib/rc/module.mk40
-rw-r--r--src/lib/rc/st24.c253
-rw-r--r--src/lib/rc/st24.h163
-rw-r--r--src/modules/px4iofirmware/controls.c72
-rw-r--r--src/modules/px4iofirmware/dsm.c10
-rw-r--r--src/modules/px4iofirmware/module.mk3
-rw-r--r--src/modules/px4iofirmware/protocol.h1
-rw-r--r--src/modules/px4iofirmware/px4io.h2
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);
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-08 02:18:38 +0000