diff options
author | Julian Oes <joes@student.ethz.ch> | 2012-11-07 14:46:28 -0800 |
---|---|---|
committer | Julian Oes <joes@student.ethz.ch> | 2012-11-07 14:46:28 -0800 |
commit | 38b20f2a1ad32ed3091c781713c01f209f95016a (patch) | |
tree | 7c66db0603018332c4fc861dae9881ebb7730a69 /apps/px4io | |
parent | 25ed791b7016cbbcd2dfeac8b62461733bd5a26e (diff) | |
download | px4-firmware-38b20f2a1ad32ed3091c781713c01f209f95016a.tar.gz px4-firmware-38b20f2a1ad32ed3091c781713c01f209f95016a.tar.bz2 px4-firmware-38b20f2a1ad32ed3091c781713c01f209f95016a.zip |
Revert "Another take on Spektrum/DSM frame decoding, based on more careful examination of the relevant docs."
This reverts commit ff3a014971f83f15f4884e584a2f58ee979f23ee.
Diffstat (limited to 'apps/px4io')
-rw-r--r-- | apps/px4io/comms.c | 119 | ||||
-rw-r--r-- | apps/px4io/dsm.c | 184 | ||||
-rw-r--r-- | apps/px4io/mixer.c | 199 | ||||
-rw-r--r-- | apps/px4io/protocol.h | 9 | ||||
-rw-r--r-- | apps/px4io/px4io.c | 2 | ||||
-rw-r--r-- | apps/px4io/px4io.h | 9 | ||||
-rw-r--r-- | apps/px4io/sbus.c | 58 |
7 files changed, 208 insertions, 372 deletions
diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index f1dac433f..0a0453b55 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -44,7 +44,6 @@ #include <debug.h> #include <stdlib.h> #include <errno.h> -#include <termios.h> #include <string.h> #include <poll.h> @@ -61,126 +60,26 @@ 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; @@ -188,7 +87,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; @@ -202,6 +101,15 @@ 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; @@ -219,7 +127,8 @@ comms_handle_config(const void *buffer, size_t length) frame_rx++; - serial_rx_init(cfg->serial_rx_mode); + mixer_set_serial_mode(cfg->serial_rx_mode); + } static void diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c deleted file mode 100644 index 79b6301c7..000000000 --- a/apps/px4io/dsm.c +++ /dev/null @@ -1,184 +0,0 @@ -/**************************************************************************** - * - * 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 <nuttx/config.h> - -#include <fcntl.h> -#include <unistd.h> -#include <string.h> - -#include <systemlib/ppm_decode.h> - -#include <drivers/drv_hrt.h> - -#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 fa4b3eb25..28edb2357 100644 --- a/apps/px4io/mixer.c +++ b/apps/px4io/mixer.c @@ -32,9 +32,7 @@ ****************************************************************************/ /** - * @file mixer.c - * - * Control channel input/output mixer and failsafe. + * @file Control channel input/output mixer and failsafe. */ #include <nuttx/config.h> @@ -45,6 +43,8 @@ #include <string.h> #include <assert.h> #include <errno.h> +#include <fcntl.h> +#include <termios.h> #include <unistd.h> #include <fcntl.h> @@ -56,12 +56,23 @@ #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. */ static struct hrt_call mixer_input_call; @@ -95,7 +106,8 @@ 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); @@ -103,6 +115,37 @@ 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) @@ -191,20 +234,148 @@ 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: <notused> + * + * 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; - /* 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; - return; + 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; } - /* 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; + if (got_input) { + mixer_input_drops = 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; + } + } + } } diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 660eed12b..7467f1adc 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -41,6 +41,11 @@ #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 @@ -64,8 +69,8 @@ struct px4io_config { uint8_t serial_rx_mode; #define RX_MODE_PPM_ONLY 0 -#define RX_MODE_DSM_10BIT 1 -#define RX_MODE_DSM_11BIT 2 +#define RX_MODE_SPEKTRUM_6 1 +#define RX_MODE_SPEKTRUM_7 2 #define RX_MODE_FUTABA_SBUS 3 }; diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index dec2cdde6..7240a80ef 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 and receiver links */ + /* init the FMU link */ 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 f50e29252..8eb0c53b2 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -147,6 +147,7 @@ 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,14 +161,6 @@ 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 */ #define A_GPIO_OPEN_FAIL 100 diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c deleted file mode 100644 index e363a0a78..000000000 --- a/apps/px4io/sbus.c +++ /dev/null @@ -1,58 +0,0 @@ -/**************************************************************************** - * - * 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 <nuttx/config.h> - -#include <fcntl.h> -#include <unistd.h> - -#include <drivers/drv_hrt.h> - -#include "px4io.h" -#include "protocol.h" - -void -sbus_init(unsigned mode) -{ -} - -void -sbus_input(int fd) -{ -} |