Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Common: add Location::linear_interpolate_alt

This commit is contained in:
Randy Mackay 2021-03-19 16:57:36 +09:00
parent 7daacfd63e
commit 70c6694798
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

@ -461,3 +461,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());
}

6
libraries/AP_Common/Location.h
View File

@ -118,6 +118,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