diff --git a/src/modules/mc_pos_control/TranslationControl.cpp b/src/modules/mc_pos_control/TranslationControl.cpp
index f0b990764c..8300ce1eb8 100644
--- a/src/modules/mc_pos_control/TranslationControl.cpp
+++ b/src/modules/mc_pos_control/TranslationControl.cpp
@@ -141,6 +141,7 @@ void TranslationControl::_interfaceMapping()
 	} else if (!PX4_ISFINITE(_yaw_sp)) {
 		_yaw_sp = _yaw_sp_int;
 	}
+
 }
 
 /* generate desired velocity */
@@ -226,9 +227,10 @@ void TranslationControl::_velocityController(const float &dt)
 	}
 
 	if (!stop_I[1]) {
-		_thr_int(2) += vel_err(3) * Iv(2) * dt;
+		_thr_int(2) += vel_err(2) * Iv(2) * dt;
 	}
 
+
 }
 
 void TranslationControl::_yawController(const float &dt)