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

AC_PosControl: move alt limit to set_alt_target_from_climb_rate 

The alt limit is instead enforced when the target is set using the
set_alt_target_from_climb_rate function
Also updated comments
This commit is contained in:
Randy Mackay 2015-01-30 13:57:37 +09:00
parent e28b7c3abe
commit 375b9a5bb1
2 changed files with 9 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -146,7 +146,13 @@ void AC_PosControl::set_alt_target_from_climb_rate(float climb_rate_cms, float d
if ((climb_rate_cms<0 && (!_motors.limit.throttle_lower || force_descend)) || (climb_rate_cms>0 && !_motors.limit.throttle_upper && !_limit.pos_up)) {
_pos_target.z += climb_rate_cms * dt;
}
// do not let target alt get above limit
if (_alt_max > 0 && _pos_target.z > _alt_max) {
_pos_target.z = _alt_max;
_limit.pos_up = true;
}
_vel_desired.z = climb_rate_cms;
}
@ -257,12 +263,6 @@ void AC_PosControl::pos_to_rate_z()
_limit.pos_up = false;
_limit.pos_down = false;
// do not let target alt get above limit
if (_alt_max > 0 && _pos_target.z > _alt_max) {
_pos_target.z = _alt_max;
_limit.pos_up = true;
}
// calculate altitude error
_pos_error.z = _pos_target.z - curr_alt;

4
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -67,8 +67,8 @@ public:
///
/// set_alt_max - sets maximum altitude above home in cm
/// set to zero to disable limit
/// To-Do: update this intermittantly from main code after checking if fence is enabled/disabled
/// only enforced when set_alt_target_from_climb_rate is used
/// set to zero to disable limit
void set_alt_max(float alt) { _alt_max = alt; }
/// set_speed_z - sets maximum climb and descent rates