AP_Math: handle NaN in constrain(), returning average

this makes it less likely a NaN will propogate

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