diff --git a/posix-configs/SITL/init/rcS_gazebo_iris b/posix-configs/SITL/init/rcS_gazebo_iris
index 72d205447d..5978c609de 100644
--- a/posix-configs/SITL/init/rcS_gazebo_iris
+++ b/posix-configs/SITL/init/rcS_gazebo_iris
@@ -29,8 +29,8 @@ param set RTL_LAND_DELAY 0
 param set MIS_TAKEOFF_ALT 2.5
 param set MC_ROLLRATE_P 0.3
 param set MC_PITCHRATE_P 0.3
-param set MC_PITCH_P 5.5
-param set MC_ROLL_P 5.5
+param set MC_PITCH_P 7
+param set MC_ROLL_P 7
 param set MC_ROLLRATE_I 0.1
 param set MC_PITCHRATE_I 0.1
 param set MPC_HOLD_MAX_Z 2.0
diff --git a/posix-configs/SITL/init/rcS_gazebo_iris_opt_flow b/posix-configs/SITL/init/rcS_gazebo_iris_opt_flow
index 59fa38daf3..7aea1f3f40 100644
--- a/posix-configs/SITL/init/rcS_gazebo_iris_opt_flow
+++ b/posix-configs/SITL/init/rcS_gazebo_iris_opt_flow
@@ -27,10 +27,10 @@ param set RTL_RETURN_ALT 30.0
 param set RTL_DESCEND_ALT 10.0
 param set RTL_LAND_DELAY 0
 param set MIS_TAKEOFF_ALT 2.5
-param set MC_ROLLRATE_P 0.5
-param set MC_PITCHRATE_P 0.5
-param set MC_PITCH_P 5
-param set MC_ROLL_P 5
+param set MC_ROLLRATE_P 0.3
+param set MC_PITCHRATE_P 0.3
+param set MC_PITCH_P 5.5
+param set MC_ROLL_P 5.5
 param set MC_ROLLRATE_I 0.1
 param set MC_PITCHRATE_I 0.1
 param set MPC_HOLD_MAX_Z 2.0
diff --git a/posix-configs/SITL/init/rcS_gazebo_solo b/posix-configs/SITL/init/rcS_gazebo_solo
index 198bb2815d..ded76b8505 100644
--- a/posix-configs/SITL/init/rcS_gazebo_solo
+++ b/posix-configs/SITL/init/rcS_gazebo_solo
@@ -27,10 +27,10 @@ param set RTL_RETURN_ALT 30.0
 param set RTL_DESCEND_ALT 10.0
 param set RTL_LAND_DELAY 0
 param set MIS_TAKEOFF_ALT 2.5
-param set MC_ROLLRATE_P 0.25
-param set MC_PITCHRATE_P 0.25
-param set MC_PITCH_P 5
-param set MC_ROLL_P 5
+param set MC_ROLLRATE_P 0.3
+param set MC_PITCHRATE_P 0.3
+param set MC_PITCH_P 7
+param set MC_ROLL_P 7
 param set MC_ROLLRATE_I 0.1
 param set MC_PITCHRATE_I 0.1
 param set MPC_HOLD_MAX_Z 2.0
diff --git a/posix-configs/SITL/init/rcS_gazebo_tailsitter b/posix-configs/SITL/init/rcS_gazebo_tailsitter
index ebaf0bc136..5d3d44e633 100644
--- a/posix-configs/SITL/init/rcS_gazebo_tailsitter
+++ b/posix-configs/SITL/init/rcS_gazebo_tailsitter
@@ -1,10 +1,6 @@
 uorb start
 param load
 param set MAV_TYPE 20
-param set MC_PITCHRATE_P 0.25
-param set MC_ROLLRATE_P 0.25
-param set MC_PITCH_P 5
-param set MC_ROLL_P 5
 param set VT_TYPE 0
 param set SYS_AUTOSTART 4010
 param set SYS_RESTART_TYPE 2
@@ -22,6 +18,12 @@ 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 MC_ROLLRATE_P 0.3
+param set MC_PITCHRATE_P 0.3
+param set MC_PITCH_P 5.5
+param set MC_ROLL_P 5.5
+param set MC_ROLLRATE_I 0.1
+param set MC_PITCHRATE_I 0.1
 param set MPC_XY_P 0.15
 param set MPC_XY_VEL_P 0.05
 param set MPC_XY_VEL_D 0.005
diff --git a/posix-configs/SITL/init/rcS_jmavsim_iris b/posix-configs/SITL/init/rcS_jmavsim_iris
index 99501dd819..dbcc533a81 100644
--- a/posix-configs/SITL/init/rcS_jmavsim_iris
+++ b/posix-configs/SITL/init/rcS_jmavsim_iris
@@ -1,12 +1,6 @@
 uorb start
 param load
 param set MAV_TYPE 2
-param set MC_PITCHRATE_P 0.15
-param set MC_PITCH_P 7
-param set MC_ROLL_P 7
-param set MC_ROLLRATE_P 0.15
-param set MC_YAW_P 2.8
-param set MC_YAWRATE_P 0.35
 param set SYS_AUTOSTART 4010
 param set SYS_RESTART_TYPE 2
 dataman start
@@ -36,6 +30,12 @@ param set RTL_RETURN_ALT 30.0
 param set RTL_DESCEND_ALT 10.0
 param set RTL_LAND_DELAY 0
 param set MIS_TAKEOFF_ALT 2.5
