AP_Math: create and use INTERNAL_ERROR macro so we get line numbers

libraries/AP_Math/AP_Math.cpp

@@ -266,7 +266,7 @@ T constrain_value(const T amt, const T low, const T high)
     // 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);
+        INTERNAL_ERROR(AP_InternalError::error_t::constraining_nan);
         return (low + high) / 2;
     }
 

libraries/AP_Math/quaternion.cpp

@@ -368,14 +368,14 @@ void Quaternion::from_rotation(enum Rotation rotation)
 
     case ROTATION_CUSTOM:
         // Error; custom rotations not supported
-        AP::internalerror().error(AP_InternalError::error_t::flow_of_control);
+        INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
         return;
 
     case ROTATION_MAX:
         break;
     }
     // rotation invalid
-    AP::internalerror().error(AP_InternalError::error_t::bad_rotation);
+    INTERNAL_ERROR(AP_InternalError::error_t::bad_rotation);
 }
 
 // rotate this quaternion by the given rotation

libraries/AP_Math/vector3.cpp

@@ -253,13 +253,13 @@ void Vector3<T>::rotate(enum Rotation rotation)
     }
     case ROTATION_CUSTOM: 
         // Error: caller must perform custom rotations via matrix multiplication
-        AP::internalerror().error(AP_InternalError::error_t::flow_of_control);
+        INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
         return;
     case ROTATION_MAX:
         break;
     }
     // rotation invalid
-    AP::internalerror().error(AP_InternalError::error_t::bad_rotation);
+    INTERNAL_ERROR(AP_InternalError::error_t::bad_rotation);
 }
 
 template <typename T>