diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 37714efaaca57e3abe827d9f2b37f220d4d2fbd0..0cd3a5d6d19d75c8a867fdb9e915e4fedad0afa1 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -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; } } diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 52ffaa02e721fb81a3a6ce8ba341d7e9fcd8553c..08bc77440b40c1874de80bef673d7c94e905ca04 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -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(); }