AP_Math: added safe_sqrt() function

this function will never return NAN. It will return zero for negative
numbers.

libraries/AP_Math/AP_Math.cpp

@@ -6,7 +6,7 @@
 float safe_asin(float v)
 {
 	if (isnan(v)) {
-		return 0;
+		return 0.0;
 	}
 	if (v >= 1.0) {
 		return PI/2;
@@ -16,3 +16,16 @@ float safe_asin(float v)
 	}
 	return asin(v);
 }
+
+// a varient of sqrt() that checks the input ranges and ensures a
+// valid value as output. If a negative number is given then 0 is
+// returned. The reasoning is that a negative number for sqrt() in our
+// code is usually caused by small numerical rounding errors, so the
+// real input should have been zero
+float safe_sqrt(float v)
+{
+	if (isnan(v) || v <= 0.0) {
+		return 0.0;
+	}
+	return sqrt(v);
+}

libraries/AP_Math/AP_Math.h

@@ -15,3 +15,6 @@ AP_PARAMDEFV(Vector3f, Vector3f, AP_PARAM_VECTOR3F);
 
 // a varient of asin() that always gives a valid answer.
 float safe_asin(float v);
+
+// a varient of sqrt() that always gives a valid answer.
+float safe_sqrt(float v);