Sub: fixes for use of longitude_scale()

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),

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
 

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);
 

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