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

AC_WPNav: integrate set_desired_velocity_xy function name change

This commit is contained in:
Randy Mackay 2014-06-12 11:52:40 +09:00
parent f5640dadbf
commit c9661cfb09
1 changed files with 6 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -135,7 +135,7 @@ void AC_WPNav::init_loiter_target(const Vector3f& position, bool reset_I)
_pos_control.set_xy_target(position.x, position.y);
// initialise feed forward velocity to zero
_pos_control.set_desired_velocity(0,0);
_pos_control.set_desired_velocity_xy(0,0);
// initialise desired accel and add fake wind
_loiter_desired_accel.x = 0;
@ -164,7 +164,7 @@ void AC_WPNav::init_loiter_target()
_pos_control.set_xy_target(curr_pos.x, curr_pos.y);
// move current vehicle velocity into feed forward velocity
_pos_control.set_desired_velocity(curr_vel.x, curr_vel.y);
_pos_control.set_desired_velocity_xy(curr_vel.x, curr_vel.y);
// initialise desired accel and add fake wind
_loiter_desired_accel.x = (_loiter_accel_cms)*curr_vel.x/_loiter_speed_cms;
@ -239,10 +239,11 @@ void AC_WPNav::calc_loiter_desired_velocity(float nav_dt)
_loiter_desired_accel += des_accel_diff;
// get pos_control's feed forward velocity
Vector2f desired_vel = _pos_control.get_desired_velocity();
Vector3f desired_vel = _pos_control.get_desired_velocity();
// add pilot commanded acceleration
desired_vel += _loiter_desired_accel * nav_dt;
desired_vel.x += _loiter_desired_accel.x * nav_dt;
desired_vel.y += _loiter_desired_accel.y * nav_dt;
// reduce velocity with fake wind resistance
if (_pilot_accel_fwd_cms != 0.0f || _pilot_accel_rgt_cms != 0.0f) {
@ -264,7 +265,7 @@ void AC_WPNav::calc_loiter_desired_velocity(float nav_dt)
}
// send adjusted feed forward velocity back to position controller
_pos_control.set_desired_velocity(desired_vel.x,desired_vel.y);
_pos_control.set_desired_velocity_xy(desired_vel.x,desired_vel.y);
}
/// get_bearing_to_target - get bearing to loiter target in centi-degrees