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

AC_WPNav: brake, wp and spline clear desired velocity_xy

Browse Source
This commit is contained in:
Leonard Hall 2017-07-13 20:45:24 +09:00 committed by Randy Mackay
parent 6430b75224
commit c7c3dd561a
1 changed files with 5 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -347,6 +347,7 @@ void AC_WPNav::init_brake_target(float accel_cmss)
Vector3f stopping_point; Vector3f stopping_point;
// initialise position controller // initialise position controller
_pos_control.set_desired_accel_xy(0.0f,0.0f);
_pos_control.init_xy_controller(); _pos_control.init_xy_controller();
// initialise pos controller speed and acceleration // initialise pos controller speed and acceleration
@ -391,9 +392,13 @@ void AC_WPNav::wp_and_spline_init()
} }
// initialise position controller // initialise position controller
_pos_control.set_desired_accel_xy(0.0f,0.0f);
_pos_control.init_xy_controller(); _pos_control.init_xy_controller();
_pos_control.clear_desired_velocity_ff_z(); _pos_control.clear_desired_velocity_ff_z();
// initialise feed forward velocity to zero
_pos_control.set_desired_velocity_xy(0.0f, 0.0f);
// initialise position controller speed and acceleration // initialise position controller speed and acceleration
_pos_control.set_speed_xy(_wp_speed_cms); _pos_control.set_speed_xy(_wp_speed_cms);
_pos_control.set_accel_xy(_wp_accel_cms); _pos_control.set_accel_xy(_wp_accel_cms);
@ -547,7 +552,6 @@ void AC_WPNav::shift_wp_origin_to_current_pos()
// move pos controller target and disable feed forward // move pos controller target and disable feed forward
_pos_control.set_pos_target(curr_pos); _pos_control.set_pos_target(curr_pos);
_pos_control.freeze_ff_xy();
_pos_control.freeze_ff_z(); _pos_control.freeze_ff_z();
} }
@ -763,7 +767,6 @@ bool AC_WPNav::update_wpnav()
// TODO: why always consider Z axis discontinuous? // TODO: why always consider Z axis discontinuous?
if (_flags.new_wp_destination) { if (_flags.new_wp_destination) {
_flags.new_wp_destination = false; _flags.new_wp_destination = false;
_pos_control.freeze_ff_xy();
} }
_pos_control.freeze_ff_z(); _pos_control.freeze_ff_z();
@ -1055,7 +1058,6 @@ bool AC_WPNav::update_spline()
// TODO: why always consider Z axis discontinuous? // TODO: why always consider Z axis discontinuous?
if (_flags.new_wp_destination) { if (_flags.new_wp_destination) {
_flags.new_wp_destination = false; _flags.new_wp_destination = false;
_pos_control.freeze_ff_xy();
} }
_pos_control.freeze_ff_z(); _pos_control.freeze_ff_z();