diff --git a/libraries/AP_Math/AP_Math.cpp b/libraries/AP_Math/AP_Math.cpp
index 669f5aa3b1d..fbcbd175313 100644
--- a/libraries/AP_Math/AP_Math.cpp
+++ b/libraries/AP_Math/AP_Math.cpp
@@ -259,6 +259,30 @@ template float wrap_2PI<short>(const short radian);
 template float wrap_2PI<float>(const float radian);
 template float wrap_2PI<double>(const double radian);
 
+template <typename T>
+T constrain_value_line(const T amt, const T low, const T high, uint32_t line)
+{
+    // the check for NaN as a float prevents propagation of floating point
+    // errors through any function that uses constrain_value(). The normal
+    // float semantics already handle -Inf and +Inf
+    if (isnan(amt)) {
+        AP::internalerror().error(AP_InternalError::error_t::constraining_nan, line);
+        return (low + high) / 2;
+    }
+
+    if (amt < low) {
+        return low;
+    }
+
+    if (amt > high) {
+        return high;
+    }
+
+    return amt;
+}
+
+template float constrain_value_line<float>(const float amt, const float low, const float high, uint32_t line);
+
 template <typename T>
 T constrain_value(const T amt, const T low, const T high)
 {
diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h
index 01912a34788..29199266092 100644
--- a/libraries/AP_Math/AP_Math.h
+++ b/libraries/AP_Math/AP_Math.h
@@ -144,10 +144,14 @@ float wrap_2PI(const T radian);
 template <typename T>
 T constrain_value(const T amt, const T low, const T high);
 
-inline float constrain_float(const float amt, const float low, const float high)
-{
-    return constrain_value(amt, low, high);
-}
+template <typename T>
+T constrain_value_line(const T amt, const T low, const T high, uint32_t line);
+
+#if BOARD_FLASH_SIZE > 1024
+  #define constrain_float(amt, low, high) constrain_value_line(float(amt), float(low), float(high), uint32_t(__LINE__))
+#else
+  #define constrain_float(amt, low, high) constrain_value(float(amt), float(low), float(high))
+#endif
 
 inline int16_t constrain_int16(const int16_t amt, const int16_t low, const int16_t high)
 {