From a30404fb11c8ebe0c868fb53a108afcb87339969 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Fri, 5 Apr 2019 08:11:26 +0200 Subject: [PATCH] AP_Common: move location_update to Location and rename to offset_bearing --- libraries/AP_Common/Location.cpp | 14 ++++++++++++++ libraries/AP_Common/Location.h | 3 +++ 2 files changed, 17 insertions(+) diff --git a/libraries/AP_Common/Location.cpp b/libraries/AP_Common/Location.cpp index e74764d2a1..022de9ebda 100644 --- a/libraries/AP_Common/Location.cpp +++ b/libraries/AP_Common/Location.cpp @@ -232,6 +232,20 @@ void Location::offset(float ofs_north, float ofs_east) } } +/* + * extrapolate latitude/longitude given bearing and distance + * Note that this function is accurate to about 1mm at a distance of + * 100m. This function has the advantage that it works in relative + * positions, so it keeps the accuracy even when dealing with small + * distances and floating point numbers + */ +void Location::offset_bearing(float bearing, float distance) +{ + const float ofs_north = cosf(radians(bearing)) * distance; + const float ofs_east = sinf(radians(bearing)) * distance; + offset(ofs_north, ofs_east); +} + float Location::longitude_scale() const { float scale = cosf(lat * (1.0e-7f * DEG_TO_RAD)); diff --git a/libraries/AP_Common/Location.h b/libraries/AP_Common/Location.h index eb594f6c41..02aff84ab5 100644 --- a/libraries/AP_Common/Location.h +++ b/libraries/AP_Common/Location.h @@ -75,6 +75,9 @@ public: // extrapolate latitude/longitude given distances (in meters) north and east void offset(float ofs_north, float ofs_east); + // extrapolate latitude/longitude given bearing and distance + void offset_bearing(float bearing, float distance); + // 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