diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 442c75f8a5cb6b267ad38600f521ba83345a5285..f92e585386f957261cc7b392058f8eecf095eee4 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -649,18 +649,17 @@ MulticopterPositionControl::run()
 			_wv_controller->update(matrix::Quatf(_att_sp.q_d), _states.yaw);
 		}
 
-		if (_control_mode.flag_armed) {
+		// arm hysteresis prevents vehicle to takeoff
+		// before propeller reached idle speed.
+		_arm_hysteresis.set_state_and_update(_control_mode.flag_armed);
+
+		if (_arm_hysteresis.get_state()) {
 			// as soon vehicle is armed check for flighttask
 			start_flight_task();
-			// arm hysteresis prevents vehicle to takeoff
-			// before propeller reached idle speed.
-			_arm_hysteresis.set_state_and_update(true);
 
 		} else {
 			// disable flighttask
 			_flight_tasks.switchTask(FlightTaskIndex::None);
-			// reset arm hysteresis
-			_arm_hysteresis.set_state_and_update(false);
 		}
 
 		// check if any task is active