From 8db37225d6059b7bcf1d129db9c00f14b7e9a619 Mon Sep 17 00:00:00 2001
From: garfieldG <garfieldG@users.noreply.github.com>
Date: Thu, 25 Feb 2021 02:30:30 -0800
Subject: [PATCH] Added rc.mavlink_override to test the new parameters with
 sitl

---
 .../px4fmu_common/init.d-posix/CMakeLists.txt |  1 +
 .../init.d-posix/rc.mavlink_override          | 30 +++++++++++++++++++
 ROMFS/px4fmu_common/init.d-posix/rcS          |  2 +-
 boards/px4/sitl/default.cmake                 |  1 +
 src/modules/mavlink/module.yaml               |  6 +++-
 5 files changed, 38 insertions(+), 2 deletions(-)
 create mode 100644 ROMFS/px4fmu_common/init.d-posix/rc.mavlink_override

diff --git a/ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt
index b64d7d8ef5..a74cc61c62 100644
--- a/ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt
+++ b/ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt
@@ -36,4 +36,5 @@ add_subdirectory(airframes)
 px4_add_romfs_files(
 	rc.replay
 	rcS
+	rc.mavlink_override
 )
diff --git a/ROMFS/px4fmu_common/init.d-posix/rc.mavlink_override b/ROMFS/px4fmu_common/init.d-posix/rc.mavlink_override
new file mode 100644
index 0000000000..79962ac40b
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d-posix/rc.mavlink_override
@@ -0,0 +1,30 @@
+#!/bin/sh
+. px4-alias.sh
+
+# using parameters to open mavlink channels
+
+for i in 0 1 2
+do
+if param greater MAV_${i}_BROADCAST 0
+then
+		set MAV_ARGS "-u p:MAV_${i}_UDP_PRT -o p:MAV_${i}_REMOTE_PRT -m p:MAV_${i}_MODE -r p:MAV_${i}_RATE"
+	
+		if param compare MAV_${i}_BROADCAST 1
+		then
+			set MAV_ARGS "${MAV_ARGS} -p"
+		fi
+	
+		if param compare MAV_${i}_BROADCAST 2
+		then
+			set MAV_ARGS "${MAV_ARGS} -c"
+		fi
+	
+		if param compare MAV_${i}_FORWARD 1
+		then
+			set MAV_ARGS "${MAV_ARGS} -f"
+		fi
+	
+		mavlink start ${MAV_ARGS}
+		echo "Starting mavlink with command: mavlink start ${MAV_ARGS}"
+fi
+done
diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS
index 6cee00668b..5c6a93b103 100644
--- a/ROMFS/px4fmu_common/init.d-posix/rcS
+++ b/ROMFS/px4fmu_common/init.d-posix/rcS
@@ -262,7 +262,7 @@ else
 	mavlink start -x -u $udp_onboard_payload_port_local -r 4000 -f -m onboard -o $udp_onboard_payload_port_remote -p
 
 	# Onboard link to gimbal
-	mavlink start -x -u $udp_onboard_gimbal_port_local -r 400000 -m gimbal -o $udp_onboard_gimbal_port_remote
+	mavlink start -x -u $udp_onboard_gimbal_port_local -r 400000 -m gimbal -o $udp_onboard_gimbal_port_remote -p
 fi
 
 # execute autostart post script if any
diff --git a/boards/px4/sitl/default.cmake b/boards/px4/sitl/default.cmake
index 217b262699..3c9b7f1533 100644
--- a/boards/px4/sitl/default.cmake
+++ b/boards/px4/sitl/default.cmake
@@ -7,6 +7,7 @@ px4_add_board(
 	LABEL default
 	EMBEDDED_METADATA parameters
 	TESTING
+	ETHERNET
 	DRIVERS
 		#barometer # all available barometer drivers
 		#batt_smbus
diff --git a/src/modules/mavlink/module.yaml b/src/modules/mavlink/module.yaml
index 45d84ab2bc..5c1ba9b2d6 100644
--- a/src/modules/mavlink/module.yaml
+++ b/src/modules/mavlink/module.yaml
@@ -10,7 +10,11 @@ serial_config:
             if param compare MAV_${i}_BROADCAST 1
             then
                 set MAV_ARGS "${MAV_ARGS} -p"
-            fi        
+            fi
+            if param compare MAV_${i}_BROADCAST 2
+            then
+                set MAV_ARGS "${MAV_ARGS} -c"
+            fi
         fi
         if param compare MAV_${i}_FORWARD 1
         then