From 87cf160875a99b6200be031792370094e0a02422 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 5 Nov 2018 13:01:17 +1100 Subject: [PATCH] AP_RCProtocol: switched SBUS to SoftSerial decoder --- libraries/AP_RCProtocol/AP_RCProtocol.cpp | 5 +- .../AP_RCProtocol/AP_RCProtocol_SBUS.cpp | 110 +++++------------- libraries/AP_RCProtocol/AP_RCProtocol_SBUS.h | 14 ++- .../AP_RCProtocol/AP_RCProtocol_SBUS_NI.h | 32 ----- 4 files changed, 39 insertions(+), 122 deletions(-) delete mode 100644 libraries/AP_RCProtocol/AP_RCProtocol_SBUS_NI.h diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index 6baeae4194..faa310b92f 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp @@ -19,7 +19,6 @@ #include "AP_RCProtocol_PPMSum.h" #include "AP_RCProtocol_DSM.h" #include "AP_RCProtocol_SBUS.h" -#include "AP_RCProtocol_SBUS_NI.h" #include "AP_RCProtocol_SUMD.h" #include "AP_RCProtocol_SRXL.h" #include "AP_RCProtocol_ST24.h" @@ -30,8 +29,8 @@ AP_RCProtocol *AP_RCProtocol::instance; void AP_RCProtocol::init() { backend[AP_RCProtocol::PPM] = new AP_RCProtocol_PPMSum(*this); - backend[AP_RCProtocol::SBUS] = new AP_RCProtocol_SBUS(*this); - backend[AP_RCProtocol::SBUS_NI] = new AP_RCProtocol_SBUS_NI(*this); + backend[AP_RCProtocol::SBUS] = new AP_RCProtocol_SBUS(*this, true); + backend[AP_RCProtocol::SBUS_NI] = new AP_RCProtocol_SBUS(*this, false); backend[AP_RCProtocol::DSM] = new AP_RCProtocol_DSM(*this); backend[AP_RCProtocol::SUMD] = new AP_RCProtocol_SUMD(*this); backend[AP_RCProtocol::SRXL] = new AP_RCProtocol_SRXL(*this); diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp index bb4726129e..636dab4cc7 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp @@ -107,6 +107,13 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { }; +// constructor +AP_RCProtocol_SBUS::AP_RCProtocol_SBUS(AP_RCProtocol &_frontend, bool _inverted) : + AP_RCProtocol_Backend(_frontend), + inverted(_inverted) +{} + +// decode a full SBUS frame bool AP_RCProtocol_SBUS::sbus_decode(const uint8_t frame[25], uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values) { @@ -203,97 +210,29 @@ bool AP_RCProtocol_SBUS::sbus_decode(const uint8_t frame[25], uint16_t *values, */ void AP_RCProtocol_SBUS::process_pulse(uint32_t width_s0, uint32_t width_s1) { - // convert to bit widths, allowing for up to 4usec error, assuming 100000 bps - uint16_t bits_s0 = (width_s0+4) / 10; - uint16_t bits_s1 = (width_s1+4) / 10; - uint16_t nlow; - - uint8_t byte_ofs = sbus_state.bit_ofs/12; - uint8_t bit_ofs = sbus_state.bit_ofs%12; - - if (bits_s0 == 0 || bits_s1 == 0) { - // invalid data - goto reset; - } - - if (bits_s0+bit_ofs > 10) { - // invalid data as last two bits must be stop bits - goto reset; - } - - if (byte_ofs >= ARRAY_SIZE(sbus_state.bytes)) { - goto reset; + uint32_t w0 = width_s0; + uint32_t w1 = width_s1; + if (inverted) { + w0 = saved_width; + w1 = width_s0; + saved_width = width_s1; } - // pull in the high bits - sbus_state.bytes[byte_ofs] |= ((1U< 12) { - nlow = 12 - bit_ofs; + uint8_t b; + if (ss.process_pulse(w0, w1, b)) { + _process_byte(ss.get_byte_timestamp_us(), b); } - bits_s1 -= nlow; - sbus_state.bit_ofs += nlow; - - if (sbus_state.bit_ofs == 25*12 && bits_s1 > 12) { - // we have a full frame - uint8_t bytes[25]; - uint8_t i; - for (i=0; i<25; i++) { - // get inverted data - uint16_t v = ~sbus_state.bytes[i]; - // check start bit - if ((v & 1) != 0) { - goto reset; - } - // check stop bits - if ((v & 0xC00) != 0xC00) { - goto reset; - } - // check parity - uint8_t parity = 0, j; - for (j=1; j<=8; j++) { - parity ^= (v & (1U<>9) { - goto reset; - } - bytes[i] = ((v>>1) & 0xFF); - } - uint16_t values[MAX_RCIN_CHANNELS]; - uint16_t num_values=0; - bool sbus_failsafe=false, sbus_frame_drop=false; - if (sbus_decode(bytes, values, &num_values, - &sbus_failsafe, &sbus_frame_drop, - MAX_RCIN_CHANNELS) && - num_values >= MIN_RCIN_CHANNELS) { - add_input(num_values, values, sbus_failsafe); - } - goto reset; - } else if (bits_s1 > 12) { - // break - goto reset; - } - return; -reset: - memset(&sbus_state, 0, sizeof(sbus_state)); } // support byte input -void AP_RCProtocol_SBUS::process_byte(uint8_t b, uint32_t baudrate) +void AP_RCProtocol_SBUS::_process_byte(uint32_t timestamp_us, uint8_t b) { - if (baudrate != 100000) { - return; - } - uint32_t now = AP_HAL::millis(); - if (now - byte_input.last_byte_ms > 2 || + if (timestamp_us - byte_input.last_byte_us > 2000U || byte_input.ofs == sizeof(byte_input.buf)) { byte_input.ofs = 0; } - byte_input.last_byte_ms = now; + byte_input.last_byte_us = timestamp_us; byte_input.buf[byte_input.ofs++] = b; + if (byte_input.ofs == sizeof(byte_input.buf)) { uint16_t values[SBUS_INPUT_CHANNELS]; uint16_t num_values=0; @@ -307,3 +246,12 @@ void AP_RCProtocol_SBUS::process_byte(uint8_t b, uint32_t baudrate) byte_input.ofs = 0; } } + +// support byte input +void AP_RCProtocol_SBUS::process_byte(uint8_t b, uint32_t baudrate) +{ + if (baudrate != 100000) { + return; + } + _process_byte(AP_HAL::micros(), b); +} diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.h b/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.h index 7f23203114..cec5aff7df 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.h @@ -18,23 +18,25 @@ #pragma once #include "AP_RCProtocol.h" +#include "SoftSerial.h" class AP_RCProtocol_SBUS : public AP_RCProtocol_Backend { public: - AP_RCProtocol_SBUS(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend) {} + AP_RCProtocol_SBUS(AP_RCProtocol &_frontend, bool inverted); void process_pulse(uint32_t width_s0, uint32_t width_s1) override; void process_byte(uint8_t byte, uint32_t baudrate) override; private: + void _process_byte(uint32_t timestamp_us, uint8_t byte); bool sbus_decode(const uint8_t frame[25], uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values); - struct { - uint16_t bytes[25]; // including start bit, parity and stop bits - uint16_t bit_ofs; - } sbus_state; + + bool inverted; + SoftSerial ss{100000, SoftSerial::SERIAL_CONFIG_8E2I}; + uint32_t saved_width; struct { uint8_t buf[25]; uint8_t ofs; - uint32_t last_byte_ms; + uint32_t last_byte_us; } byte_input; }; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SBUS_NI.h b/libraries/AP_RCProtocol/AP_RCProtocol_SBUS_NI.h deleted file mode 100644 index d6d39a4a73..0000000000 --- a/libraries/AP_RCProtocol/AP_RCProtocol_SBUS_NI.h +++ /dev/null @@ -1,32 +0,0 @@ -/* - * This file is free software: you can redistribute it and/or modify it - * under the terms of the GNU General Public License as published by the - * Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This file is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - * See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program. If not, see . - * - */ - -#pragma once - -#include "AP_RCProtocol.h" -#include "AP_RCProtocol_SBUS.h" - -class AP_RCProtocol_SBUS_NI : public AP_RCProtocol_SBUS { -public: - AP_RCProtocol_SBUS_NI(AP_RCProtocol &_frontend) : AP_RCProtocol_SBUS(_frontend), saved_width(0) {} - void process_pulse(uint32_t width_s0, uint32_t width_s1) override - { - AP_RCProtocol_SBUS::process_pulse(saved_width, width_s0); - saved_width = width_s1; - } -private: - uint32_t saved_width; -};