From a9c62fa5655f77a2f0a1b19f823b181443bd844f Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 6 Sep 2023 14:03:29 +1000 Subject: [PATCH] AP_Common: Location: add offset(Vector3p &ned_offset) --- libraries/AP_Common/Location.cpp | 8 ++++++++ libraries/AP_Common/Location.h | 3 +++ libraries/AP_Common/tests/test_location.cpp | 13 +++++++++++++ 3 files changed, 24 insertions(+) diff --git a/libraries/AP_Common/Location.cpp b/libraries/AP_Common/Location.cpp index fb5225caaa..31c44e3307 100644 --- a/libraries/AP_Common/Location.cpp +++ b/libraries/AP_Common/Location.cpp @@ -306,6 +306,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 diff --git a/libraries/AP_Common/Location.h b/libraries/AP_Common/Location.h index 122bead689..61ed03b78e 100644 --- a/libraries/AP_Common/Location.h +++ b/libraries/AP_Common/Location.h @@ -79,6 +79,9 @@ public: // 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); diff --git a/libraries/AP_Common/tests/test_location.cpp b/libraries/AP_Common/tests/test_location.cpp index 0c484e20d4..f4bef012ef 100644 --- a/libraries/AP_Common/tests/test_location.cpp +++ b/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;