AP_Common: added double methods for SITL

libraries/AP_Common/Location.cpp

@@ -253,22 +253,34 @@ Vector3d Location::get_distance_NED_double(const Location &loc2) const
                     (alt - loc2.alt) * 0.01);
 }
 
+Vector2d Location::get_distance_NE_double(const Location &loc2) const
+{
+    return Vector2d((loc2.lat - lat) * double(LOCATION_SCALING_FACTOR),
+                    diff_longitude(loc2.lng,lng) * double(LOCATION_SCALING_FACTOR) * longitude_scale());
+}
+
+Vector2F Location::get_distance_NE_ftype(const Location &loc2) const
+{
+    return Vector2F((loc2.lat - lat) * ftype(LOCATION_SCALING_FACTOR),
+                    diff_longitude(loc2.lng,lng) * ftype(LOCATION_SCALING_FACTOR) * longitude_scale());
+}
+
 // extrapolate latitude/longitude given distances (in meters) north and east
 void Location::offset(float ofs_north, float ofs_east)
 {
     const int32_t dlat = ofs_north * LOCATION_SCALING_FACTOR_INV;
-    const int32_t dlng = (ofs_east * LOCATION_SCALING_FACTOR_INV) / longitude_scale();
+    const int64_t dlng = (ofs_east * LOCATION_SCALING_FACTOR_INV) / longitude_scale();
     lat += dlat;
-    lng += dlng;
-    lng = wrap_longitude(lng);
+    lat = limit_lattitude(lat);
+    lng = wrap_longitude(dlng+lng);
 }
 
 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;
+    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);
 }
 
 /*
@@ -393,14 +405,14 @@ float Location::line_path_proportion(const Location &point1, const Location &poi
 /*
   wrap longitude for -180e7 to 180e7
  */
-int32_t Location::wrap_longitude(int32_t lon)
+int32_t Location::wrap_longitude(int64_t lon)
 {
     if (lon > 1800000000L) {
-        lon = int32_t(int64_t(lon)-3600000000LL);
+        lon = int32_t(lon-3600000000LL);
     } else if (lon < -1800000000L) {
-        lon = int32_t(int64_t(lon)+3600000000LL);
+        lon = int32_t(lon+3600000000LL);
     }
-    return lon;
+    return int32_t(lon);
 }
 
 /*
@@ -420,3 +432,16 @@ int32_t Location::diff_longitude(int32_t lon1, int32_t lon2)
     }
     return int32_t(dlon);
 }
+
+/*
+  limit lattitude to -90e7 to 90e7
+ */
+int32_t Location::limit_lattitude(int32_t lat)
+{
+    if (lat > 900000000L) {
+        lat = 1800000000LL - lat;
+    } else if (lat < -900000000L) {
+        lat = -(1800000000LL + lat);
+    }
+    return lat;
+}

libraries/AP_Common/Location.h

@@ -68,6 +68,8 @@ public:
 
     // return the distance in meters in North/East plane as a N/E vector to loc2
     Vector2f get_distance_NE(const Location &loc2) const;
+    Vector2d get_distance_NE_double(const Location &loc2) const;
+    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);
@@ -122,8 +124,11 @@ public:
     bool initialised() const { return (lat !=0 || lng != 0 || alt != 0); }
 
     // wrap longitude at -180e7 to 180e7
-    static int32_t wrap_longitude(int32_t lon);
+    static int32_t wrap_longitude(int64_t lon);
 
+    // limit lattitude to -90e7 to 90e7
+    static int32_t limit_lattitude(int32_t lat);
+    
     // get lon1-lon2, wrapping at -180e7 to 180e7
     static int32_t diff_longitude(int32_t lon1, int32_t lon2);
     

libraries/AP_Common/tests/test_location.cpp

@@ -127,6 +127,33 @@ TEST(Location, LatLngWrapping)
     }
 }
 
+TEST(Location, LocOffsetDouble)
+{
+    struct {
+        int32_t home_lat;
+        int32_t home_lng;
+        Vector2d delta_metres_ne1;
+        Vector2d delta_metres_ne2;
+        Vector2d expected_pos_change;
+    } tests[] {
+               -353632620, 1491652373,
+               Vector2d{4682795.4576701336, 5953662.7673837934},
+               Vector2d{4682797.1904749088, 5953664.1586009059},
+               Vector2d{1.7365739867091179,1.7025913001452864},
+    };
+
+    for (auto &test : tests) {
+        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);
+        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);
+    }
+}
+
 TEST(Location, Tests)
 {
     Location test_location;

libraries/AP_Common/tests/wscript

@@ -5,3 +5,8 @@ def build(bld):
     bld.ap_find_tests(
         use='ap',
     )
+
+# location test needs double precision
+def configure(cfg):
+    cfg.env.DOUBLE_PRECISION_SOURCES['AP_Common'] = ['tests/test_location.cpp']
+