manual condition includes threshold values

9 years ago · 50175ca7ea
1 changed files with 2 additions and 2 deletions
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@ -1944,8 +1944,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
				@@ -1944,8 +1944,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi

 		}

-		if (!_yaw_lock_engaged || fabsf(_manual.y) > HDG_HOLD_MAN_INPUT_THRESH ||
-		    fabsf(_manual.r) > HDG_HOLD_MAN_INPUT_THRESH) {
+		if (!_yaw_lock_engaged || fabsf(_manual.y) >= HDG_HOLD_MAN_INPUT_THRESH ||
+		    fabsf(_manual.r) >= HDG_HOLD_MAN_INPUT_THRESH) {
 			_hdg_hold_enabled = false;
 			_yaw_lock_engaged = false;
 			_att_sp.roll_body = _manual.y * _parameters.man_roll_max_rad;