+param set MC_ROLLRATE_P 0.3
+param set MC_PITCHRATE_P 0.3
+param set MC_PITCH_P 7
+param set MC_ROLL_P 7
+param set MC_ROLLRATE_I 0.1
+param set MC_PITCHRATE_I 0.1
 param set MPC_HOLD_MAX_Z 2.0
 param set MPC_HOLD_XY_DZ 0.1
 param set MPC_Z_VEL_MAX 2.0
diff --git a/posix-configs/SITL/init/rcS_lpe_gazebo_iris b/posix-configs/SITL/init/rcS_lpe_gazebo_iris
index 9ce02acf76..514ad7668a 100644
--- a/posix-configs/SITL/init/rcS_lpe_gazebo_iris
+++ b/posix-configs/SITL/init/rcS_lpe_gazebo_iris
@@ -1,10 +1,6 @@
 uorb start
 param load
 param set MAV_TYPE 2
-param set MC_PITCHRATE_P 0.3
-param set MC_ROLLRATE_P 0.3
-param set MC_YAW_P 2.0
-param set MC_YAWRATE_P 0.35
 param set SYS_AUTOSTART 4010
 param set SYS_RESTART_TYPE 2
 dataman start
@@ -21,6 +17,12 @@ 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 MC_ROLLRATE_P 0.3
+param set MC_PITCHRATE_P 0.3
+param set MC_PITCH_P 5.5
+param set MC_ROLL_P 5.5
+param set MC_ROLLRATE_I 0.1
+param set MC_PITCHRATE_I 0.1
 param set MPC_XY_P 0.15
 param set MPC_XY_VEL_P 0.05
 param set MPC_XY_VEL_D 0.005
diff --git a/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow b/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow
index bd93519031..085dc77a7b 100644
--- a/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow
+++ b/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow
@@ -1,10 +1,6 @@
 uorb start
 param load
 param set MAV_TYPE 2
-param set MC_PITCHRATE_P 0.3
-param set MC_ROLLRATE_P 0.3
-param set MC_YAW_P 2.0
-param set MC_YAWRATE_P 0.35
 param set SYS_AUTOSTART 4010
 param set SYS_RESTART_TYPE 2
 dataman start
@@ -21,6 +17,12 @@ 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 MC_ROLLRATE_P 0.3
+param set MC_PITCHRATE_P 0.3
+param set MC_PITCH_P 5.5
+param set MC_ROLL_P 5.5
+param set MC_ROLLRATE_I 0.1
+param set MC_PITCHRATE_I 0.1
 param set MPC_XY_P 0.15
 param set MPC_XY_VEL_P 0.05
 param set MPC_XY_VEL_D 0.005
diff --git a/posix-configs/SITL/init/rcS_lpe_jmavsim_iris b/posix-configs/SITL/init/rcS_lpe_jmavsim_iris
index 76685313ed..4de8c96030 100644
--- a/posix-configs/SITL/init/rcS_lpe_jmavsim_iris
+++ b/posix-configs/SITL/init/rcS_lpe_jmavsim_iris
@@ -1,12 +1,6 @@
 uorb start
 param load
 param set MAV_TYPE 2
-param set MC_PITCHRATE_P 0.15
-param set MC_PITCH_P 7
-param set MC_ROLL_P 7
-param set MC_ROLLRATE_P 0.15
-param set MC_YAW_P 2.8
-param set MC_YAWRATE_P 0.35
 param set SYS_AUTOSTART 4010
 param set SYS_RESTART_TYPE 2
 dataman start
@@ -23,6 +17,12 @@ 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 MC_ROLLRATE_P 0.3
+param set MC_PITCHRATE_P 0.3
+param set MC_PITCH_P 5.5
+param set MC_ROLL_P 5.5
+param set MC_ROLLRATE_I 0.1
+param set MC_PITCHRATE_I 0.1
 param set MPC_XY_P 0.4
 param set MPC_XY_VEL_P 0.2
 param set MPC_XY_VEL_D 0.005
diff --git a/posix-configs/SITL/init/rcS_multiple_gazebo_iris b/posix-configs/SITL/init/rcS_multiple_gazebo_iris
index 674ff3f312..a791aa8174 100644
--- a/posix-configs/SITL/init/rcS_multiple_gazebo_iris
+++ b/posix-configs/SITL/init/rcS_multiple_gazebo_iris
@@ -2,10 +2,6 @@ uorb start
 simulator start -s -u _SIMPORT_
 param load
 param set MAV_TYPE 2
-param set MC_PITCHRATE_P 0.3
-param set MC_ROLLRATE_P 0.3
-param set MC_YAW_P 2.0
-param set MC_YAWRATE_P 0.35
 param set SYS_AUTOSTART 4010
 param set SYS_RESTART_TYPE 2
 dataman start
@@ -22,6 +18,12 @@ 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 MC_ROLLRATE_P 0.3
+param set MC_PITCHRATE_P 0.3
+param set MC_PITCH_P 5.5
+param set MC_ROLL_P 5.5
+param set MC_ROLLRATE_I 0.1
+param set MC_PITCHRATE_I 0.1
 param set MPC_XY_P 0.15
 param set MPC_XY_VEL_P 0.05
 param set MPC_XY_VEL_D 0.005