aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_PosControl: add force_descend option to set_alt_target_from_climb_rate

Browse Source
This commit is contained in:
Jonathan Challinger 2014-11-13 16:56:38 -08:00 committed by Randy Mackay
parent c4093b159f
commit e81c1dd5a1
2 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -137,11 +137,11 @@ void AC_PosControl::set_alt_target_with_slew(float alt_cm, float dt)
/// should be called continuously (with dt set to be the expected time between calls)
/// actual position target will be moved no faster than the speed_down and speed_up
/// target will also be stopped if the motors hit their limits or leash length is exceeded
void AC_PosControl::set_alt_target_from_climb_rate(float climb_rate_cms, float dt)
void AC_PosControl::set_alt_target_from_climb_rate(float climb_rate_cms, float dt, bool force_descend)
{
// adjust desired alt if motors have not hit their limits
// To-Do: add check of _limit.pos_up and _limit.pos_down?
if ((climb_rate_cms<0 && !_motors.limit.throttle_lower) || (climb_rate_cms>0 && !_motors.limit.throttle_upper)) {
if ((climb_rate_cms<0 && (!_motors.limit.throttle_lower || force_descend)) || (climb_rate_cms>0 && !_motors.limit.throttle_upper)) {
_pos_target.z += climb_rate_cms * dt;
}
}

2
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -109,7 +109,7 @@ public:
/// should be called continuously (with dt set to be the expected time between calls)
/// actual position target will be moved no faster than the speed_down and speed_up
/// target will also be stopped if the motors hit their limits or leash length is exceeded
void set_alt_target_from_climb_rate(float climb_rate_cms, float dt);
void set_alt_target_from_climb_rate(float climb_rate_cms, float dt, bool force_descend = true);
/// set_alt_target_to_current_alt - set altitude target to current altitude
void set_alt_target_to_current_alt() { _pos_target.z = _inav.get_altitude(); }