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

AC_PosControl: velocity controller uses feed-forward althold

This commit is contained in:
Randy Mackay 2015-11-17 13:25:51 +09:00
parent 4b41710261
commit 323a527734
1 changed files with 4 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -689,13 +689,14 @@ void AC_PosControl::init_vel_controller_xyz()
_flags.reset_rate_to_accel_xy = true; _flags.reset_rate_to_accel_xy = true;
_flags.reset_accel_to_lean_xy = true; _flags.reset_accel_to_lean_xy = true;
// set target position in xy axis // set target position
const Vector3f& curr_pos = _inav.get_position(); const Vector3f& curr_pos = _inav.get_position();
set_xy_target(curr_pos.x, curr_pos.y); set_xy_target(curr_pos.x, curr_pos.y);
set_alt_target(curr_pos.z);
// move current vehicle velocity into feed forward velocity // move current vehicle velocity into feed forward velocity
const Vector3f& curr_vel = _inav.get_velocity(); const Vector3f& curr_vel = _inav.get_velocity();
set_desired_velocity_xy(curr_vel.x, curr_vel.y); set_desired_velocity(curr_vel);
} }
/// update_velocity_controller_xyz - run the velocity controller - should be called at 100hz or higher /// update_velocity_controller_xyz - run the velocity controller - should be called at 100hz or higher
@ -729,7 +730,7 @@ void AC_PosControl::update_vel_controller_xyz(float ekfNavVelGainScaler)
accel_to_lean_angles(dt, ekfNavVelGainScaler, false); accel_to_lean_angles(dt, ekfNavVelGainScaler, false);
// update altitude target // update altitude target
set_alt_target_from_climb_rate(_vel_desired.z, dt, false); set_alt_target_from_climb_rate_ff(_vel_desired.z, dt, false);
// run z-axis position controller // run z-axis position controller
update_z_controller(); update_z_controller();