AP_Common: use AHRS to get Locations from origin-offset

ArduCopter/mode_circle.cpp

@@ -26,9 +26,13 @@ bool ModeCircle::init(bool ignore_checks)
 
     // Check if the CIRCLE_OPTIONS parameter have roi_at_center
     if (copter.circle_nav->roi_at_center()) {
-        Vector3p loc = copter.circle_nav->get_center();
-        loc.z = 0;
-        Location circle_center(loc, Location::AltFrame::ABOVE_TERRAIN);
+        const Vector3p &pos { copter.circle_nav->get_center() };
+        Location circle_center;
+        if (!AP::ahrs().get_location_from_origin_offset(circle_center, pos * 0.01)) {
+            return false;
+        }
+        // point at the ground:
+        circle_center.set_alt_cm(0, Location::AltFrame::ABOVE_TERRAIN);
         s->set_roi_target(circle_center);
     }
 #endif

libraries/AP_AHRS/AP_AHRS.cpp

@@ -3191,6 +3191,31 @@ bool AP_AHRS::get_velocity_NED(Vector3f &vec) const
     return state.velocity_NED_ok;
 }
 
+// return location corresponding to vector relative to the
+// vehicle's origin
+bool AP_AHRS::get_location_from_origin_offset(Location &loc, const Vector3p &offset_ned) const
+{
+    if (!get_origin(loc)) {
+        return false;
+    }
+    loc.offset(offset_ned);
+
+    return true;
+}
+
+// return location corresponding to vector relative to the
+// vehicle's home location
+bool AP_AHRS::get_location_from_home_offset(Location &loc, const Vector3p &offset_ned) const
+{
+    if (!home_is_set()) {
+        return false;
+    }
+    loc = get_home();
+    loc.offset(offset_ned);
+
+    return true;
+}
+
 // singleton instance
 AP_AHRS *AP_AHRS::_singleton;
 

libraries/AP_AHRS/AP_AHRS.h

@@ -244,6 +244,11 @@ class AP_AHRS {
     void get_relative_position_D_home(float &posD) const;
     bool get_relative_position_D_origin(float &posD) const WARN_IF_UNUSED;
 
+    // return location corresponding to vector relative to the
+    // vehicle's origin
+    bool get_location_from_origin_offset(Location &loc, const Vector3p &offset_ned) const WARN_IF_UNUSED;
+    bool get_location_from_home_offset(Location &loc, const Vector3p &offset_ned) const WARN_IF_UNUSED;
+
     // Get a derivative of the vertical position in m/s which is kinematically consistent with the vertical position is required by some control loops.
     // This is different to the vertical velocity from the EKF which is not always consistent with the vertical position due to the various errors that are being corrected for.
     bool get_vert_pos_rate_D(float &velocity) const;

libraries/AP_Common/Location.cpp

@@ -49,22 +49,6 @@ Location::Location(const Vector3f &ekf_offset_neu, AltFrame frame)
     }
 }
 
-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);
-    }
-}
-
 void Location::set_alt_cm(int32_t alt_cm, AltFrame frame)
 {
     alt = alt_cm;
@@ -306,6 +290,14 @@ void Location::offset(ftype ofs_north, ftype ofs_east)
     offset_latlng(lat, lng, ofs_north, ofs_east);
 }
 
+// extrapolate latitude/longitude given distances (in meters) north
+// and east. Note that this is metres, *even for the altitude*.
+void Location::offset(const Vector3p &ofs_ned)
+{
+    offset_latlng(lat, lng, ofs_ned.x, ofs_ned.y);
+    alt += -ofs_ned.z * 100;  // m -> cm
+}
+
 /*
  *  extrapolate latitude/longitude given bearing and distance
  * Note that this function is accurate to about 1mm at a distance of

libraries/AP_Common/Location.h

@@ -79,6 +79,9 @@ class Location
     // extrapolate latitude/longitude given distances (in meters) north and 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 distances (in meters) north
+    // and east. Note that this is metres, *even for the altitude*.
+    void offset(const Vector3p &ofs_ned);
 
     // extrapolate latitude/longitude given bearing and distance
     void offset_bearing(ftype bearing_deg, ftype distance);

libraries/AP_Common/tests/test_location.cpp

@@ -113,6 +113,19 @@ TEST(Location, LocOffsetDouble)
     }
 }
 
+TEST(Location, LocOffset3DDouble)
+{
+    Location loc {
+        -353632620, 1491652373, 60000, Location::AltFrame::ABSOLUTE
+    };
+    // this is ned, so our latitude should change, and our new
+    // location should be above the original:
+    loc.offset(Vector3d{1000, 0, -10});
+    EXPECT_EQ(loc.lat, -353542788);
+    EXPECT_EQ(loc.lng, 1491652373);
+    EXPECT_EQ(loc.alt, 61000);
+}
+
 TEST(Location, Tests)
 {
     Location test_location;