From ff3a014971f83f15f4884e584a2f58ee979f23ee Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 7 Nov 2012 02:47:01 -0800 Subject: Another take on Spektrum/DSM frame decoding, based on more careful examination of the relevant docs. --- apps/px4io/comms.c | 119 ++++++++++++++++++++++++++---- apps/px4io/dsm.c | 184 ++++++++++++++++++++++++++++++++++++++++++++++ apps/px4io/mixer.c | 199 ++++---------------------------------------------- apps/px4io/protocol.h | 9 +-- apps/px4io/px4io.c | 2 +- apps/px4io/px4io.h | 9 ++- apps/px4io/sbus.c | 58 +++++++++++++++ 7 files changed, 372 insertions(+), 208 deletions(-) create mode 100644 apps/px4io/dsm.c create mode 100644 apps/px4io/sbus.c (limited to 'apps/px4io') diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index 1edff25b1..3964d7503 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -44,6 +44,7 @@ #include #include #include +#include #include #include @@ -60,26 +61,126 @@ static int fmu_fd; static hx_stream_t stream; - +static int rx_fd; static struct px4io_report report; static void comms_handle_frame(void *arg, const void *buffer, size_t length); +static struct pollfd pollfds[2]; +static int pollcount; + void comms_init(void) { + /* initialise the FMU interface */ fmu_fd = open("/dev/ttyS1", O_RDWR | O_NONBLOCK); if (fmu_fd < 0) lib_lowprintf("COMMS: fmu open failed %d\n", errno); - stream = hx_stream_init(fmu_fd, comms_handle_frame, NULL); + pollfds[0].fd = fmu_fd; + pollfds[0].events = POLLIN; + pollcount = 1; + /* default state in the report to FMU */ report.i2f_magic = I2F_MAGIC; + +} + +static void +serial_rx_init(unsigned serial_mode) +{ + if (serial_mode == system_state.serial_rx_mode) + return; + system_state.serial_rx_mode = serial_mode; + + if (serial_mode == RX_MODE_PPM_ONLY) { + if (rx_fd != -1) { + pollcount = 1; + close(rx_fd); + rx_fd = -1; + } + return; + } + + /* open the serial port used for DSM and S.bus communication */ + rx_fd = open("/dev/ttyS0", O_RDONLY | O_NONBLOCK); + pollfds[1].fd = rx_fd; + pollfds[1].events = POLLIN; + pollcount = 2; + + struct termios t; + tcgetattr(rx_fd, &t); + + switch (serial_mode) { + case RX_MODE_DSM_10BIT: + case RX_MODE_DSM_11BIT: + + /* 115200, no parity, one stop bit */ + cfsetspeed(&t, 115200); + t.c_cflag &= ~(CSTOPB | PARENB); + + dsm_init(serial_mode); + break; + + case RX_MODE_FUTABA_SBUS: + /* 100000, even parity, two stop bits */ + cfsetspeed(&t, 100000); + t.c_cflag |= (CSTOPB | PARENB); + + sbus_init(serial_mode); + break; + + default: + break; + } + + tcsetattr(rx_fd, TCSANOW, &t); } void comms_check(void) { + /* + * Check for serial data + */ + int ret = poll(pollfds, pollcount, 0); + + if (ret > 0) { + /* + * Pull bytes from FMU and feed them to the HX engine. + * Limit the number of bytes we actually process on any one iteration. + */ + if (pollfds[0].revents & POLLIN) { + char buf[32]; + ssize_t count = read(fmu_fd, buf, sizeof(buf)); + for (int i = 0; i < count; i++) + hx_stream_rx(stream, buf[i]); + } + + /* + * Pull bytes from the serial RX port and feed them to the decoder + * if we care about serial RX data. + */ + if ((pollcount > 1) && (pollfds[1].revents & POLLIN)) { + switch (system_state.serial_rx_mode) { + case RX_MODE_DSM_10BIT: + case RX_MODE_DSM_11BIT: + dsm_input(rx_fd); + break; + + case RX_MODE_FUTABA_SBUS: + sbus_input(rx_fd); + break; + + default: + break; + } + } + } + + /* + * Decide if it's time to send an update to the FMU. + */ static hrt_abstime last_report_time; hrt_abstime now, delta; @@ -87,7 +188,7 @@ comms_check(void) now = hrt_absolute_time(); delta = now - last_report_time; if ((delta > FMU_MIN_REPORT_INTERVAL) && - (system_state.fmu_report_due || (delta > FMU_MAX_REPORT_INTERVAL))) { + (system_state.fmu_report_due || (delta > FMU_MAX_REPORT_INTERVAL))) { system_state.fmu_report_due = false; last_report_time = now; @@ -101,15 +202,6 @@ comms_check(void) /* and send it */ hx_stream_send(stream, &report, sizeof(report)); } - - /* - * Check for bytes and feed them to the RX engine. - * Limit the number of bytes we actually process on any one iteration. - */ - char buf[32]; - ssize_t count = read(fmu_fd, buf, sizeof(buf)); - for (int i = 0; i < count; i++) - hx_stream_rx(stream, buf[i]); } int frame_rx; @@ -127,8 +219,7 @@ comms_handle_config(const void *buffer, size_t length) frame_rx++; - mixer_set_serial_mode(cfg->serial_rx_mode); - + serial_rx_init(cfg->serial_rx_mode); } static void diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c new file mode 100644 index 000000000..79b6301c7 --- /dev/null +++ b/apps/px4io/dsm.c @@ -0,0 +1,184 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 dsm.c + * + * Serial protocol decoder for the Spektrum DSM* family of protocols. + * + * Decodes into the global PPM buffer and updates accordingly. + */ + +#include + +#include +#include +#include + +#include + +#include + +#include "px4io.h" +#include "protocol.h" + +#define DSM_FRAME_SIZE 16 +#define DSM_FRAME_CHANNELS 7 + +static hrt_abstime last_frame_time; + +static uint8_t frame[DSM_FRAME_SIZE]; + +static unsigned partial_frame_count; +static bool insync; +static unsigned channel_shift; + +static void dsm_decode(void); + +void +dsm_init(unsigned mode) +{ + insync = false; + partial_frame_count = 0; + + if (mode == RX_MODE_DSM_10BIT) { + channel_shift = 10; + } else { + channel_shift = 11; + } + + last_frame_time = hrt_absolute_time(); +} + +void +dsm_input(int fd) +{ + uint8_t buf[DSM_FRAME_SIZE]; + ssize_t ret; + hrt_abstime now; + + /* + * The DSM* protocol doesn't provide any explicit framing, + * so we detect frame boundaries by the inter-frame delay. + * + * The minimum frame spacing is 11ms; with 16 bytes at 115200bps + * frame transmission time is ~1.4ms. + * + * We expect to only be called when bytes arrive for processing, + * and if an interval of more than 5ms passes between calls, + * the first byte we read will be the first byte of a frame. + */ + now = hrt_absolute_time(); + if ((now - last_frame_time) > 5000) + partial_frame_count = 0; + + /* + * Fetch bytes, but no more than we would need to complete + * the current frame. + */ + ret = read(fd, buf, DSM_FRAME_SIZE - partial_frame_count); + + /* if the read failed for any reason, just give up here */ + if (ret < 1) + return; + + /* + * Add bytes to the current frame + */ + memcpy(&frame[partial_frame_count], buf, ret); + partial_frame_count += ret; + + /* + * If we don't have a full frame, return + */ + if (partial_frame_count < DSM_FRAME_SIZE) + return; + last_frame_time = now; + + /* + * Great, it looks like we might have a frame. Go ahead and + * decode it. + */ + dsm_decode(); + partial_frame_count = 0; +} + +static void +dsm_decode(void) +{ + uint16_t data_mask = (1 << channel_shift) - 1; + + /* + * The encoding of the first byte is uncertain, so we're going + * to ignore it for now. + * + * The second byte may tell us about the protocol, but it's not + * actually very interesting since what we really want to know + * is how the channel data is formatted, and there doesn't seem + * to be a reliable way to determine this from the protocol ID + * alone. + * + * Each channel is a 16-bit unsigned value containing either a 10- + * or 11-bit channel value and a 4-bit channel number, shifted + * either 10 or 11 bits. The MSB may also be set to indicate the + * second frame in variants of the protocol where more than + * seven channels are being transmitted. + */ + + for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { + + uint8_t *dp = &frame[2 + (2 * i)]; + uint16_t raw = (dp[0] << 8) | dp[1]; + + /* ignore pad channels */ + if (raw == 0xffff) + continue; + + unsigned channel = (raw >> channel_shift) & 0xf; + + /* ignore channels out of range */ + if (channel >= PX4IO_INPUT_CHANNELS) + continue; + + if (channel > ppm_decoded_channels) + ppm_decoded_channels = channel; + + /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ + unsigned data = raw & data_mask; + if (channel_shift == 11) + data /= 2; + ppm_buffer[channel] = 988 + data; + + } + ppm_last_valid_decode = hrt_absolute_time(); +} diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c index 471965fd7..b614f3fa4 100644 --- a/apps/px4io/mixer.c +++ b/apps/px4io/mixer.c @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file Control channel input/output mixer and failsafe. + * @file mixer.c + * + * Control channel input/output mixer and failsafe. */ #include @@ -43,8 +45,6 @@ #include #include #include -#include -#include #include #include @@ -55,23 +55,12 @@ #include "px4io.h" -/* - * Count of periodic calls in which we have no data. - */ -static unsigned mixer_input_drops; -#define MIXER_INPUT_DROP_LIMIT 10 - /* * Count of periodic calls in which we have no FMU input. */ static unsigned fmu_input_drops; #define FMU_INPUT_DROP_LIMIT 20 -/* - * Serial port fd for serial RX protocols - */ -static int rx_port = -1; - /* * HRT periodic call used to check for control input data. */ @@ -106,8 +95,7 @@ struct mixer { int mixer_init(void) { - /* open the serial port */ - rx_port = open("/dev/ttyS0", O_RDONLY | O_NONBLOCK); + /* look for control data at 50Hz */ hrt_call_every(&mixer_input_call, 1000, 20000, mixer_tick, NULL); @@ -115,37 +103,6 @@ mixer_init(void) return 0; } -void -mixer_set_serial_mode(uint8_t serial_mode) -{ - - if (serial_mode == system_state.serial_rx_mode) - return; - - struct termios t; - tcgetattr(rx_port, &t); - - switch (serial_mode) { - case RX_MODE_PPM_ONLY: - break; - case RX_MODE_SPEKTRUM_6: - case RX_MODE_SPEKTRUM_7: - /* 115200, no parity, one stop bit */ - cfsetspeed(&t, 115200); - t.c_cflag &= ~(CSTOPB | PARENB); - break; - case RX_MODE_FUTABA_SBUS: - /* 100000, even parity, two stop bits */ - cfsetspeed(&t, 100000); - t.c_cflag |= (CSTOPB | PARENB); - break; - default: - return; - } - - tcsetattr(rx_port, TCSANOW, &t); - system_state.serial_rx_mode = serial_mode; -} static void mixer_tick(void *arg) @@ -234,148 +191,20 @@ mixer_update(int mixer, uint16_t *inputs, int input_count) } } -static bool -mixer_get_spektrum_input(void) -{ - static uint8_t buf[16]; - static unsigned count; - - /* always read as much data as we can into the buffer */ - if (count >= sizeof(buf)) - count = 0; - ssize_t result = read(rx_port, buf, sizeof(buf) - count); - /* no data or an error */ - if (result <= 0) - return false; - count += result; - - /* if there are more than two bytes in the buffer, check for sync */ - if (count >= 2) { - if ((buf[0] != 0x3) || (buf[1] != 0x1)) { - /* not in sync; look for a possible sync marker */ - for (unsigned i = 1; i < count; i++) { - if (buf[i] == 0x3) { - /* could be a frame marker; move buffer bytes */ - count -= i; - memmove(buf, buf + i, count); - break; - } - } - } - } - if (count < sizeof(buf)) - return false; - - /* we got a frame; decode it */ - const uint16_t *channels = (const uint16_t *)&buf[2]; - - /* - * Channel assignment for DX6i vs. DX7 is different. - * - * DX7 etc. is: - * - * 0: Aileron - * 1: Flaps - * 2: Gear - * 3: Elevator - * 4: Aux2 - * 5: Throttle - * 6: Rudder - * - * DX6i is: - * - * 0: Aileron - * 1: Flaps - * 2: Elevator - * 3: Rudder - * 4: Throttle - * 5: Gear - * 6: - * - * We convert these to our standard Futaba-style assignment: - * - * 0: Throttle (Throttle) - * 1: Roll (Aileron) - * 2: Pitch (Elevator) - * 3: Yaw (Rudder) - * 4: Override (Flaps) - * 5: FUNC_0 (Gear) - * 6: FUNC_1 (Aux2) - */ - if (system_state.serial_rx_mode == RX_MODE_SPEKTRUM_7) { - system_state.rc_channel_data[0] = channels[5]; /* Throttle */ - system_state.rc_channel_data[1] = channels[0]; /* Roll */ - system_state.rc_channel_data[2] = channels[3]; /* Pitch */ - system_state.rc_channel_data[3] = channels[6]; /* Yaw */ - system_state.rc_channel_data[4] = channels[1]; /* Override */ - system_state.rc_channel_data[5] = channels[2]; /* FUNC_0 */ - system_state.rc_channel_data[6] = channels[4]; /* FUNC_1 */ - system_state.rc_channels = 7; - } else { - system_state.rc_channel_data[0] = channels[4]; /* Throttle */ - system_state.rc_channel_data[1] = channels[0]; /* Roll */ - system_state.rc_channel_data[2] = channels[2]; /* Pitch */ - system_state.rc_channel_data[3] = channels[3]; /* Yaw */ - system_state.rc_channel_data[4] = channels[1]; /* Override */ - system_state.rc_channel_data[5] = channels[5]; /* FUNC_0 */ - system_state.rc_channels = 6; - } - count = 0; - return true; -} - -static bool -mixer_get_sbus_input(void) -{ - /* XXX not implemented yet */ - return false; -} - static void mixer_get_rc_input(void) { - bool got_input = false; - - switch (system_state.serial_rx_mode) { - case RX_MODE_PPM_ONLY: - if (ppm_decoded_channels > 0) { - /* copy channel data */ - system_state.rc_channels = ppm_decoded_channels; - for (unsigned i = 0; i < ppm_decoded_channels; i++) - system_state.rc_channel_data[i] = ppm_buffer[i]; - got_input = true; - } - break; - - case RX_MODE_SPEKTRUM_6: - case RX_MODE_SPEKTRUM_7: - got_input = mixer_get_spektrum_input(); - break; - - case RX_MODE_FUTABA_SBUS: - got_input = mixer_get_sbus_input(); - break; - - default: - break; - } - if (got_input) { - mixer_input_drops = 0; + /* if we haven't seen any new data in 200ms, assume we have lost input and tell FMU */ + if ((hrt_absolute_time() - ppm_last_valid_decode) > 200000) { + system_state.rc_channels = 0; system_state.fmu_report_due = true; - } else { - /* - * No data; count the 'frame drops' and once we hit the limit - * assume that we have lost input. - */ - if (mixer_input_drops < MIXER_INPUT_DROP_LIMIT) { - mixer_input_drops++; - - /* if we hit the limit, stop pretending we have input and let the FMU know */ - if (mixer_input_drops == MIXER_INPUT_DROP_LIMIT) { - system_state.rc_channels = 0; - system_state.fmu_report_due = true; - } - } + return; } + + /* otherwise, copy channel data */ + system_state.rc_channels = ppm_decoded_channels; + for (unsigned i = 0; i < ppm_decoded_channels; i++) + system_state.rc_channel_data[i] = ppm_buffer[i]; + system_state.fmu_report_due = true; } diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 7467f1adc..660eed12b 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -41,11 +41,6 @@ #pragma once -/* - * XXX MUST BE KEPT IN SYNC WITH THE VERSION IN PX4FMU UNTIL - * TREES ARE MERGED. - */ - #define PX4IO_OUTPUT_CHANNELS 8 #define PX4IO_INPUT_CHANNELS 12 #define PX4IO_RELAY_CHANNELS 4 @@ -69,8 +64,8 @@ struct px4io_config { uint8_t serial_rx_mode; #define RX_MODE_PPM_ONLY 0 -#define RX_MODE_SPEKTRUM_6 1 -#define RX_MODE_SPEKTRUM_7 2 +#define RX_MODE_DSM_10BIT 1 +#define RX_MODE_DSM_11BIT 2 #define RX_MODE_FUTABA_SBUS 3 }; diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 7039e5d58..e02d865d5 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -72,7 +72,7 @@ int user_start(int argc, char *argv[]) /* configure the high-resolution time/callout interface */ hrt_init(); - /* init the FMU link */ + /* init the FMU and receiver links */ comms_init(); /* configure the first 8 PWM outputs (i.e. all of them) */ diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 274b27ff3..536dbebf1 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -147,7 +147,6 @@ extern volatile int timers[TIMER_NUM_TIMERS]; * Mixer */ extern int mixer_init(void); -extern void mixer_set_serial_mode(uint8_t newmode); /* * Safety switch/LED. @@ -160,6 +159,14 @@ extern void safety_init(void); extern void comms_init(void); extern void comms_check(void); +/* + * Serial receiver decoders. + */ +extern void dsm_init(unsigned mode); +extern void dsm_input(int fd); +extern void sbus_init(unsigned mode); +extern void sbus_input(int fd); + /* * Assertion codes */ diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c new file mode 100644 index 000000000..e363a0a78 --- /dev/null +++ b/apps/px4io/sbus.c @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 sbus.c + * + * Serial protocol decoder for the Futaba S.bus protocol. + */ + +#include + +#include +#include + +#include + +#include "px4io.h" +#include "protocol.h" + +void +sbus_init(unsigned mode) +{ +} + +void +sbus_input(int fd) +{ +} -- cgit v1.2.3