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

thanks to Michael for finding this
This commit is contained in:
Andrew Tridgell 2015-11-30 21:53:54 +11:00
parent 6eca767db0
commit 52f2fce1d5

View File

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