From c059f8c044a214f563504c5775ebd827fa42eede Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 1 Jul 2021 09:19:05 +1000
Subject: [PATCH] Sub: fixes for use of longitude_scale()

---
 ArduSub/Sub.cpp             | 1 -
 ArduSub/Sub.h               | 4 ----
 ArduSub/commands.cpp        | 2 --
 ArduSub/position_vector.cpp | 5 ++++-
 4 files changed, 4 insertions(+), 8 deletions(-)

diff --git a/ArduSub/Sub.cpp b/ArduSub/Sub.cpp
index 3dc96038f7..c55a6cd1a3 100644
--- a/ArduSub/Sub.cpp
+++ b/ArduSub/Sub.cpp
@@ -27,7 +27,6 @@ Sub::Sub()
     : logger(g.log_bitmask),
           control_mode(MANUAL),
           motors(MAIN_LOOP_RATE),
-          scaleLongDown(1),
           auto_mode(Auto_WP),
           guided_mode(Guided_WP),
           auto_yaw_mode(AUTO_YAW_LOOK_AT_NEXT_WP),
diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h
index 68f6996184..327e8decb1 100644
--- a/ArduSub/Sub.h
+++ b/ArduSub/Sub.h
@@ -255,10 +255,6 @@ private:
 
     AP_Motors6DOF motors;
 
-    // GPS variables
-    // Sometimes we need to remove the scaling for distance calcs
-    float scaleLongDown;
-
     // Auto
     AutoMode auto_mode;   // controls which auto controller is run
 
diff --git a/ArduSub/commands.cpp b/ArduSub/commands.cpp
index daf7ca5dd8..4085a20a6e 100644
--- a/ArduSub/commands.cpp
+++ b/ArduSub/commands.cpp
@@ -70,8 +70,6 @@ bool Sub::set_home(const Location& loc, bool lock)
 
     // init inav and compass declination
     if (!home_was_set) {
-        // update navigation scalers.  used to offset the shrinking longitude as we go towards the poles
-        scaleLongDown = loc.longitude_scale();
         // record home is set
         AP::logger().Write_Event(LogEvent::SET_HOME);
 
diff --git a/ArduSub/position_vector.cpp b/ArduSub/position_vector.cpp
index 44b61aa556..c71a8247d6 100644
--- a/ArduSub/position_vector.cpp
+++ b/ArduSub/position_vector.cpp
@@ -15,7 +15,10 @@ Vector3f Sub::pv_location_to_vector(const Location& loc)
         origin.zero();
     }
     float alt_above_origin = pv_alt_above_origin(loc.alt);  // convert alt-relative-to-home to alt-relative-to-origin
-    return Vector3f((loc.lat-origin.lat) * LATLON_TO_CM, Location::diff_longitude(loc.lng,origin.lng) * LATLON_TO_CM * scaleLongDown, alt_above_origin);
+    Vector3f vec = origin.get_distance_NED(loc);
+    vec.xy() *= 100;
+    vec.z = alt_above_origin;
+    return vec;
 }
 
 // pv_alt_above_origin - convert altitude above home to altitude above EKF origin