diff --git a/integrationtests/demo_tests/mavros_offboard_posctl_test.py b/integrationtests/demo_tests/mavros_offboard_posctl_test.py
index f42617cfd8..7747d904d8 100755
--- a/integrationtests/demo_tests/mavros_offboard_posctl_test.py
+++ b/integrationtests/demo_tests/mavros_offboard_posctl_test.py
@@ -134,7 +134,7 @@ class MavrosOffboardPosctlTest(unittest.TestCase):
                                    128 | 1, 6, 0, 0, 0, 0, 0)
                 self.armed = True
 
-            if self.is_at_position(pos.pose.position.x, pos.pose.position.y, pos.pose.position.z, 0.5):
+            if self.is_at_position(pos.pose.position.x, pos.pose.position.y, pos.pose.position.z, 1):
                 break
             count = count + 1
             self.rate.sleep()
@@ -154,16 +154,6 @@ class MavrosOffboardPosctlTest(unittest.TestCase):
         for i in range(0, len(positions)):
             self.reach_position(positions[i][0], positions[i][1], positions[i][2], 180)
 
-        count = 0
-        timeout = 50
-        while count < timeout:
-            if not self.is_at_position(2, 2, 2, 0.5):
-                break
-            count = count + 1
-            self.rate.sleep()
-
-        self.assertTrue(count == timeout, "position could not be held")
-
 
 if __name__ == '__main__':
     import rostest
diff --git a/posix-configs/SITL/init/rcS_gazebo_iris b/posix-configs/SITL/init/rcS_gazebo_iris
index d129603870..f95c7da6d8 100644
--- a/posix-configs/SITL/init/rcS_gazebo_iris
+++ b/posix-configs/SITL/init/rcS_gazebo_iris
@@ -18,12 +18,17 @@ param set CAL_ACC0_YSCALE 1.01
 param set CAL_ACC0_ZSCALE 1.01
 param set CAL_ACC1_XOFF 0.01
 param set CAL_MAG0_XOFF 0.01
+param set MPC_XY_P 0.25
+param set MPC_XY_VEL_P 0.05
+param set MPC_XY_VEL_D 0.005
+param set MPC_XY_FF 0.1
+param set MPC_Z_P 1.3
 param set SENS_BOARD_ROT 8
 param set SENS_BOARD_X_OFF 0.000001
 param set COM_RC_IN_MODE 1
 param set COM_DL_LOSS_EN 1
 param set COM_DISARM_LAND 3
-param set NAV_ACC_RAD 12.0
+param set NAV_ACC_RAD 2.0
 param set RTL_RETURN_ALT 30.0
 param set RTL_DESCEND_ALT 10.0
 param set RTL_LAND_DELAY 0