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

AC_WPNav: add shift_wp_origin_to_current_pos_xy

Browse Source 
also add shift_wp_origin_and_destination_to_stopping_point_xy
This commit is contained in:
Leonard Hall 2020-02-26 16:55:15 +09:00 committed by Randy Mackay
parent 29332ac112
commit 25f1a4a4e1
2 changed files with 51 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -317,6 +317,47 @@ void AC_WPNav::shift_wp_origin_to_current_pos()
_pos_control.freeze_ff_z();
}
/// shifts the origin and destination horizontally to the current position
/// used to reset the track when taking off without horizontal position control
/// relies on set_wp_destination or set_wp_origin_and_destination having been called first
void AC_WPNav::shift_wp_origin_and_destination_to_current_pos_xy()
{
// get current and target locations
const Vector3f& curr_pos = _inav.get_position();
// shift origin and destination horizontally
_origin.x = curr_pos.x;
_origin.y = curr_pos.y;
_destination.x = curr_pos.x;
_destination.y = curr_pos.y;
// move pos controller target horizontally
_pos_control.set_xy_target(curr_pos.x, curr_pos.y);
}
/// shifts the origin and destination horizontally to the achievable stopping point
/// used to reset the track when horizontal navigation is enabled after having been disabled (see Copter's wp_navalt_min)
/// relies on set_wp_destination or set_wp_origin_and_destination having been called first
void AC_WPNav::shift_wp_origin_and_destination_to_stopping_point_xy()
{
// relax position control in xy axis
// removing velocity error also impacts stopping point calculation
_pos_control.relax_velocity_controller_xy();
// get current and target locations
Vector3f stopping_point;
get_wp_stopping_point_xy(stopping_point);
// shift origin and destination horizontally
_origin.x = stopping_point.x;
_origin.y = stopping_point.y;
_destination.x = stopping_point.x;
_destination.y = stopping_point.y;
// move pos controller target horizontally
_pos_control.set_xy_target(stopping_point.x, stopping_point.y);
}
/// get_wp_stopping_point_xy - returns vector to stopping point based on a horizontal position and velocity
void AC_WPNav::get_wp_stopping_point_xy(Vector3f& stopping_point) const
{

10
libraries/AC_WPNav/AC_WPNav.h
View File

@ -135,6 +135,16 @@ public:
/// relies on set_wp_destination or set_wp_origin_and_destination having been called first
void shift_wp_origin_to_current_pos();
/// shifts the origin and destination horizontally to the current position
/// used to reset the track when taking off without horizontal position control
/// relies on set_wp_destination or set_wp_origin_and_destination having been called first
void shift_wp_origin_and_destination_to_current_pos_xy();
/// shifts the origin and destination horizontally to the achievable stopping point
/// used to reset the track when horizontal navigation is enabled after having been disabled (see Copter's wp_navalt_min)
/// relies on set_wp_destination or set_wp_origin_and_destination having been called first
void shift_wp_origin_and_destination_to_stopping_point_xy();
/// get_wp_stopping_point_xy - calculates stopping point based on current position, velocity, waypoint acceleration
/// results placed in stopping_position vector
void get_wp_stopping_point_xy(Vector3f& stopping_point) const;