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

AC_PosControl: allow desired vel z to be above speed limit

This commit is contained in:
Leonard Hall 2015-10-27 22:44:41 +09:00 committed by Randy Mackay
parent f55c31a157
commit 245f7ce268
1 changed files with 13 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -351,6 +351,19 @@ void AC_PosControl::pos_to_rate_z()
// calculate _vel_target.z using from _pos_error.z using sqrt controller // calculate _vel_target.z using from _pos_error.z using sqrt controller
_vel_target.z = AC_AttitudeControl::sqrt_controller(_pos_error.z, _p_pos_z.kP(), _accel_z_cms); _vel_target.z = AC_AttitudeControl::sqrt_controller(_pos_error.z, _p_pos_z.kP(), _accel_z_cms);
// check speed limits
// To-Do: check these speed limits here or in the pos->rate controller
_limit.vel_up = false;
_limit.vel_down = false;
if (_vel_target.z < _speed_down_cms) {
_vel_target.z = _speed_down_cms;
_limit.vel_down = true;
}
if (_vel_target.z > _speed_up_cms) {
_vel_target.z = _speed_up_cms;
_limit.vel_up = true;
}
// add feed forward component // add feed forward component
_vel_target.z += _vel_desired.z; _vel_target.z += _vel_desired.z;
@ -365,19 +378,6 @@ void AC_PosControl::rate_to_accel_z()
const Vector3f& curr_vel = _inav.get_velocity(); const Vector3f& curr_vel = _inav.get_velocity();
float p; // used to capture pid values for logging float p; // used to capture pid values for logging
// check speed limits
// To-Do: check these speed limits here or in the pos->rate controller
_limit.vel_up = false;
_limit.vel_down = false;
if (_vel_target.z < _speed_down_cms) {
_vel_target.z = _speed_down_cms;
_limit.vel_down = true;
}
if (_vel_target.z > _speed_up_cms) {
_vel_target.z = _speed_up_cms;
_limit.vel_up = true;
}
// reset last velocity target to current target // reset last velocity target to current target
if (_flags.reset_rate_to_accel_z) { if (_flags.reset_rate_to_accel_z) {
_vel_last.z = _vel_target.z; _vel_last.z = _vel_target.z;