From b1845ed00db559a1c5d618a49cd09581b6712ed4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 7 Oct 2014 13:24:23 +1100 Subject: [PATCH] HAL_Linux: initial support for parallel SBUS and PPM-SUM decoding --- libraries/AP_HAL_Linux/RCInput.cpp | 112 +++++++++++++++++++++++-- libraries/AP_HAL_Linux/RCInput.h | 11 ++- libraries/AP_HAL_Linux/RCInput_PRU.cpp | 2 +- 3 files changed, 118 insertions(+), 7 deletions(-) diff --git a/libraries/AP_HAL_Linux/RCInput.cpp b/libraries/AP_HAL_Linux/RCInput.cpp index c5d42d2409..8d46e65766 100644 --- a/libraries/AP_HAL_Linux/RCInput.cpp +++ b/libraries/AP_HAL_Linux/RCInput.cpp @@ -15,6 +15,7 @@ #include #include "RCInput.h" +#include "sbus.h" using namespace Linux; @@ -95,7 +96,7 @@ void LinuxRCInput::clear_overrides() /* - process a pulse of the given width + process a PPM-sum pulse of the given width */ void LinuxRCInput::_process_ppmsum_pulse(uint16_t width_usec) { @@ -114,11 +115,18 @@ void LinuxRCInput::_process_ppmsum_pulse(uint16_t width_usec) return; } - // take a reading for the current channel - _pulse_capt[_channel_counter] = width_usec; + /* + we limit inputs to between 700usec and 2300usec. This allows us + to decode SBUS on the same pin, as SBUS will have a maximum + pulse width of 100usec + */ + if (width_usec > 700 && width_usec < 2300) { + // take a reading for the current channel + _pulse_capt[_channel_counter] = width_usec; - // move to next channel - _channel_counter++; + // move to next channel + _channel_counter++; + } // if we have reached the maximum supported channels then // mark as unsynchronised, so we wait for a wide pulse @@ -129,4 +137,98 @@ void LinuxRCInput::_process_ppmsum_pulse(uint16_t width_usec) } } +/* + process a SBUS input pulse of the given width + */ +void LinuxRCInput::_process_sbus_pulse(uint16_t width_s0, uint16_t width_s1) +{ + // convert to bit widths, allowing for up to 1usec error, assuming 100000 bps + uint16_t bits_s0 = (width_s0+1) / 10; + uint16_t bits_s1 = (width_s1+1) / 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; + } + + // pull in the high bits + sbus_state.bytes[byte_ofs] |= ((1U< 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<>9) { + goto reset; + } + bytes[i] = ((v>>1) & 0xFF); + } + uint16_t values[LINUX_RC_INPUT_NUM_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, + LINUX_RC_INPUT_NUM_CHANNELS)) { + for (i=0; i 12) { + // break + goto reset; + } + return; +reset: + memset(&sbus_state, 0, sizeof(sbus_state)); +} + +/* + process a RC input pulse of the given width + */ +void LinuxRCInput::_process_rc_pulse(uint16_t width_s0, uint16_t width_s1) +{ + // treat as PPM-sum + _process_ppmsum_pulse(width_s0 + width_s1); + + // treat as SBUS + _process_sbus_pulse(width_s0, width_s1); +} + #endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_HAL_Linux/RCInput.h b/libraries/AP_HAL_Linux/RCInput.h index d1110a44e2..a745b8321d 100644 --- a/libraries/AP_HAL_Linux/RCInput.h +++ b/libraries/AP_HAL_Linux/RCInput.h @@ -24,7 +24,7 @@ public: virtual void _timer_tick() {} protected: - void _process_ppmsum_pulse(uint16_t width_usec); + void _process_rc_pulse(uint16_t width_s0, uint16_t width_s1); private: volatile bool new_rc_input; @@ -35,8 +35,17 @@ public: // the channel we will receive input from next, or -1 when not synchronised int8_t _channel_counter; + void _process_ppmsum_pulse(uint16_t width); + void _process_sbus_pulse(uint16_t width_s0, uint16_t width_s1); + /* override state */ uint16_t _override[LINUX_RC_INPUT_NUM_CHANNELS]; + + // state of SBUS bit decoder + struct { + uint16_t bytes[25]; // including start bit, parity and stop bits + uint16_t bit_ofs; + } sbus_state; }; #include "RCInput_PRU.h" diff --git a/libraries/AP_HAL_Linux/RCInput_PRU.cpp b/libraries/AP_HAL_Linux/RCInput_PRU.cpp index 303d0a4b16..fb36d75694 100644 --- a/libraries/AP_HAL_Linux/RCInput_PRU.cpp +++ b/libraries/AP_HAL_Linux/RCInput_PRU.cpp @@ -49,7 +49,7 @@ void LinuxRCInput_PRU::_timer_tick() } else { // the pulse value is the sum of the time spent in the low // and high states - _process_ppmsum_pulse(ring_buffer->buffer[ring_buffer->ring_head].delta_t + _s0_time); + _process_rc_pulse(_s0_time, ring_buffer->buffer[ring_buffer->ring_head].delta_t); } // move to the next ring buffer entry ring_buffer->ring_head = (ring_buffer->ring_head + 1) % NUM_RING_ENTRIES;