diff options
| -rw-r--r-- | Tools/tests-host/Makefile | 2 | ||||
| -rw-r--r-- | Tools/tests-host/sbus2_test.cpp | 63 | ||||
| -rw-r--r-- | src/modules/px4iofirmware/controls.c | 11 | ||||
| -rw-r--r-- | src/modules/px4iofirmware/mixer.cpp | 11 | ||||
| -rw-r--r-- | src/modules/px4iofirmware/px4io.h | 7 | ||||
| -rw-r--r-- | src/modules/px4iofirmware/sbus.c | 237 | ||||
| -rw-r--r-- | src/modules/px4iofirmware/sbus.h | 61 |
7 files changed, 333 insertions, 59 deletions
diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile index f0737ef88..6e1d08fc1 100644 --- a/Tools/tests-host/Makefile +++ b/Tools/tests-host/Makefile @@ -36,4 +36,4 @@ autodeclination_test: $(SBUS2_FILES) .PHONY: clean clean: - rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test
\ No newline at end of file + rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test diff --git a/Tools/tests-host/sbus2_test.cpp b/Tools/tests-host/sbus2_test.cpp index d8fcb695d..a1fee9fd2 100644 --- a/Tools/tests-host/sbus2_test.cpp +++ b/Tools/tests-host/sbus2_test.cpp @@ -2,10 +2,11 @@ #include <stdio.h> #include <unistd.h> #include <string.h> +#include <stdlib.h> #include <systemlib/mixer/mixer.h> #include <systemlib/err.h> #include <drivers/drv_hrt.h> -#include <px4iofirmware/px4io.h> +#include <px4iofirmware/sbus.h> #include "../../src/systemcmds/tests/tests.h" int main(int argc, char *argv[]) { @@ -14,6 +15,15 @@ int main(int argc, char *argv[]) { if (argc < 2) errx(1, "Need a filename for the input file"); + int byte_offset = 7; + + if (argc > 2) { + char* end; + byte_offset = strtol(argv[2],&end,10); + } + + warnx("RUNNING TEST WITH BYTE OFFSET OF: %d", byte_offset); + warnx("loading data from: %s", argv[1]); FILE *fp; @@ -33,37 +43,70 @@ int main(int argc, char *argv[]) { } // Init the parser - uint8_t frame[30]; + uint8_t frame[SBUS_BUFFER_SIZE]; unsigned partial_frame_count = 0; uint16_t rc_values[18]; uint16_t num_values; + uint16_t sbus_frame_drops = 0; + unsigned sbus_frame_resets = 0; bool sbus_failsafe; bool sbus_frame_drop; uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]); float last_time = 0; + int rate_limiter = 0; + while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) { - if (((f - last_time) * 1000 * 1000) > 3000) { + + unsigned last_drop = sbus_frame_drops + sbus_frame_resets; + + unsigned interval_us = ((f - last_time) * 1000 * 1000); + + if (interval_us > SBUS_INTER_FRAME_TIMEOUT) { + if (partial_frame_count != 0) { + warnx("[ %08.4fs ] INTERVAL: %u - FRAME RESET, DROPPED %d bytes", + interval_us, f, partial_frame_count); + + printf("\t\tdropped: "); + for (int i = 0; i < partial_frame_count; i++) { + printf("%02X ", frame[i]); + } + printf("\n"); + + sbus_frame_resets++; + } + partial_frame_count = 0; - warnx("FRAME RESET\n\n"); } + last_time = f; + frame[partial_frame_count] = x; partial_frame_count++; //warnx("%f: 0x%02x, first: 0x%02x, last: 0x%02x, pcount: %u", (double)f, x, frame[0], frame[24], partial_frame_count); - if (partial_frame_count == sizeof(frame)) + if (partial_frame_count == sizeof(frame)) { partial_frame_count = 0; - - last_time = f; + warnx("FRAME SIZE OVERFLOW!\n\n\n"); + } // Pipe the data into the parser hrt_abstime now = hrt_absolute_time(); - //if (partial_frame_count % 25 == 0) - //sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, &sbus_failsafe, &sbus_frame_drop, max_channels); + if (rate_limiter % byte_offset == 0) { + bool result = sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, + &sbus_failsafe, &sbus_frame_drop, &sbus_frame_drops, max_channels); + + if (result) + warnx("decoded packet"); + } + + if (last_drop != (sbus_frame_drops + sbus_frame_resets)) + warnx("frame dropped, now #%d", (sbus_frame_drops + sbus_frame_resets)); + + rate_limiter++; } if (ret == EOF) { @@ -72,4 +115,4 @@ int main(int argc, char *argv[]) { warnx("Test aborted, errno: %d", ret); } -} +}
\ No newline at end of file diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 185cb20dd..f86ed3a6e 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -43,8 +43,8 @@ #include <drivers/drv_hrt.h> #include <systemlib/perf_counter.h> #include <systemlib/ppm_decode.h> - #include "px4io.h" +#include "sbus.h" #define RC_FAILSAFE_TIMEOUT 2000000 /**< two seconds failsafe timeout */ #define RC_CHANNEL_HIGH_THRESH 5000 /* 75% threshold */ @@ -135,7 +135,14 @@ controls_tick() { perf_begin(c_gather_sbus); bool sbus_failsafe, sbus_frame_drop; - bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS); + + /* + * These are decoding errors on the serial link between the RX and us, + * not wireless frame drops + */ + uint16_t sbus_serial_frame_drops; + bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, + &sbus_failsafe, &sbus_frame_drop, &sbus_serial_frame_drops, PX4IO_RC_INPUT_CHANNELS); if (sbus_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 2f721bf1e..8f47a9efb 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -55,6 +55,8 @@ extern "C" { #include "px4io.h" } +#include "sbus.h" + /* * Maximum interval in us before FMU signal is considered lost */ @@ -187,15 +189,23 @@ mixer_tick(void) && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK); + unsigned actuator_count = 0; + /* * Run the mixers. */ if (source == MIX_FAILSAFE) { + actuator_count = 0; + /* copy failsafe values to the servo outputs */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { r_page_servos[i] = r_page_servo_failsafe[i]; + /* get the count of the last valid actuator */ + if ((r_page_servos[i] != 0) && ((i + 1) > actuator_count)) + actuator_count = i + 1; + /* safe actuators for FMU feedback */ r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f); } @@ -211,6 +221,7 @@ mixer_tick(void) /* poor mans mutex */ in_mixer = true; mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); + actuator_count = mixed; in_mixer = false; /* the pwm limit call takes care of out of band errors */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index b00a96717..2ae081d51 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -37,6 +37,8 @@ * General defines and structures for the PX4IO module firmware. */ +#pragma once + #include <nuttx/config.h> #include <stdbool.h> @@ -47,6 +49,7 @@ #include "protocol.h" #include <systemlib/pwm_limit/pwm_limit.h> +#include <drivers/drv_hrt.h> /* * Constants and limits. @@ -217,10 +220,6 @@ extern void controls_tick(void); extern int dsm_init(const char *device); extern bool dsm_input(uint16_t *values, uint16_t *num_values); 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); -extern void sbus1_output(uint16_t *values, uint16_t num_values); -extern void sbus2_output(uint16_t *values, uint16_t num_values); /** global debug level for isr_debug() */ extern volatile uint8_t debug_level; diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 0f0414148..925e24f65 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -42,17 +42,21 @@ #include <fcntl.h> #include <unistd.h> #include <termios.h> +#include <string.h> #include <systemlib/ppm_decode.h> #include <drivers/drv_hrt.h> -#define DEBUG +//#define DEBUG #include "px4io.h" +#include "sbus.h" #include "protocol.h" #include "debug.h" -#define SBUS_FRAME_SIZE 25 +#define SBUS2_FRAME_SIZE_RX_VOLTAGE 3 +#define SBUS2_FRAME_SIZE_GPS_DIGIT 3 + #define SBUS_INPUT_CHANNELS 16 #define SBUS_FLAGS_BYTE 23 #define SBUS_FAILSAFE_BIT 3 @@ -66,11 +70,11 @@ */ /* define range mapping here, -+100% -> 1000..2000 */ -#define SBUS_RANGE_MIN 200.0f -#define SBUS_RANGE_MAX 1800.0f +#define SBUS_RANGE_MIN 200.0f +#define SBUS_RANGE_MAX 1800.0f -#define SBUS_TARGET_MIN 1000.0f -#define SBUS_TARGET_MAX 2000.0f +#define SBUS_TARGET_MIN 1000.0f +#define SBUS_TARGET_MAX 2000.0f /* pre-calculate the floating point stuff as far as possible at compile time */ #define SBUS_SCALE_FACTOR ((SBUS_TARGET_MAX - SBUS_TARGET_MIN) / (SBUS_RANGE_MAX - SBUS_RANGE_MIN)) @@ -81,13 +85,25 @@ static int sbus_fd = -1; static hrt_abstime last_rx_time; static hrt_abstime last_frame_time; -static uint8_t frame[SBUS_FRAME_SIZE]; +static enum SBUS2_DECODE_STATE { + SBUS2_DECODE_STATE_DESYNC = 0xFFF, + SBUS2_DECODE_STATE_SBUS_START = 0x2FF, + SBUS2_DECODE_STATE_SBUS1_SYNC = 0x00, + SBUS2_DECODE_STATE_SBUS2_SYNC = 0x1FF, + SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE = 0x04, + SBUS2_DECODE_STATE_SBUS2_GPS = 0x14, + SBUS2_DECODE_STATE_SBUS2_DATA1 = 0x24, + SBUS2_DECODE_STATE_SBUS2_DATA2 = 0x34 +} sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; + +static uint8_t sbus_frame[SBUS_FRAME_SIZE + (SBUS_FRAME_SIZE / 2)]; static unsigned partial_frame_count; unsigned sbus_frame_drops; -static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels); +static bool sbus_decode(hrt_abstime frame_time, uint8_t *frame, uint16_t *values, uint16_t *num_values, + bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels); int sbus_init(const char *device) @@ -113,6 +129,9 @@ sbus_init(const char *device) } else { debug("S.Bus: open failed"); } + + sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; + return sbus_fd; } @@ -131,13 +150,13 @@ sbus2_output(uint16_t *values, uint16_t num_values) } bool -sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels) +sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t *serial_frame_drops, uint16_t max_channels) { ssize_t ret; hrt_abstime now; /* - * The S.bus protocol doesn't provide reliable framing, + * The S.BUS protocol doesn't provide reliable framing, * so we detect frame boundaries by the inter-frame delay. * * The minimum frame spacing is 7ms; with 25 bytes at 100000bps @@ -153,7 +172,7 @@ sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sb */ now = hrt_absolute_time(); - if ((now - last_rx_time) > 3000) { + if ((now - last_rx_time) > SBUS_INTER_FRAME_TIMEOUT) { if (partial_frame_count > 0) { sbus_frame_drops++; partial_frame_count = 0; @@ -162,15 +181,14 @@ sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sb /* * Fetch bytes, but no more than we would need to complete - * the current frame. + * a complete frame. */ - ret = read(sbus_fd, &frame[partial_frame_count], SBUS_FRAME_SIZE - partial_frame_count); + ret = read(sbus_fd, &sbus_frame[partial_frame_count], SBUS_FRAME_SIZE - partial_frame_count); /* if the read failed for any reason, just give up here */ - if (ret < 1) + if (ret < 1) { return false; - - last_rx_time = now; + } /* * Add bytes to the current frame @@ -178,17 +196,147 @@ sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sb partial_frame_count += ret; /* - * If we don't have a full frame, return + * Try to decode something with what we got */ - if (partial_frame_count < SBUS_FRAME_SIZE) - return false; + return sbus_parse(now, sbus_frame, &partial_frame_count, values, num_values, sbus_failsafe, sbus_frame_drop, serial_frame_drops, max_channels); +} - /* - * Great, it looks like we might have a frame. Go ahead and - * decode it. +bool +sbus_parse(hrt_abstime now, uint8_t *frame, unsigned *partial_count, uint16_t *values, + uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t *frame_drops, uint16_t max_channels) +{ + + last_rx_time = now; + + /* this is set by the decoding state machine and will default to false + * once everything that was decodable has been decoded. */ - partial_frame_count = 0; - return sbus_decode(now, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels); + bool decode_ret = true; + + /* keep decoding until we have consumed the buffer or have given up */ + for (unsigned d = 0; ((d < 10) && (*partial_count > 0) && (decode_ret)); d++) { + unsigned n_consumed = 0; + + switch (sbus_decode_state) { + case SBUS2_DECODE_STATE_DESYNC: + /* we are de-synced and only interested in the frame marker */ + { + unsigned i = 0; + while (i < *partial_count) { + + /* for the case where frame[i] is the start sign, the index + * is exactly the number of bytes to consume + */ + n_consumed = i; + + if (frame[i] == 0x0F) { + sbus_decode_state = SBUS2_DECODE_STATE_SBUS_START; + break; + } + + /* increment index to next position */ + i++; + } + } + break; + /* fall through */ + case SBUS2_DECODE_STATE_SBUS_START: + case SBUS2_DECODE_STATE_SBUS1_SYNC: + /* fall through */ + case SBUS2_DECODE_STATE_SBUS2_SYNC: + { + /* decode whatever we got and expect */ + if (*partial_count < SBUS_FRAME_SIZE) { + decode_ret = false; + break; + } + + /* + * Great, it looks like we might have a frame. Go ahead and + * decode it. + */ + decode_ret = sbus_decode(now, frame, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels); + + if (decode_ret) { + n_consumed = SBUS_FRAME_SIZE; + } + } + break; + case SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE: + { + if (*partial_count < SBUS2_FRAME_SIZE_RX_VOLTAGE) { + decode_ret = false; + break; + } + + /* find out which payload we're dealing with in this slot */ + switch(frame[0]) { + case 0x03: + { + uint16_t rx_voltage = (frame[1] << 8) | frame[2]; + isr_debug(30, "rx_voltage %d", (int)rx_voltage); + n_consumed = 3; + } + break; + case 0x0F: + /* the battery slot is unused and followed by a normal frame */ + sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC; + break; + default: + /* this is not what we expect it to be, go back to sync */ + sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; + sbus_frame_drops++; + /* throw unknown bytes away */ + n_consumed = 3; + } + } + break; + case SBUS2_DECODE_STATE_SBUS2_GPS: + { + if (*partial_count < 24) { + decode_ret = false; + break; + } + + /* find out which payload we're dealing with in this slot */ + switch(frame[0]) { + case 0x13: + { + uint16_t gps_something = (frame[1] << 8) | frame[2]; + isr_debug(30, "gps_something %d", (int)gps_something); + n_consumed = 24; + } + break; + case 0x0F: + /* the GPS slot is unused and followed by a normal frame */ + sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC; + break; + default: + /* this is not what we expect it to be, go back to sync */ + sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; + sbus_frame_drops++; + /* throw unknown bytes away */ + n_consumed = 3; + } + } + break; + default: + decode_ret = false; + } + + /* move buffer to start after this step, keep decoding if any bytes remain */ + + /* XXX should be still more efficient than single-byte ringbuffer accesses */ + uint8_t frame_buf[SBUS_FRAME_SIZE + 10]; + memcpy(&frame_buf[0], &frame[n_consumed], *partial_count - n_consumed); + memcpy(&frame[0], &frame_buf[0], *partial_count - n_consumed); + *partial_count = *partial_count - n_consumed; + } + + *frame_drops = sbus_frame_drops; + + /* return false as default */ + return decode_ret; } /* @@ -228,8 +376,10 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { }; static bool -sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values) +sbus_decode(hrt_abstime frame_time, uint8_t *frame, uint16_t *values, uint16_t *num_values, + bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values) { + /* check frame boundary markers to avoid out-of-sync cases */ if ((frame[0] != 0x0f)) { sbus_frame_drops++; @@ -238,24 +388,27 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool switch (frame[24]) { case 0x00: - /* this is S.BUS 1 */ - break; - case 0x03: - /* S.BUS 2 SLOT0: RX battery and external voltage */ - break; - case 0x83: - /* S.BUS 2 SLOT1 */ - break; - case 0x43: - case 0xC3: - case 0x23: - case 0xA3: - case 0x63: - case 0xE3: - break; + /* this is S.BUS 1 */ + sbus_decode_state = SBUS2_DECODE_STATE_SBUS1_SYNC; + break; + case 0x04: + /* receiver voltage */ + sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE; + break; + case 0x14: + /* GPS / baro */ + sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_GPS; + break; + case 0x24: + /* Unknown SBUS2 data */ + sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC; + break; + case 0x34: + /* Unknown SBUS2 data */ + sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC; + break; default: - /* we expect one of the bits above, but there are some we don't know yet */ - break; + return false; } /* we have received something we think is a frame */ diff --git a/src/modules/px4iofirmware/sbus.h b/src/modules/px4iofirmware/sbus.h new file mode 100644 index 000000000..e32bddfd3 --- /dev/null +++ b/src/modules/px4iofirmware/sbus.h @@ -0,0 +1,61 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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 sbus.h + * + * S.BUS / S.BUS2 protocol decoder / encoder + */ + +#pragma once + +#include <stdbool.h> +#include <stdint.h> + +#include <drivers/drv_hrt.h> + +__BEGIN_DECLS + +#define SBUS_INTER_FRAME_TIMEOUT 3000 /**< 3000 us frame timeout */ +#define SBUS_FRAME_SIZE 25 +#define SBUS_BUFFER_SIZE (SBUS_FRAME_SIZE + SBUS_FRAME_SIZE / 2) + +int sbus_init(const char *device); +bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t *serial_frame_drops, uint16_t max_channels); +bool sbus_parse(hrt_abstime now, uint8_t *frame, unsigned *partial_count, uint16_t *values, + uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t *serial_frame_drops, uint16_t max_channels); + +void sbus1_output(uint16_t *values, uint16_t num_values); +void sbus2_output(uint16_t *values, uint16_t num_values); + +__END_DECLS |