Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_RCProtocol: added check on baudrate in process_byte() 

and support process_byte() in SBUS input
This commit is contained in:
Andrew Tridgell 2018-11-02 20:42:29 +11:00
parent 7c1ba15031
commit baf0be6a56
13 changed files with 67 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_RCProtocol/AP_RCProtocol.cpp
View File

@ -56,7 +56,13 @@ void AP_RCProtocol::process_pulse(uint32_t width_s0, uint32_t width_s1)
}
// otherwise scan all protocols
for (uint8_t i = 0; i < AP_RCProtocol::NONE; i++) {
rcprotocol_t last_protocol = AP_RCProtocol::NONE;
#ifdef HAL_RCIN_PULSE_PPM_ONLY
// only uses pulses for PPM on this board, using process_byte() for
// other protocols
last_protocol = (rcprotocol_t)1;
#endif
for (uint8_t i = 0; i < last_protocol; i++) {
if (backend[i] != nullptr) {
backend[i]->process_pulse(width_s0, width_s1);
if (backend[i]->new_input()) {
@ -69,7 +75,7 @@ void AP_RCProtocol::process_pulse(uint32_t width_s0, uint32_t width_s1)
}
}
void AP_RCProtocol::process_byte(uint8_t byte)
void AP_RCProtocol::process_byte(uint8_t byte, uint32_t baudrate)
{
if (_detected_protocol != AP_RCProtocol::NONE && !_detected_with_bytes) {
// we're using pulse inputs, discard bytes
@ -78,7 +84,7 @@ void AP_RCProtocol::process_byte(uint8_t byte)
uint32_t now = AP_HAL::millis();
// first try current protocol
if (_detected_protocol != AP_RCProtocol::NONE && now - _last_input_ms < 200) {
backend[_detected_protocol]->process_byte(byte);
backend[_detected_protocol]->process_byte(byte, baudrate);
if (backend[_detected_protocol]->new_input()) {
_new_input = true;
_last_input_ms = AP_HAL::millis();
@ -89,7 +95,7 @@ void AP_RCProtocol::process_byte(uint8_t byte)
// otherwise scan all protocols
for (uint8_t i = 0; i < AP_RCProtocol::NONE; i++) {
if (backend[i] != nullptr) {
backend[i]->process_byte(byte);
backend[i]->process_byte(byte, baudrate);
if (backend[i]->new_input()) {
_new_input = true;
_detected_protocol = (enum AP_RCProtocol::rcprotocol_t)i;

2
libraries/AP_RCProtocol/AP_RCProtocol.h
View File

@ -44,7 +44,7 @@ public:
return _valid_serial_prot;
}
void process_pulse(uint32_t width_s0, uint32_t width_s1);
void process_byte(uint8_t byte);
void process_byte(uint8_t byte, uint32_t baudrate);
enum rcprotocol_t protocol_detected()
{
return _detected_protocol;

2
libraries/AP_RCProtocol/AP_RCProtocol_Backend.h
View File

@ -25,7 +25,7 @@ class AP_RCProtocol_Backend {
public:
AP_RCProtocol_Backend(AP_RCProtocol &_frontend);
virtual void process_pulse(uint32_t width_s0, uint32_t width_s1) {}
virtual void process_byte(uint8_t byte) {}
virtual void process_byte(uint8_t byte, uint32_t baudrate) {}
uint16_t read(uint8_t chan);
bool new_input();
uint8_t num_channels();

5
libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp
View File

@ -442,8 +442,11 @@ void AP_RCProtocol_DSM::update(void)
}
// support byte input
void AP_RCProtocol_DSM::process_byte(uint8_t b)
void AP_RCProtocol_DSM::process_byte(uint8_t b, uint32_t baudrate)
{
if (baudrate != 115200) {
return;
}
uint32_t now = AP_HAL::millis();
if (now - byte_input.last_byte_ms > 3 ||
byte_input.ofs == sizeof(byte_input.buf)) {

2
libraries/AP_RCProtocol/AP_RCProtocol_DSM.h
View File

@ -23,7 +23,7 @@ class AP_RCProtocol_DSM : public AP_RCProtocol_Backend {
public:
AP_RCProtocol_DSM(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend) {}
void process_pulse(uint32_t width_s0, uint32_t width_s1) override;
void process_byte(uint8_t byte) override;
void process_byte(uint8_t byte, uint32_t baudrate) override;
void start_bind(void) override;
void update(void) override;

27
libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp
View File

@ -280,3 +280,30 @@ void AP_RCProtocol_SBUS::process_pulse(uint32_t width_s0, uint32_t width_s1)
reset:
memset(&sbus_state, 0, sizeof(sbus_state));
}
// support byte input
void AP_RCProtocol_SBUS::process_byte(uint8_t b, uint32_t baudrate)
{
if (baudrate != 100000) {
return;
}
uint32_t now = AP_HAL::millis();
if (now - byte_input.last_byte_ms > 2 ||
byte_input.ofs == sizeof(byte_input.buf)) {
byte_input.ofs = 0;
}
byte_input.last_byte_ms = now;
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;
bool sbus_failsafe = false;
bool sbus_frame_drop = false;
if (sbus_decode(byte_input.buf, values, &num_values,
&sbus_failsafe, &sbus_frame_drop, SBUS_INPUT_CHANNELS) &&
num_values >= MIN_RCIN_CHANNELS) {
add_input(num_values, values, false);
}
byte_input.ofs = 0;
}
}

9
libraries/AP_RCProtocol/AP_RCProtocol_SBUS.h
View File

@ -23,6 +23,7 @@ class AP_RCProtocol_SBUS : public AP_RCProtocol_Backend {
public:
AP_RCProtocol_SBUS(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend) {}
void process_pulse(uint32_t width_s0, uint32_t width_s1) override;
void process_byte(uint8_t byte, uint32_t baudrate) override;
private:
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);
@ -30,4 +31,10 @@ private:
uint16_t bytes[25]; // including start bit, parity and stop bits
uint16_t bit_ofs;
} sbus_state;
};
struct {
uint8_t buf[25];
uint8_t ofs;
uint32_t last_byte_ms;
} byte_input;
};

4
libraries/AP_RCProtocol/AP_RCProtocol_SRXL.cpp
View File

@ -90,7 +90,7 @@ void AP_RCProtocol_SRXL::process_pulse(uint32_t width_s0, uint32_t width_s1)
break;
}
byte = ((v>>1) & 0xFF);
process_byte(byte);
process_byte(byte, 115200);
}
memset(&srxl_state, 0, sizeof(srxl_state));
}
@ -249,7 +249,7 @@ int AP_RCProtocol_SRXL::srxl_channels_get_v5(uint16_t max_values, uint8_t *num_v
return 0;
}
void AP_RCProtocol_SRXL::process_byte(uint8_t byte)
void AP_RCProtocol_SRXL::process_byte(uint8_t byte, uint32_t baudrate)
{
uint64_t timestamp_us = AP_HAL::micros64();
/*----------------------------------------distinguish different srxl variants at the beginning of each frame---------------------------------------------- */

2
libraries/AP_RCProtocol/AP_RCProtocol_SRXL.h
View File

@ -39,7 +39,7 @@ class AP_RCProtocol_SRXL : public AP_RCProtocol_Backend {
public:
AP_RCProtocol_SRXL(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend) {}
void process_pulse(uint32_t width_s0, uint32_t width_s1) override;
void process_byte(uint8_t byte) override;
void process_byte(uint8_t byte, uint32_t baudrate) override;
private:
static uint16_t srxl_crc16(uint16_t crc, uint8_t new_byte);
int srxl_channels_get_v1v2(uint16_t max_values, uint8_t *num_values, uint16_t *values, bool *failsafe_state);

8
libraries/AP_RCProtocol/AP_RCProtocol_ST24.cpp
View File

@ -118,7 +118,7 @@ void AP_RCProtocol_ST24::process_pulse(uint32_t width_s0, uint32_t width_s1)
break;
}
byte = ((v>>1) & 0xFF);
process_byte(byte);
process_byte(byte, 115200);
}
memset(&st24_state, 0, sizeof(st24_state));
}
@ -138,9 +138,11 @@ reset:
memset(&st24_state, 0, sizeof(st24_state));
}
void AP_RCProtocol_ST24::process_byte(uint8_t byte)
void AP_RCProtocol_ST24::process_byte(uint8_t byte, uint32_t baudrate)
{
if (baudrate != 115200) {
return;
}
switch (_decode_state) {
case ST24_DECODE_STATE_UNSYNCED:

2
libraries/AP_RCProtocol/AP_RCProtocol_ST24.h
View File

@ -39,7 +39,7 @@ class AP_RCProtocol_ST24 : public AP_RCProtocol_Backend {
public:
AP_RCProtocol_ST24(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend) {}
void process_pulse(uint32_t width_s0, uint32_t width_s1) override;
void process_byte(uint8_t byte) override;
void process_byte(uint8_t byte, uint32_t baudrate) override;
private:
static uint8_t st24_crc8(uint8_t *ptr, uint8_t len);
enum ST24_PACKET_TYPE {

7
libraries/AP_RCProtocol/AP_RCProtocol_SUMD.cpp
View File

@ -125,7 +125,7 @@ void AP_RCProtocol_SUMD::process_pulse(uint32_t width_s0, uint32_t width_s1)
break;
}
byte = ((v>>1) & 0xFF);
process_byte(byte);
process_byte(byte, 115200);
}
memset(&sumd_state, 0, sizeof(sumd_state));
}
@ -145,8 +145,11 @@ reset:
memset(&sumd_state, 0, sizeof(sumd_state));
}
void AP_RCProtocol_SUMD::process_byte(uint8_t byte)
void AP_RCProtocol_SUMD::process_byte(uint8_t byte, uint32_t baudrate)
{
if (baudrate != 115200) {
return;
}
switch (_decode_state) {
case SUMD_DECODE_STATE_UNSYNCED:
#ifdef SUMD_DEBUG

2
libraries/AP_RCProtocol/AP_RCProtocol_SUMD.h
View File

@ -24,7 +24,7 @@ class AP_RCProtocol_SUMD : public AP_RCProtocol_Backend {
public:
AP_RCProtocol_SUMD(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend) {}
void process_pulse(uint32_t width_s0, uint32_t width_s1) override;
void process_byte(uint8_t byte) override;
void process_byte(uint8_t byte, uint32_t baudrate) override;
private:
static uint16_t sumd_crc16(uint16_t crc, uint8_t value);
static uint8_t sumd_crc8(uint8_t crc, uint8_t value);