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

AC_WPNav: add get_wp_destination to get a Location_Class for the wp

Browse Source
This commit is contained in:
Peter Barker 2018-05-11 21:54:14 +10:00 committed by Peter Barker
parent c3a84a9d66
commit 2c962afe98
2 changed files with 15 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -201,6 +201,16 @@ bool AC_WPNav::set_wp_destination(const Location_Class& destination)
return set_wp_destination(dest_neu, terr_alt); return set_wp_destination(dest_neu, terr_alt);
} }
bool AC_WPNav::get_wp_destination(Location_Class& destination) {
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;
return true;
}
/// set_wp_destination waypoint using position vector (distance from home in cm) /// set_wp_destination waypoint using position vector (distance from home in cm)
/// terrain_alt should be true if destination.z is a desired altitude above terrain /// terrain_alt should be true if destination.z is a desired altitude above terrain
bool AC_WPNav::set_wp_destination(const Vector3f& destination, bool terrain_alt) bool AC_WPNav::set_wp_destination(const Vector3f& destination, bool terrain_alt)

5
libraries/AC_WPNav/AC_WPNav.h
View File

@ -103,6 +103,11 @@ public:
/// returns false if conversion from location to vector from ekf origin cannot be calculated /// returns false if conversion from location to vector from ekf origin cannot be calculated
bool set_wp_destination(const Location_Class& destination); bool set_wp_destination(const Location_Class& destination);
// returns wp location using location class.
// returns false if unable to convert from target vector to global
// coordinates
bool get_wp_destination(Location_Class& destination);
/// set_wp_destination waypoint using position vector (distance from ekf origin in cm) /// set_wp_destination waypoint using position vector (distance from ekf origin in cm)
/// terrain_alt should be true if destination.z is a desired altitude above terrain /// terrain_alt should be true if destination.z is a desired altitude above terrain
bool set_wp_destination(const Vector3f& destination, bool terrain_alt = false); bool set_wp_destination(const Vector3f& destination, bool terrain_alt = false);