mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-03-03 04:03:59 -04:00
AP_RCProtocol: switched SBUS to SoftSerial decoder
This commit is contained in:
Andrew Tridgell
parent
b7c4ae8ae9
commit
87cf160875
@ -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);
|
||||
|
||||
@ -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;
|
||||
uint32_t w0 = width_s0;
|
||||
uint32_t w1 = width_s1;
|
||||
if (inverted) {
|
||||
w0 = saved_width;
|
||||
w1 = width_s0;
|
||||
saved_width = width_s1;
|
||||
}
|
||||
|
||||
if (bits_s0+bit_ofs > 10) {
|
||||
// invalid data as last two bits must be stop bits
|
||||
goto reset;
|
||||
uint8_t b;
|
||||
if (ss.process_pulse(w0, w1, b)) {
|
||||
_process_byte(ss.get_byte_timestamp_us(), b);
|
||||
}
|
||||
|
||||
if (byte_ofs >= ARRAY_SIZE(sbus_state.bytes)) {
|
||||
goto reset;
|
||||
}
|
||||
// pull in the high bits
|
||||
sbus_state.bytes[byte_ofs] |= ((1U<<bits_s0)-1) << bit_ofs;
|
||||
sbus_state.bit_ofs += bits_s0;
|
||||
bit_ofs += bits_s0;
|
||||
|
||||
// pull in the low bits
|
||||
nlow = bits_s1;
|
||||
if (nlow + bit_ofs > 12) {
|
||||
nlow = 12 - bit_ofs;
|
||||
}
|
||||
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<<j))?1:0;
|
||||
}
|
||||
if (parity != (v&0x200)>>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);
|
||||
}
|
||||
|
||||
@ -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;
|
||||
};
|
||||
|
||||
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
*/
|
||||
|
||||
#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;
|
||||
};
|
||||
Loading…
Reference in New Issue
Block a user