Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 23:43:58 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: add get_stopping_point

Browse Source 
returns 3D stopping point, simply re-uses pos_controller methods
This commit is contained in:
Randy Mackay 2017-04-27 13:43:19 +09:00
parent a342b73604
commit e2cf836734
2 changed files with 8 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

@ -557,6 +557,13 @@ void AC_WPNav::get_wp_stopping_point_xy(Vector3f& stopping_point) const
_pos_control.get_stopping_point_xy(stopping_point); _pos_control.get_stopping_point_xy(stopping_point);
} }
/// get_wp_stopping_point - returns vector to stopping point based on 3D position and velocity
void AC_WPNav::get_wp_stopping_point(Vector3f& stopping_point) const
{
_pos_control.get_stopping_point_xy(stopping_point);
_pos_control.get_stopping_point_z(stopping_point);
}
/// advance_wp_target_along_track - move target location along track from origin to destination /// advance_wp_target_along_track - move target location along track from origin to destination
bool AC_WPNav::advance_wp_target_along_track(float dt) bool AC_WPNav::advance_wp_target_along_track(float dt)
{ {

1
libraries/AC_WPNav/AC_WPNav.h
View File

@ -168,6 +168,7 @@ public:
/// get_wp_stopping_point_xy - calculates stopping point based on current position, velocity, waypoint acceleration /// get_wp_stopping_point_xy - calculates stopping point based on current position, velocity, waypoint acceleration
/// results placed in stopping_position vector /// results placed in stopping_position vector
void get_wp_stopping_point_xy(Vector3f& stopping_point) const; void get_wp_stopping_point_xy(Vector3f& stopping_point) const;
void get_wp_stopping_point(Vector3f& stopping_point) const;
/// get_wp_distance_to_destination - get horizontal distance to destination in cm /// get_wp_distance_to_destination - get horizontal distance to destination in cm
float get_wp_distance_to_destination() const; float get_wp_distance_to_destination() const;