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

AP_RCProtocol: make sbus_decode public to allow for a test suite

This commit is contained in:
Andrew Tridgell 2023-10-04 11:08:25 +11:00
parent 120a8c1b1d
commit ca4f26db17
3 changed files with 10 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_RCProtocol/AP_RCProtocol_Backend.h
View File

@ -122,7 +122,7 @@ protected:
void log_data(AP_RCProtocol::rcprotocol_t prot, uint32_t timestamp, const uint8_t *data, uint8_t len) const; void log_data(AP_RCProtocol::rcprotocol_t prot, uint32_t timestamp, const uint8_t *data, uint8_t len) const;
// decode channels from the standard 11bit format (used by CRSF and SBUS) // decode channels from the standard 11bit format (used by CRSF and SBUS)
void decode_11bit_channels(const uint8_t* data, uint8_t nchannels, uint16_t *values, uint16_t mult, uint16_t div, uint16_t offset); static void decode_11bit_channels(const uint8_t* data, uint8_t nchannels, uint16_t *values, uint16_t mult, uint16_t div, uint16_t offset);
private: private:
uint32_t rc_input_count; uint32_t rc_input_count;

17
libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp
View File

@ -88,7 +88,7 @@ AP_RCProtocol_SBUS::AP_RCProtocol_SBUS(AP_RCProtocol &_frontend, bool _inverted,
// decode a full SBUS frame // decode a full SBUS frame
bool AP_RCProtocol_SBUS::sbus_decode(const uint8_t frame[25], uint16_t *values, uint16_t *num_values, 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) bool &sbus_failsafe, uint16_t max_values)
{ {
/* check frame boundary markers to avoid out-of-sync cases */ /* check frame boundary markers to avoid out-of-sync cases */
if ((frame[0] != 0x0f)) { if ((frame[0] != 0x0f)) {
@ -129,11 +129,9 @@ bool AP_RCProtocol_SBUS::sbus_decode(const uint8_t frame[25], uint16_t *values,
/* decode and handle failsafe and frame-lost flags */ /* decode and handle failsafe and frame-lost flags */
if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */ if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */
/* report that we failed to read anything valid off the receiver */ /* report that we failed to read anything valid off the receiver */
*sbus_failsafe = true; sbus_failsafe = true;
*sbus_frame_drop = true;
} else if (invalid_data) { } else if (invalid_data) {
*sbus_failsafe = true; sbus_failsafe = true;
*sbus_frame_drop = false;
} else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */ } else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */
/* set a special warning flag /* set a special warning flag
* *
@ -141,11 +139,9 @@ bool AP_RCProtocol_SBUS::sbus_decode(const uint8_t frame[25], uint16_t *values,
* condition as fail-safe greatly reduces the reliability and range of the radio link, * condition as fail-safe greatly reduces the reliability and range of the radio link,
* e.g. by prematurely issuing return-to-launch!!! */ * e.g. by prematurely issuing return-to-launch!!! */
*sbus_failsafe = false; sbus_failsafe = false;
*sbus_frame_drop = true;
} else { } else {
*sbus_failsafe = false; sbus_failsafe = false;
*sbus_frame_drop = false;
} }
return true; return true;
@ -197,9 +193,8 @@ void AP_RCProtocol_SBUS::_process_byte(uint32_t timestamp_us, uint8_t b)
uint16_t values[SBUS_INPUT_CHANNELS]; uint16_t values[SBUS_INPUT_CHANNELS];
uint16_t num_values=0; uint16_t num_values=0;
bool sbus_failsafe = false; bool sbus_failsafe = false;
bool sbus_frame_drop = false;
if (sbus_decode(byte_input.buf, values, &num_values, if (sbus_decode(byte_input.buf, values, &num_values,
&sbus_failsafe, &sbus_frame_drop, SBUS_INPUT_CHANNELS) && sbus_failsafe, SBUS_INPUT_CHANNELS) &&
num_values >= MIN_RCIN_CHANNELS) { num_values >= MIN_RCIN_CHANNELS) {
add_input(num_values, values, sbus_failsafe); add_input(num_values, values, sbus_failsafe);
} }

5
libraries/AP_RCProtocol/AP_RCProtocol_SBUS.h
View File

@ -30,10 +30,11 @@ public:
void process_pulse(uint32_t width_s0, uint32_t width_s1) override; void process_pulse(uint32_t width_s0, uint32_t width_s1) override;
void process_byte(uint8_t byte, uint32_t baudrate) override; void process_byte(uint8_t byte, uint32_t baudrate) override;
static bool sbus_decode(const uint8_t frame[25], uint16_t *values, uint16_t *num_values,
bool &sbus_failsafe, uint16_t max_values);
private: private:
void _process_byte(uint32_t timestamp_us, uint8_t byte); 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);
bool inverted; bool inverted;
SoftSerial ss; SoftSerial ss;