Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: unset yaw when setting new origin and destination 

This ensures that old yaw targets are not used in the short interval before they are initialised in advance_wp_target_along_track or advance_spline_along_track
This commit is contained in:
Randy Mackay 2017-05-01 21:50:12 -07:00
parent 1691a39b36
commit bffc5daeb0
1 changed files with 2 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -512,6 +512,7 @@ bool AC_WPNav::set_wp_origin_and_destination(const Vector3f& origin, const Vecto
_flags.slowing_down = false; // target is not slowing down yet
_flags.segment_type = SEGMENT_STRAIGHT;
_flags.new_wp_destination = true; // flag new waypoint so we can freeze the pos controller's feed forward and smooth the transition
_flags.wp_yaw_set = false;
// initialise the limited speed to current speed along the track
const Vector3f &curr_vel = _inav.get_velocity();
@ -1017,6 +1018,7 @@ bool AC_WPNav::set_spline_origin_and_destination(const Vector3f& origin, const V
_flags.reached_destination = false;
_flags.segment_type = SEGMENT_SPLINE;
_flags.new_wp_destination = true; // flag new waypoint so we can freeze the pos controller's feed forward and smooth the transition
_flags.wp_yaw_set = false;
// initialise yaw related variables
_track_length_xy = safe_sqrt(sq(_destination.x - _origin.x)+sq(_destination.y - _origin.y)); // horizontal track length (used to decide if we should update yaw)