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!
This commit is contained in:
Andrew Tridgell 2020-07-31 18:37:30 +10:00
parent 5ed5631c3c
commit 92e35f6a9e

View File

@ -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;
}