diff --git a/posix-configs/SITL/init/rcS b/posix-configs/SITL/init/rcS
index e8f257195aa..445b7ce3397 100644
--- a/posix-configs/SITL/init/rcS
+++ b/posix-configs/SITL/init/rcS
@@ -22,6 +22,9 @@ 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.4
+param set MPC_XY_VEL_P 0.2
+param set MPC_XY_VEL_D 0.005
 rgbled start
 tone_alarm start
 gyrosim start
@@ -32,8 +35,13 @@ gpssim start
 hil mode_pwm
 commander start
 sensors start
+navigator start
 attitude_estimator_q start
 position_estimator_inav start
 mc_pos_control start
 mc_att_control start
 mixer load /dev/pwm_output0 ../../ROMFS/px4fmu_common/mixers/quad_x.main.mix
+mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556
+mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14556
+mavlink stream -r 50 -s ATTITUDE -u 14556
+mavlink stream -r 50 -s ATTITUDE_TARGET -u 14556