mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-18 22:03:56 -04:00
AP_Math: handle NaN in constrain(), returning average
this makes it less likely a NaN will propogate
This commit is contained in:
parent
b69481f74d
commit
83dc7dbc92
@ -70,7 +70,15 @@ enum Rotation rotation_combination(enum Rotation r1, enum Rotation r2, bool *fou
|
|||||||
#endif
|
#endif
|
||||||
|
|
||||||
// constrain a value
|
// constrain a value
|
||||||
float constrain(float amt, float low, float high) {
|
float constrain(float amt, float low, float high)
|
||||||
|
{
|
||||||
|
// the check for NaN as a float prevents propogation of
|
||||||
|
// floating point errors through any function that uses
|
||||||
|
// constrain(). The normal float semantics already handle -Inf
|
||||||
|
// and +Inf
|
||||||
|
if (isnan(amt)) {
|
||||||
|
return (low+high)*0.5f;
|
||||||
|
}
|
||||||
return ((amt)<(low)?(low):((amt)>(high)?(high):(amt)));
|
return ((amt)<(low)?(low):((amt)>(high)?(high):(amt)));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user