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 // rotate a vector by a standard rotation, attempting // to use the minimum number of floating point operations @@ -28,7 +29,6 @@ void Vector3::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::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