Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 14:08:45 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: init_loiter sets speed, accel before calcing stopping distance

Browse Source 
The stopping distance depends upon the speed and acceleration so these
must be updated first
This commit is contained in:
Randy Mackay 2014-04-30 21:20:51 +09:00
parent a3036fc443
commit 7dd366d84e
1 changed files with 8 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -134,21 +134,19 @@ void AC_WPNav::set_loiter_target(const Vector3f& position, bool reset_I)
/// init_loiter_target - initialize's loiter position and feed-forward velocity from current pos and velocity
void AC_WPNav::init_loiter_target()
{
Vector3f curr_vel = _inav->get_velocity();
Vector3f curr_vel = _inav->get_velocity();
// set target position
_pos_control.set_pos_target(_inav->get_position());
// initialise pos controller speed and acceleration
_pos_control.set_speed_xy(_loiter_speed_cms);
_loiter_accel_cms = _loiter_speed_cms/2.0f;
_pos_control.set_accel_xy(_loiter_accel_cms);
// set target position
_pos_control.set_target_to_stopping_point_xy();
// initialise feed forward velocities to zero
_pos_control.set_desired_velocity(curr_vel.x, curr_vel.y);
// initialise pos controller speed
_pos_control.set_speed_xy(_loiter_speed_cms);
// initialise pos controller acceleration
_loiter_accel_cms = _loiter_speed_cms/2.0f;
_pos_control.set_accel_xy(_loiter_accel_cms);
// initialise pilot input
_pilot_accel_fwd_cms = 0;
_pilot_accel_rgt_cms = 0;