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

AC_WPNav: add set_wp_destination_NED to accept target in meters NED

Browse Source
This commit is contained in:
squilter 2017-07-26 13:12:11 -04:00 committed by Randy Mackay
parent f37d742160
commit da7aa36f2d
2 changed files with 10 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -470,6 +470,13 @@ bool AC_WPNav::set_wp_destination(const Vector3f& destination, bool terrain_alt)
return set_wp_origin_and_destination(origin, destination, terrain_alt); return set_wp_origin_and_destination(origin, destination, terrain_alt);
} }
/// set waypoint destination using NED position vector from ekf origin in meters
bool AC_WPNav::set_wp_destination_NED(const Vector3f& destination_NED)
{
// convert NED to NEU and do not use terrain following
return set_wp_destination(Vector3f(destination_NED.x * 100.0f, destination_NED.y * 100.0f, -destination_NED.z * 100.0f), false);
}
/// set_origin_and_destination - set origin and destination waypoints using position vectors (distance from home in cm) /// set_origin_and_destination - set origin and destination waypoints using position vectors (distance from home in cm)
/// terrain_alt should be true if origin.z and destination.z are desired altitudes above terrain (false if these are alt-above-ekf-origin) /// terrain_alt should be true if origin.z and destination.z are desired altitudes above terrain (false if these are alt-above-ekf-origin)
/// returns false on failure (likely caused by missing terrain data) /// returns false on failure (likely caused by missing terrain data)

3
libraries/AC_WPNav/AC_WPNav.h
View File

@ -157,6 +157,9 @@ public:
/// 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);
/// set waypoint destination using NED position vector from ekf origin in meters
bool set_wp_destination_NED(const Vector3f& destination_NED);
/// set_wp_origin_and_destination - set origin and destination waypoints using position vectors (distance from ekf origin in cm) /// set_wp_origin_and_destination - set origin and destination waypoints using position vectors (distance from ekf origin in cm)
/// terrain_alt should be true if origin.z and destination.z are desired altitudes above terrain (false if these are alt-above-ekf-origin) /// terrain_alt should be true if origin.z and destination.z are desired altitudes above terrain (false if these are alt-above-ekf-origin)
/// returns false on failure (likely caused by missing terrain data) /// returns false on failure (likely caused by missing terrain data)