From e81c1dd5a11d4ae3bea5998b0082a5f79e354be3 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Thu, 13 Nov 2014 16:56:38 -0800 Subject: [PATCH] AC_PosControl: add force_descend option to set_alt_target_from_climb_rate --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 4 ++-- libraries/AC_AttitudeControl/AC_PosControl.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 0f5352a76c..c849cc0a6a 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 52ffaa02e7..08bc77440b 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(); }