Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Common: add Location::linear_interpolate_alt

Browse Source
This commit is contained in:
Randy Mackay 2021-03-19 16:57:36 +09:00
parent b6ce206c3d
commit d946a2c95d
2 changed files with 16 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_Common/Location.cpp
View File

@ -459,3 +459,13 @@ int32_t Location::limit_lattitude(int32_t lat)
} }
return 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());
}

6
libraries/AP_Common/Location.h
View File

@ -122,6 +122,12 @@ public:
*/ */
float line_path_proportion(const Location &point1, const Location &point2) const; 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); } bool initialised() const { return (lat !=0 || lng != 0 || alt != 0); }
// wrap longitude at -180e7 to 180e7 // wrap longitude at -180e7 to 180e7