From a8844a84286213d55aacac747a5cbd8f38547272 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 22 Jul 2021 12:34:43 +1000
Subject: [PATCH] AP_Common: added ftype precision offset call

# Conflicts:
#	libraries/AP_Common/Location.cpp
#	libraries/AP_Common/Location.h
---
 libraries/AP_Common/Location.cpp            | 34 +++++++++++++++------
 libraries/AP_Common/Location.h              |  8 ++---
 libraries/AP_Common/tests/test_location.cpp |  4 +--
 3 files changed, 30 insertions(+), 16 deletions(-)

diff --git a/libraries/AP_Common/Location.cpp b/libraries/AP_Common/Location.cpp
index 1661ec3c68..9ec1d01d9f 100644
--- a/libraries/AP_Common/Location.cpp
+++ b/libraries/AP_Common/Location.cpp
@@ -47,7 +47,23 @@ Location::Location(const Vector3f &ekf_offset_neu, AltFrame frame)
     if (AP::ahrs().get_origin(ekf_origin)) {
         lat = ekf_origin.lat;
         lng = ekf_origin.lng;
-        offset(ekf_offset_neu.x / 100.0f, ekf_offset_neu.y / 100.0f);
+        offset(ekf_offset_neu.x * 0.01, ekf_offset_neu.y * 0.01);
+    }
+}
+
+Location::Location(const Vector3d &ekf_offset_neu, AltFrame frame)
+{
+    zero();
+
+    // store alt and alt frame
+    set_alt_cm(ekf_offset_neu.z, frame);
+
+    // calculate lat, lon
+    Location ekf_origin;
+    if (AP::ahrs().get_origin(ekf_origin)) {
+        lat = ekf_origin.lat;
+        lng = ekf_origin.lng;
+        offset(ekf_offset_neu.x * 0.01, ekf_offset_neu.y * 0.01);
     }
 }
 
@@ -266,7 +282,7 @@ Vector2F Location::get_distance_NE_ftype(const Location &loc2) const
 }
 
 // extrapolate latitude/longitude given distances (in meters) north and east
-void Location::offset(float ofs_north, float ofs_east)
+void Location::offset_latlng(int32_t &lat, int32_t &lng, ftype ofs_north, ftype ofs_east)
 {
     const int32_t dlat = ofs_north * LOCATION_SCALING_FACTOR_INV;
     const int64_t dlng = (ofs_east * LOCATION_SCALING_FACTOR_INV) / longitude_scale();
@@ -275,12 +291,10 @@ void Location::offset(float ofs_north, float ofs_east)
     lng = wrap_longitude(dlng+lng);
 }
 
-void Location::offset_double(double ofs_north, double ofs_east)
+// extrapolate latitude/longitude given distances (in meters) north and east
+void Location::offset(ftype ofs_north, ftype ofs_east)
 {
-    const int64_t dlat = ofs_north * double(LOCATION_SCALING_FACTOR_INV);
-    const int64_t dlng = (ofs_east * double(LOCATION_SCALING_FACTOR_INV)) / longitude_scale();
-    lat = limit_lattitude(int64_t(lat)+dlat);
-    lng = wrap_longitude(int64_t(lng)+dlng);
+    offset_latlng(lat, lng, ofs_north, ofs_east);
 }
 
 /*
@@ -308,10 +322,10 @@ void Location::offset_bearing_and_pitch(float bearing_deg, float pitch_deg, floa
 }
 
 
-float Location::longitude_scale() const
+ftype Location::longitude_scale(int32_t lat)
 {
-    float scale = cosf(lat * (1.0e-7f * DEG_TO_RAD));
-    return MAX(scale, 0.01f);
+    ftype scale = cosF(lat * (1.0e-7 * DEG_TO_RAD));
+    return MAX(scale, 0.01);
 }
 
 /*
diff --git a/libraries/AP_Common/Location.h b/libraries/AP_Common/Location.h
index 3a2a809b3a..572dfad57c 100644
--- a/libraries/AP_Common/Location.h
+++ b/libraries/AP_Common/Location.h
@@ -72,8 +72,8 @@ public:
     Vector2F get_distance_NE_ftype(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);
+    static void offset_latlng(int32_t &lat, int32_t &lng, ftype ofs_north, ftype ofs_east);
+    void offset(ftype ofs_north, ftype ofs_east);
 
     // extrapolate latitude/longitude given bearing and distance
     void offset_bearing(float bearing_deg, float distance);
@@ -85,7 +85,7 @@ public:
     // shrinking longitude as you move north or south from the equator
     // Note: this does not include the scaling to convert
     // longitude/latitude points to meters or centimeters
-    float longitude_scale() const;
+    static ftype longitude_scale(int32_t lat);
 
     bool is_zero(void) const WARN_IF_UNUSED;
 
@@ -131,7 +131,7 @@ public:
     
     // get lon1-lon2, wrapping at -180e7 to 180e7
     static int32_t diff_longitude(int32_t lon1, int32_t lon2);
-    
+
 private:
     static AP_Terrain *_terrain;
 
diff --git a/libraries/AP_Common/tests/test_location.cpp b/libraries/AP_Common/tests/test_location.cpp
index 9152bf6311..f252d6f0c5 100644
--- a/libraries/AP_Common/tests/test_location.cpp
+++ b/libraries/AP_Common/tests/test_location.cpp
@@ -146,8 +146,8 @@ TEST(Location, LocOffsetDouble)
         Location home{test.home_lat, test.home_lng, 0, Location::AltFrame::ABOVE_HOME};
         Location loc1 = home;
         Location loc2 = home;
-        loc1.offset_double(test.delta_metres_ne1.x, test.delta_metres_ne1.y);
-        loc2.offset_double(test.delta_metres_ne2.x, test.delta_metres_ne2.y);
+        loc1.offset(test.delta_metres_ne1.x, test.delta_metres_ne1.y);
+        loc2.offset(test.delta_metres_ne2.x, test.delta_metres_ne2.y);
         Vector2d diff = loc1.get_distance_NE_double(loc2);
         EXPECT_FLOAT_EQ(diff.x, test.expected_pos_change.x);
         EXPECT_FLOAT_EQ(diff.y, test.expected_pos_change.y);