From 3b8ec3a2a0788422153e5a66d92869236336a7e1 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 4 May 2018 11:19:31 +1000
Subject: [PATCH] AP_Math: allow double precision maths where needed

---
 libraries/AP_Math/definitions.h  | 4 ++++
 libraries/AP_Math/location.cpp   | 5 +++++
 libraries/AP_Math/quaternion.cpp | 2 +-
 3 files changed, 10 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_Math/definitions.h b/libraries/AP_Math/definitions.h
index e668f160b4..df17e9c287 100644
--- a/libraries/AP_Math/definitions.h
+++ b/libraries/AP_Math/definitions.h
@@ -36,8 +36,10 @@
 // GPS Specific double precision conversions
 // The precision here does matter when using the wsg* functions for converting
 // between LLH and ECEF coordinates.
+#ifdef ALLOW_DOUBLE_MATH_FUNCTIONS
 static const double DEG_TO_RAD_DOUBLE = asin(1) / 90;
 static const double RAD_TO_DEG_DOUBLE = 1 / DEG_TO_RAD_DOUBLE;
+#endif
 
 #define RadiansToCentiDegrees(x) (static_cast<float>(x) * RAD_TO_DEG * static_cast<float>(100))
 
@@ -65,7 +67,9 @@ static const double WGS84_F = ((double)1.0 / WGS84_IF);
 static const double WGS84_B = (WGS84_A * (1 - WGS84_F));
 
 // Eccentricity of the Earth
+#ifdef ALLOW_DOUBLE_MATH_FUNCTIONS
 static const double WGS84_E = (sqrt(2 * WGS84_F - WGS84_F * WGS84_F));
+#endif
 
 // air density at 15C at sea level in kg/m^3
 #define AIR_DENSITY_SEA_LEVEL    1.225f
diff --git a/libraries/AP_Math/location.cpp b/libraries/AP_Math/location.cpp
index b5de2d3d1c..cd08849521 100644
--- a/libraries/AP_Math/location.cpp
+++ b/libraries/AP_Math/location.cpp
@@ -211,6 +211,10 @@ void print_latlon(AP_HAL::BetterStream *s, int32_t lat_or_lon)
     s->printf("%ld.%07ld",(long)dec_portion,(long)frac_portion);
 }
 
+#ifdef ALLOW_DOUBLE_MATH_FUNCTIONS
+/*
+  these are not currently used. They should be moved to location_double.cpp if we do enable them in the future
+ */
 void wgsllh2ecef(const Vector3d &llh, Vector3d &ecef) {
   double d = WGS84_E * sin(llh[0]);
   double N = WGS84_A / sqrt(1 - d*d);
@@ -312,6 +316,7 @@ void wgsecef2llh(const Vector3d &ecef, Vector3d &llh) {
   llh[0] = copysign(1.0, ecef[2]) * atan(S / (e_c*C));
   llh[2] = (p*e_c*C + fabs(ecef[2])*S - WGS84_A*e_c*A_n) / sqrt(e_c*e_c*C*C + S*S);
 }
+#endif // ALLOW_DOUBLE_MATH_FUNCTIONS
 
 // return true when lat and lng are within range
 bool check_lat(float lat)
diff --git a/libraries/AP_Math/quaternion.cpp b/libraries/AP_Math/quaternion.cpp
index 6098489639..c3b1a1a9d2 100644
--- a/libraries/AP_Math/quaternion.cpp
+++ b/libraries/AP_Math/quaternion.cpp
@@ -188,7 +188,7 @@ void Quaternion::rotate(const Vector3f &v)
 
 void Quaternion::to_axis_angle(Vector3f &v)
 {
-    float l = sqrt(sq(q2)+sq(q3)+sq(q4));
+    float l = sqrtf(sq(q2)+sq(q3)+sq(q4));
     v = Vector3f(q2,q3,q4);
     if (!is_zero(l)) {
         v /= l;