diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp index bb6c746835..9d7555bebf 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp @@ -440,3 +440,25 @@ void AP_RCProtocol_DSM::update(void) } #endif } + +// support byte input +void AP_RCProtocol_DSM::process_byte(uint8_t b) +{ + uint32_t now = AP_HAL::millis(); + if (now - byte_input.last_byte_ms > 3 || + 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 == 16) { + uint16_t values[8]; + uint16_t num_values=0; + if (dsm_decode(AP_HAL::micros64(), byte_input.buf, values, &num_values, 8) && + num_values >= MIN_RCIN_CHANNELS) { + add_input(num_values, values, false); + } + + byte_input.ofs = 0; + } +} diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h index 956d5be9f0..faad1dadea 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h @@ -23,6 +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 start_bind(void) override; void update(void) override; @@ -51,4 +52,9 @@ private: } bind_state; uint32_t bind_last_ms; + struct { + uint8_t buf[16]; + uint8_t ofs; + uint32_t last_byte_ms; + } byte_input; };