From d946a2c95dbf54b6681cec590a3790c5a74104af Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 19 Mar 2021 16:57:36 +0900 Subject: [PATCH] AP_Common: add Location::linear_interpolate_alt --- libraries/AP_Common/Location.cpp | 10 ++++++++++ libraries/AP_Common/Location.h | 6 ++++++ 2 files changed, 16 insertions(+) diff --git a/libraries/AP_Common/Location.cpp b/libraries/AP_Common/Location.cpp index f90b001f82..20617363e9 100644 --- a/libraries/AP_Common/Location.cpp +++ b/libraries/AP_Common/Location.cpp @@ -459,3 +459,13 @@ int32_t Location::limit_lattitude(int32_t lat) } return lat; } + +// update altitude and alt-frame base on this location's horizontal position between point1 and point2 +// this location's lat,lon is used to calculate the alt of the closest point on the line between point1 and point2 +// origin and destination's altitude frames must be the same +// this alt-frame will be updated to match the destination alt frame +void Location::linearly_interpolate_alt(const Location &point1, const Location &point2) +{ + // new target's distance along the original track and then linear interpolate between the original origin and destination altitudes + set_alt_cm(point1.alt + (point2.alt - point1.alt) * constrain_float(line_path_proportion(point1, point2), 0.0f, 1.0f), point2.get_alt_frame()); +} diff --git a/libraries/AP_Common/Location.h b/libraries/AP_Common/Location.h index 5f9496adb8..a4787dbe5d 100644 --- a/libraries/AP_Common/Location.h +++ b/libraries/AP_Common/Location.h @@ -122,6 +122,12 @@ public: */ float line_path_proportion(const Location &point1, const Location &point2) const; + // update altitude and alt-frame base on this location's horizontal position between point1 and point2 + // this location's lat,lon is used to calculate the alt of the closest point on the line between point1 and point2 + // origin and destination's altitude frames must be the same + // this alt-frame will be updated to match the destination alt frame + void linearly_interpolate_alt(const Location &point1, const Location &point2); + bool initialised() const { return (lat !=0 || lng != 0 || alt != 0); } // wrap longitude at -180e7 to 180e7