Move longitude_scale() into Location class

ArduSub/commands.cpp

@@ -71,7 +71,7 @@ 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 = longitude_scale(loc);
+        scaleLongDown = loc.longitude_scale();
         // record home is set
         Log_Write_Event(DATA_SET_HOME);
 

libraries/AP_Common/Location.cpp

@@ -188,7 +188,7 @@ bool Location::get_vector_xy_from_origin_NE(Vector2f &vec_ne) const
         return false;
     }
     vec_ne.x = (lat-ekf_origin.lat) * LATLON_TO_CM;
-    vec_ne.y = (lng-ekf_origin.lng) * LATLON_TO_CM * longitude_scale(ekf_origin);
+    vec_ne.y = (lng-ekf_origin.lng) * LATLON_TO_CM * ekf_origin.longitude_scale();
     return true;
 }
 
@@ -216,7 +216,7 @@ bool Location::get_vector_from_origin_NEU(Vector3f &vec_neu) const
 float Location::get_distance(const struct Location &loc2) const
 {
     float dlat = (float)(loc2.lat - lat);
-    float dlng = ((float)(loc2.lng - lng)) * longitude_scale(loc2);
+    float dlng = ((float)(loc2.lng - lng)) * loc2.longitude_scale();
     return norm(dlat, dlng) * LOCATION_SCALING_FACTOR;
 }
 
@@ -226,11 +226,17 @@ void Location::offset(float ofs_north, float ofs_east)
     // use is_equal() because is_zero() is a local class conflict and is_zero() in AP_Math does not belong to a class
     if (!is_equal(ofs_north, 0.0f) || !is_equal(ofs_east, 0.0f)) {
         int32_t dlat = ofs_north * LOCATION_SCALING_FACTOR_INV;
-        int32_t dlng = (ofs_east * LOCATION_SCALING_FACTOR_INV) / longitude_scale(*this);
+        int32_t dlng = (ofs_east * LOCATION_SCALING_FACTOR_INV) / longitude_scale();
         lat += dlat;
         lng += dlng;
     }
 }
 
+float Location::longitude_scale() const
+{
+    float scale = cosf(lat * (1.0e-7f * DEG_TO_RAD));
+    return constrain_float(scale, 0.01f, 1.0f);
+}
+
 // make sure we know what size the Location object is:
 assert_storage_size<Location, 16> _assert_storage_size_Location;

libraries/AP_Common/Location.h

@@ -75,6 +75,12 @@ class Location
     // extrapolate latitude/longitude given distances (in meters) north and east
     void offset(float ofs_north, float ofs_east);
 
+    // longitude_scale - returns the scaler to compensate for
+    // 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;
+
     bool is_zero(void) const;
 
     void zero(void);

libraries/AP_Math/location.cpp

@@ -25,12 +25,6 @@
 #include "location.h"
 #include "AP_Common/Location.h"
 
-float longitude_scale(const struct Location &loc)
-{
-    float scale = cosf(loc.lat * (1.0e-7f * DEG_TO_RAD));
-    return constrain_float(scale, 0.01f, 1.0f);
-}
-
 // return horizontal distance between two positions in cm
 float get_horizontal_distance_cm(const Vector3f &origin, const Vector3f &destination)
 {
@@ -41,7 +35,7 @@ float get_horizontal_distance_cm(const Vector3f &origin, const Vector3f &destina
 int32_t get_bearing_cd(const struct Location &loc1, const struct Location &loc2)
 {
     int32_t off_x = loc2.lng - loc1.lng;
-    int32_t off_y = (loc2.lat - loc1.lat) / longitude_scale(loc2);
+    const int32_t off_y = (loc2.lat - loc1.lat) / loc2.longitude_scale();
     int32_t bearing = 9000 + atan2f(-off_y, off_x) * DEGX100;
     if (bearing < 0) bearing += 36000;
     return bearing;
@@ -111,7 +105,7 @@ void location_update(struct Location &loc, float bearing, float distance)
 Vector2f location_diff(const struct Location &loc1, const struct Location &loc2)
 {
     return Vector2f((loc2.lat - loc1.lat) * LOCATION_SCALING_FACTOR,
-                    (loc2.lng - loc1.lng) * LOCATION_SCALING_FACTOR * longitude_scale(loc1));
+                    (loc2.lng - loc1.lng) * LOCATION_SCALING_FACTOR * loc1.longitude_scale());
 }
 
 /*
@@ -121,7 +115,7 @@ Vector2f location_diff(const struct Location &loc1, const struct Location &loc2)
 Vector3f location_3d_diff_NED(const struct Location &loc1, const struct Location &loc2)
 {
     return Vector3f((loc2.lat - loc1.lat) * LOCATION_SCALING_FACTOR,
-                    (loc2.lng - loc1.lng) * LOCATION_SCALING_FACTOR * longitude_scale(loc1),
+                    (loc2.lng - loc1.lng) * LOCATION_SCALING_FACTOR * loc1.longitude_scale(),
                     (loc1.alt - loc2.alt) * 0.01f);
 }
 

libraries/AP_Math/location.h

@@ -17,9 +17,6 @@
 /*
  * LOCATION
  */
-// longitude_scale - returns the scaler to compensate for 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 struct Location &loc);
 
 // return horizontal distance in centimeters between two positions
 float        get_horizontal_distance_cm(const Vector3f &origin, const Vector3f &destination);