AP_Math: Log line numbers on constrain_nan's for constrain float

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)
 {

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)
 {