|
|
@ -137,8 +137,8 @@ void AC_PosControl::set_alt_target_with_slew(float alt_cm, float dt) |
|
|
|
void AC_PosControl::set_alt_target_from_climb_rate(float climb_rate_cms, float dt, bool force_descend) |
|
|
|
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
|
|
|
|
// adjust desired alt if motors have not hit their limits
|
|
|
|
// To-Do: add check of _limit.pos_up and _limit.pos_down?
|
|
|
|
// To-Do: add check of _limit.pos_down?
|
|
|
|
if ((climb_rate_cms<0 && (!_motors.limit.throttle_lower || force_descend)) || (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 && !_limit.pos_up)) { |
|
|
|
_pos_target.z += climb_rate_cms * dt; |
|
|
|
_pos_target.z += climb_rate_cms * dt; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|