From afb928081a862250efc866814c457fb5096f048f Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 9 Jul 2021 12:20:50 +1000
Subject: [PATCH] AP_Math: use ftype for a few internal trig fns

---
 libraries/AP_Math/AP_Math.h      |  6 +++---
 libraries/AP_Math/definitions.h  |  2 +-
 libraries/AP_Math/matrix3.cpp    |  8 ++++----
 libraries/AP_Math/quaternion.cpp | 20 ++++++++++----------
 4 files changed, 18 insertions(+), 18 deletions(-)

diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h
index e00a8f9056..c8b093bee7 100644
--- a/libraries/AP_Math/AP_Math.h
+++ b/libraries/AP_Math/AP_Math.h
@@ -143,13 +143,13 @@ double wrap_360_cd(const double angle);
   wrap an angle in radians to -PI ~ PI (equivalent to +- 180 degrees)
  */
 template <typename T>
-float wrap_PI(const T radian);
+ftype wrap_PI(const T radian);
 
 /*
  * wrap an angle in radians to 0..2PI
  */
 template <typename T>
-float wrap_2PI(const T radian);
+ftype wrap_2PI(const T radian);
 
 /*
  * Constrain a value to be within the range: low and high
@@ -179,7 +179,7 @@ inline int64_t constrain_int64(const int64_t amt, const int64_t low, const int64
 }
 
 // degrees -> radians
-static inline constexpr float radians(float deg)
+static inline constexpr ftype radians(ftype deg)
 {
     return deg * DEG_TO_RAD;
 }
diff --git a/libraries/AP_Math/definitions.h b/libraries/AP_Math/definitions.h
index 9465641dfb..0eb5250833 100644
--- a/libraries/AP_Math/definitions.h
+++ b/libraries/AP_Math/definitions.h
@@ -7,7 +7,7 @@
 #ifdef M_PI
 # undef M_PI
 #endif
-#define M_PI      (3.141592653589793f)
+#define M_PI      (3.141592653589793)
 
 #ifdef M_PI_2
 # undef M_PI_2
diff --git a/libraries/AP_Math/matrix3.cpp b/libraries/AP_Math/matrix3.cpp
index ed16beaff2..bd64d62a02 100644
--- a/libraries/AP_Math/matrix3.cpp
+++ b/libraries/AP_Math/matrix3.cpp
@@ -52,10 +52,10 @@ void Matrix3<T>::to_euler(T *roll, T *pitch, T *yaw) const
         *pitch = -safe_asin(c.x);
     }
     if (roll != nullptr) {
-        *roll = atan2f(c.y, c.z);
+        *roll = atan2F(c.y, c.z);
     }
     if (yaw != nullptr) {
-        *yaw = atan2f(b.x, a.x);
+        *yaw = atan2F(b.x, a.x);
     }
 }
 
@@ -81,8 +81,8 @@ template <typename T>
 Vector3<T> Matrix3<T>::to_euler312() const
 {
     return Vector3<T>(asinF(c.y),
-                      atan2f(-c.x, c.z),
-                      atan2f(-a.y, b.y));
+                      atan2F(-c.x, c.z),
+                      atan2F(-a.y, b.y));
 }
 
 /*
diff --git a/libraries/AP_Math/quaternion.cpp b/libraries/AP_Math/quaternion.cpp
index 98a376a59b..2cc0fc70c3 100644
--- a/libraries/AP_Math/quaternion.cpp
+++ b/libraries/AP_Math/quaternion.cpp
@@ -481,9 +481,9 @@ void QuaternionT<T>::from_axis_angle(const Vector3<T> &axis, T theta)
         q2=q3=q4=0.0f;
         return;
     }
-    const T st2 = sinF(theta/2.0f);
+    const T st2 = sinF(0.5*theta);
 
-    q1 = cosF(theta/2.0f);
+    q1 = cosF(0.5*theta);
     q2 = axis.x * st2;
     q3 = axis.y * st2;
     q4 = axis.z * st2;
@@ -507,7 +507,7 @@ void QuaternionT<T>::to_axis_angle(Vector3<T> &v) const
     v = Vector3<T>(q2,q3,q4);
     if (!is_zero(l)) {
         v /= l;
-        v *= wrap_PI(2.0f * atan2f(l,q1));
+        v *= wrap_PI(2.0f * atan2F(l,q1));
     }
 }
 
@@ -531,11 +531,11 @@ void QuaternionT<T>::from_axis_angle_fast(Vector3<T> v)
 template <typename T>
 void QuaternionT<T>::from_axis_angle_fast(const Vector3<T> &axis, T theta)
 {
-    const T t2 = theta/2.0f;
+    const T t2 = 0.5*theta;
     const T sqt2 = sq(t2);
     const T st2 = t2-sqt2*t2/6.0f;
 
-    q1 = 1.0f-(sqt2/2.0f)+sq(sqt2)/24.0f;
+    q1 = 1.0f-(0.5*sqt2)+sq(sqt2)/24.0f;
     q2 = axis.x * st2;
     q3 = axis.y * st2;
     q4 = axis.z * st2;
@@ -550,13 +550,13 @@ void QuaternionT<T>::rotate_fast(const Vector3<T> &v)
     if (is_zero(theta)) {
         return;
     }
-    const T t2 = theta/2.0f;
+    const T t2 = 0.5*theta;
     const T sqt2 = sq(t2);
     T st2 = t2-sqt2*t2/6.0f;
     st2 /= theta;
 
     //"rotation quaternion"
-    const T w2 = 1.0f-(sqt2/2.0f)+sq(sqt2)/24.0f;
+    const T w2 = 1.0f-(0.5*sqt2)+sq(sqt2)/24.0f;
     const T x2 = v.x * st2;
     const T y2 = v.y * st2;
     const T z2 = v.z * st2;
@@ -578,7 +578,7 @@ void QuaternionT<T>::rotate_fast(const Vector3<T> &v)
 template <typename T>
 T QuaternionT<T>::get_euler_roll() const
 {
-    return (atan2f(2.0f*(q1*q2 + q3*q4), 1.0f - 2.0f*(q2*q2 + q3*q3)));
+    return (atan2F(2.0f*(q1*q2 + q3*q4), 1.0f - 2.0f*(q2*q2 + q3*q3)));
 }
 
 // get euler pitch angle
@@ -592,7 +592,7 @@ T QuaternionT<T>::get_euler_pitch() const
 template <typename T>
 T QuaternionT<T>::get_euler_yaw() const
 {
-    return atan2f(2.0f*(q1*q4 + q2*q3), 1.0f - 2.0f*(q3*q3 + q4*q4));
+    return atan2F(2.0f*(q1*q4 + q2*q3), 1.0f - 2.0f*(q3*q3 + q4*q4));
 }
 
 // create eulers from a quaternion
@@ -772,7 +772,7 @@ T QuaternionT<T>::roll_pitch_difference(const QuaternionT<T> &v) const
     const T vec_len_div2 = constrain_float(vec_diff.length() * 0.5, 0.0, 1.0);
 
     // calculate and return angular difference
-    return (2.0 * asinf(vec_len_div2));
+    return (2.0 * asinF(vec_len_div2));
 }
 
 // define for float and double