From aa134d5a617cd90142d82039c4466aef2f009383 Mon Sep 17 00:00:00 2001 From: Roman Date: Mon, 20 Aug 2018 21:29:42 +0200 Subject: [PATCH] l1 position controller: make sure that a NAN roll setpoint does not keep the roll setpoint state at NAN forever Signed-off-by: Roman --- l1/ecl_l1_pos_controller.cpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/l1/ecl_l1_pos_controller.cpp b/l1/ecl_l1_pos_controller.cpp index 5be25b5c47..a9fa980af7 100644 --- a/l1/ecl_l1_pos_controller.cpp +++ b/l1/ecl_l1_pos_controller.cpp @@ -51,16 +51,14 @@ void ECL_L1_Pos_Controller::update_roll_setpoint() float roll_new = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G); roll_new = math::constrain(roll_new, -_roll_lim_rad, _roll_lim_rad); - // no slew rate limiting active - if (_dt <= 0.0f || _roll_slew_rate <= 0.0f) { - _roll_setpoint = roll_new; - return; + if (_dt > 0.0f && _roll_slew_rate > 0.0f) { + // slew rate limiting active + roll_new = math::constrain(roll_new, _roll_setpoint - _roll_slew_rate * _dt, _roll_setpoint + _roll_slew_rate * _dt); } - // slew rate limiting active - roll_new = math::constrain(roll_new, _roll_setpoint - _roll_slew_rate * _dt, _roll_setpoint + _roll_slew_rate * _dt); - - _roll_setpoint = roll_new; + if (ISFINITE(roll_new)) { + _roll_setpoint = roll_new; + } }