From 83dc7dbc92f388162cc7af73f643c5e1e75a08f0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 15 Apr 2013 14:28:51 +1000 Subject: [PATCH] AP_Math: handle NaN in constrain(), returning average this makes it less likely a NaN will propogate --- libraries/AP_Math/AP_Math.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Math/AP_Math.cpp b/libraries/AP_Math/AP_Math.cpp index 3147c9154c..4d6f6655ff 100644 --- a/libraries/AP_Math/AP_Math.cpp +++ b/libraries/AP_Math/AP_Math.cpp @@ -70,7 +70,15 @@ enum Rotation rotation_combination(enum Rotation r1, enum Rotation r2, bool *fou #endif // 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))); }