diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp
index 2a7819eb..5253fb1a 100644
--- a/src/px4_connection/src/px4_controller.cpp
+++ b/src/px4_connection/src/px4_controller.cpp
@@ -338,7 +338,7 @@ private:
         // from polar to cartesian coordinates
         des_x = rho * cos(theta);
         des_y = rho * sin(theta);
-        des_z = position[3]; // the z position can be set to the received height
+        des_z = position[2]; // the z position can be set to the received height
 
         // velocity computation
         float dot_rho = K * omega;