l1 position controller: make sure that a NAN roll setpoint

does not keep the roll setpoint state at NAN forever Signed-off-by: Roman <[email protected]>
PX4 · Aug 21, 2018 · aa134d5 · aa134d5
1 parent 34cd136
commit aa134d5
Showing 1 changed file with 6 additions and 8 deletions.
diff --git 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;
+	}
 
 }