From 61faeb2d7f1eaad92df4e5e01ae8beea358a3574 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 22 Jun 2021 15:44:00 +1000
Subject: [PATCH] AP_Common: added double methods for SITL

---
 libraries/AP_Common/Location.cpp | 18 +++++++++++++++++-
 libraries/AP_Common/Location.h   |  2 ++
 2 files changed, 19 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_Common/Location.cpp b/libraries/AP_Common/Location.cpp
index 07fa4e2157c..f465f5469fa 100644
--- a/libraries/AP_Common/Location.cpp
+++ b/libraries/AP_Common/Location.cpp
@@ -242,7 +242,15 @@ Vector3f Location::get_distance_NED(const Location &loc2) const
 {
     return Vector3f((loc2.lat - lat) * LOCATION_SCALING_FACTOR,
                     (loc2.lng - lng) * LOCATION_SCALING_FACTOR * longitude_scale(),
-                    (alt - loc2.alt) * 0.01f);
+                    (alt - loc2.alt) * 0.01);
+}
+
+// return the distance in meters in North/East/Down plane as a N/E/D vector to loc2
+Vector3d Location::get_distance_NED_double(const Location &loc2) const
+{
+    return Vector3d((loc2.lat - lat) * double(LOCATION_SCALING_FACTOR),
+                    (loc2.lng - lng) * double(LOCATION_SCALING_FACTOR) * longitude_scale(),
+                    (alt - loc2.alt) * 0.01);
 }
 
 // extrapolate latitude/longitude given distances (in meters) north and east
@@ -254,6 +262,14 @@ void Location::offset(float ofs_north, float ofs_east)
     lng += dlng;
 }
 
+void Location::offset_double(double ofs_north, double ofs_east)
+{
+    const int32_t dlat = ofs_north * double(LOCATION_SCALING_FACTOR_INV);
+    const int32_t dlng = (ofs_east * double(LOCATION_SCALING_FACTOR_INV)) / longitude_scale();
+    lat += dlat;
+    lng += dlng;
+}
+
 /*
  *  extrapolate latitude/longitude given bearing and distance
  * Note that this function is accurate to about 1mm at a distance of
diff --git a/libraries/AP_Common/Location.h b/libraries/AP_Common/Location.h
index 00f947f4601..490d50059bd 100644
--- a/libraries/AP_Common/Location.h
+++ b/libraries/AP_Common/Location.h
@@ -64,12 +64,14 @@ public:
 
     // return the distance in meters in North/East/Down plane as a N/E/D vector to loc2
     Vector3f get_distance_NED(const Location &loc2) const;
+    Vector3d get_distance_NED_double(const Location &loc2) const;
 
     // return the distance in meters in North/East plane as a N/E vector to loc2
     Vector2f get_distance_NE(const Location &loc2) const;
 
     // extrapolate latitude/longitude given distances (in meters) north and east
     void offset(float ofs_north, float ofs_east);
+    void offset_double(double ofs_north, double ofs_east);
 
     // extrapolate latitude/longitude given bearing and distance
     void offset_bearing(float bearing_deg, float distance);