From 0669619ceac793752e04a0f0bc96cf6f7beee44c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 15 Nov 2018 17:17:34 +1100 Subject: [PATCH] AP_RCProtocol: fixed DSM parser for 8 channels this ensures we remember channels from the previous frame, avoiding a bug with more than 7 channels --- libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp | 8 +++----- libraries/AP_RCProtocol/AP_RCProtocol_DSM.h | 6 +++++- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp index 118b6f85a3..682c21a630 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp @@ -81,11 +81,9 @@ void AP_RCProtocol_DSM::process_pulse(uint32_t width_s0, uint32_t width_s1) } bytes[i] = ((v>>1) & 0xFF); } - uint16_t values[8]; - uint16_t num_values=0; - if (dsm_decode(AP_HAL::micros64(), bytes, values, &num_values, 8) && - num_values >= MIN_RCIN_CHANNELS) { - add_input(num_values, values, false); + if (dsm_decode(AP_HAL::micros64(), bytes, last_values, &num_channels, AP_DSM_MAX_CHANNELS) && + num_channels >= MIN_RCIN_CHANNELS) { + add_input(num_channels, last_values, false); } } memset(&dsm_state, 0, sizeof(dsm_state)); diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h index 0900750f18..b85a312975 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h @@ -19,6 +19,8 @@ #include "AP_RCProtocol.h" +#define AP_DSM_MAX_CHANNELS 12 + class AP_RCProtocol_DSM : public AP_RCProtocol_Backend { public: AP_RCProtocol_DSM(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend) {} @@ -50,5 +52,7 @@ private: BIND_STATE4, } bind_state; uint32_t bind_last_ms; - + + uint16_t last_values[AP_DSM_MAX_CHANNELS]; + uint16_t num_channels; };