From c9a337efcceb86628e7693aefe56e0fe03152804 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 31 Jul 2020 18:37:30 +1000 Subject: [PATCH] RC_Channel: fixed use of radio_in out of range for angle inputs this prevents mis-calculation of the angle when RC input goes outside of the configured range. This impacted on throttle nudge in plane when reverse thrust was enabled thanks to Pompecukor for reporting! --- libraries/RC_Channel/RC_Channel.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index d1ad94d276..f3346f65eb 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -190,10 +190,14 @@ int16_t RC_Channel::pwm_to_angle_dz_trim(uint16_t _dead_zone, uint16_t _trim) co int16_t radio_trim_low = _trim - _dead_zone; int16_t reverse_mul = (reversed?-1:1); - if (radio_in > radio_trim_high && radio_max != radio_trim_high) { - return reverse_mul * ((int32_t)high_in * (int32_t)(radio_in - radio_trim_high)) / (int32_t)(radio_max - radio_trim_high); - } else if (radio_in < radio_trim_low && radio_trim_low != radio_min) { - return reverse_mul * ((int32_t)high_in * (int32_t)(radio_in - radio_trim_low)) / (int32_t)(radio_trim_low - radio_min); + + // don't allow out of range values + int16_t r_in = constrain_int16(radio_in, radio_min.get(), radio_max.get()); + + if (r_in > radio_trim_high && radio_max != radio_trim_high) { + return reverse_mul * ((int32_t)high_in * (int32_t)(r_in - radio_trim_high)) / (int32_t)(radio_max - radio_trim_high); + } else if (r_in < radio_trim_low && radio_trim_low != radio_min) { + return reverse_mul * ((int32_t)high_in * (int32_t)(r_in - radio_trim_low)) / (int32_t)(radio_trim_low - radio_min); } else { return 0; }