diff --git a/ROMFS/px4fmu_common/init.d-posix/1030_plane b/ROMFS/px4fmu_common/init.d-posix/1030_plane
index f847687289..7082d8ea4e 100644
--- a/ROMFS/px4fmu_common/init.d-posix/1030_plane
+++ b/ROMFS/px4fmu_common/init.d-posix/1030_plane
@@ -37,6 +37,8 @@ then
 
 	param set RWTO_TKOFF 1
 
+	# Distance trigger mode enabled
+	param set TRIG_MODE 4
 fi
 
 set MIXER_FILE etc/mixers-sitl/plane_sitl.main.mix
diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS
index e723ee355b..a4c5f16e43 100644
--- a/ROMFS/px4fmu_common/init.d-posix/rcS
+++ b/ROMFS/px4fmu_common/init.d-posix/rcS
@@ -110,9 +110,10 @@ simulator_tcp_port=$((4560+px4_instance))
 udp_offboard_port_local=$((14580+px4_instance))
 udp_offboard_port_remote=$((14540+px4_instance))
 [ $px4_instance -gt 9 ] && udp_offboard_port_remote=14549 # use the same ports for more than 10 instances to avoid port overlaps
+udp_onboard_payload_port_local=$((14280+px4_instance))
+udp_onboard_payload_port_remote=$((14030+px4_instance))
 udp_gcs_port_local=$((18570+px4_instance))
 
-
 if [ $AUTOCNF = yes ]
 then
 	param set SYS_AUTOSTART $REQUESTED_AUTOSTART
@@ -276,6 +277,9 @@ mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u $udp_gcs_port_local
 # API/Offboard link
 mavlink start -x -u $udp_offboard_port_local -r 4000000 -m onboard -o $udp_offboard_port_remote
 
+# Onboard link to camera
+mavlink start -x -u $udp_onboard_payload_port_local -r 4000 -f -m onboard -o $udp_onboard_payload_port_remote
+
 # execute autostart post script if any
 [ -e "$autostart_file".post ] && sh "$autostart_file".post