From 7d3d664ddf0c418ede6386b3a29c39424af7f109 Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Mon, 20 Apr 2020 07:45:33 -0600
Subject: [PATCH] AP_Math: generate internalError on call to Vector3<T>::rotate
 and Quaternion::from_rotation with bad rotation value

---
 libraries/AP_Math/quaternion.cpp |  8 ++++++--
 libraries/AP_Math/vector3.cpp    | 10 ++++++++--
 2 files changed, 14 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_Math/quaternion.cpp b/libraries/AP_Math/quaternion.cpp
index 82b065fb7f..7544c6a94f 100644
--- a/libraries/AP_Math/quaternion.cpp
+++ b/libraries/AP_Math/quaternion.cpp
@@ -366,12 +366,16 @@ void Quaternion::from_rotation(enum Rotation rotation)
         q3 = 0.06104854f;
         return;
 
-    case ROTATION_MAX:
     case ROTATION_CUSTOM:
-        // no-op; custom rotations not supported
+        // Error; custom rotations not supported
         AP::internalerror().error(AP_InternalError::error_t::flow_of_control);
         return;
+
+    case ROTATION_MAX:
+        break;
     }
+    // rotation invalid
+    AP::internalerror().error(AP_InternalError::error_t::bad_rotation);
 }
 
 // rotate this quaternion by the given rotation
diff --git a/libraries/AP_Math/vector3.cpp b/libraries/AP_Math/vector3.cpp
index 984f4172c8..728ea7c311 100644
--- a/libraries/AP_Math/vector3.cpp
+++ b/libraries/AP_Math/vector3.cpp
@@ -19,6 +19,7 @@
 #pragma GCC optimize("O2")
 
 #include "AP_Math.h"
+#include <AP_InternalError/AP_InternalError.h>
 
 // rotate a vector by a standard rotation, attempting
 // to use the minimum number of floating point operations
@@ -28,7 +29,6 @@ void Vector3<T>::rotate(enum Rotation rotation)
     T tmp;
     switch (rotation) {
     case ROTATION_NONE:
-    case ROTATION_MAX:
         return;
     case ROTATION_YAW_45: {
         tmp = HALF_SQRT_2*(float)(x - y);
@@ -251,9 +251,15 @@ void Vector3<T>::rotate(enum Rotation rotation)
         z = -sin_pitch * tmpx + cos_pitch * tmpz;
         return;
     }
-    case ROTATION_CUSTOM: // no-op; caller should perform custom rotations via matrix multiplication
+    case ROTATION_CUSTOM: 
+        // Error: caller must perform custom rotations via matrix multiplication
+        AP::internalerror().error(AP_InternalError::error_t::flow_of_control);
         return;
+    case ROTATION_MAX:
+        break;
     }
+    // rotation invalid
+    AP::internalerror().error(AP_InternalError::error_t::bad_rotation);
 }
 
 template <typename T>