AP_Math: added a safe_asin() function

this adds range checking to asin()

libraries/AP_Math/AP_Math.cpp

@@ -0,0 +1,18 @@
+#include "AP_Math.h"
+
+// a varient of asin() that checks the input ranges and ensures a
+// valid angle as output. If nan is given as input then zero is
+// returned.
+float safe_asin(float v)
+{
+	if (isnan(v)) {
+		return 0;
+	}
+	if (v >= 1.0) {
+		return PI/2;
+	}
+	if (v <= -1.0) {
+		return -PI/2;
+	}
+	return asin(v);
+}

libraries/AP_Math/AP_Math.h

@@ -12,3 +12,6 @@
 // define AP_Param types AP_Vector3f and Ap_Matrix3f
 AP_PARAMDEFV(Matrix3f, Matrix3f, AP_PARAM_MATRIX3F);
 AP_PARAMDEFV(Vector3f, Vector3f, AP_PARAM_VECTOR3F);
+
+// a varient of asin() that always gives a valid answer.
+float safe_asin(float v);