diff --git a/libraries/AP_Math/AP_Math.cpp b/libraries/AP_Math/AP_Math.cpp
new file mode 100644
index 0000000000..cbe56b8142
--- /dev/null
+++ b/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);
+}
diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h
index fbff53cb3d..06eceac6a3 100644
--- a/libraries/AP_Math/AP_Math.h
+++ b/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);