Browse Source

RC_Channel: prevent float exception with bad RCn_MIN/MAX/TRIM

thanks to Michael for finding this
mission-4.1.18
Andrew Tridgell 9 years ago
parent
commit
52f2fce1d5
  1. 6
      libraries/RC_Channel/RC_Channel.cpp

6
libraries/RC_Channel/RC_Channel.cpp

@ -370,8 +370,14 @@ RC_Channel::norm_input() @@ -370,8 +370,14 @@ RC_Channel::norm_input()
float ret;
int16_t reverse_mul = (_reverse==-1?-1:1);
if (radio_in < radio_trim) {
if (radio_min >= radio_trim) {
return 0.0f;
}
ret = reverse_mul * (float)(radio_in - radio_trim) / (float)(radio_trim - radio_min);
} else {
if (radio_max <= radio_trim) {
return 0.0f;
}
ret = reverse_mul * (float)(radio_in - radio_trim) / (float)(radio_max - radio_trim);
}
return constrain_float(ret, -1.0f, 1.0f);

Loading…
Cancel
Save