mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
AC_WPNav: fix get_wp_destination_loc
was incorrectly adding alt-above-terrain as if it was alt-above-ekf-origin also fixup comments
This commit is contained in:
parent
32c27b32aa
commit
85b24cf641
@ -237,14 +237,19 @@ bool AC_WPNav::set_wp_destination_next_loc(const Location& destination)
|
||||
return set_wp_destination_next(dest_neu, terr_alt);
|
||||
}
|
||||
|
||||
// get destination as a location. Altitude frame will be absolute (AMSL) or above terrain
|
||||
// returns false if unable to return a destination (for example if origin has not yet been set)
|
||||
bool AC_WPNav::get_wp_destination_loc(Location& destination) const
|
||||
{
|
||||
Vector3f dest = get_wp_destination();
|
||||
if (!AP::ahrs().get_origin(destination)) {
|
||||
return false;
|
||||
}
|
||||
destination.offset(dest.x*0.01f, dest.y*0.01f);
|
||||
destination.alt += dest.z;
|
||||
destination.offset(_destination.x*0.01f, _destination.y*0.01f);
|
||||
if (_terrain_alt) {
|
||||
destination.set_alt_cm(_destination.z, Location::AltFrame::ABOVE_TERRAIN);
|
||||
} else {
|
||||
destination.alt += _destination.z;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
|
@ -106,9 +106,8 @@ public:
|
||||
bool set_wp_destination_loc(const Location& destination);
|
||||
bool set_wp_destination_next_loc(const Location& destination);
|
||||
|
||||
// returns wp location using location class.
|
||||
// returns false if unable to convert from target vector to global
|
||||
// coordinates
|
||||
// get destination as a location. Altitude frame will be absolute (AMSL) or above terrain
|
||||
// returns false if unable to return a destination (for example if origin has not yet been set)
|
||||
bool get_wp_destination_loc(Location& destination) const;
|
||||
|
||||
// returns object avoidance adjusted destination which is always the same as get_wp_destination
|
||||
|
Loading…
Reference in New Issue
Block a user