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 5f1166c4d9..bdf32ba58b 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -1750,12 +1750,25 @@ MulticopterPositionControl::control_position(float dt)
 			thrust_sp(1) = 0.0f;
 		}
 
-		/* if still or already on ground command a zero XY velocity */
+		/* if still or already on ground command zero xy velcoity and zero xy thrust_sp in body frame to consider uneven ground */
 		if (_vehicle_land_detected.ground_contact) {
-			thrust_sp(0) = 0.0f;
-			thrust_sp(1) = 0.0f;
-			_vel_sp(0) = _vel(0);
-			_vel_sp(1) = _vel(1);
+
+			/* thrust setpoint in body frame*/
+			math::Vector<3> thrust_sp_body = _R.transposed() * thrust_sp;
+
+			/* we dont want to make any correction in body x and y*/
+			thrust_sp_body(0) = 0.0f;
+			thrust_sp_body(1) = 0.0f;
+
+			/* make sure z component of thrust_sp_body is larger than 0 (positive thrust is downward) */
+			thrust_sp_body(2) = thrust_sp(2) > 0.0f ? thrust_sp(2) : 0.0f;
+
+			/* convert back to local frame (NED) */
+			thrust_sp = _R * thrust_sp_body;
+
+			/* set velocity setpoint to zero and reset position */
+			_vel_sp(0) = 0.0f;
+			_vel_sp(1) = 0.0f;
 			_pos_sp(0) = _pos(0);
 			_pos_sp(1) = _pos(1);
 		}