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 a631336273..7adc51392e 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -1370,9 +1370,10 @@ MulticopterPositionControl::task_main()
 								}
 							}
 						}
+					}
 
-					} else {
-						/* thrust compensation for altitude only control mode */
+					if (_control_mode.flag_control_altitude_enabled) {
+						/* thrust compensation for altitude only control modes */
 						float att_comp;
 
 						if (_R(2, 2) > TILT_COS_MAX) {