From dfdf7dce4d6dac46016fdd849968e6eb8d08a461 Mon Sep 17 00:00:00 2001
From: ChristophTobler <toblech@student.ethz.ch>
Date: Tue, 17 Nov 2015 16:38:11 +0100
Subject: [PATCH] new mode for fake gps

---
 ROMFS/px4fmu_common/init.d/rcS       | 4 ++++
 src/modules/mavlink/mavlink_main.cpp | 9 ++++++++-
 src/modules/mavlink/mavlink_main.h   | 1 +
 3 files changed, 13 insertions(+), 1 deletion(-)

diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index c4b25987be..411eb0d892 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -564,6 +564,10 @@ then
 		then
 			mavlink start -d /dev/ttyS2 -b 57600 -m osd -r 1000
 		fi
+		if param compare SYS_COMPANION 257600
+		then
+			mavlink start -d /dev/ttyS2 -b 57600 -m magic -r 5000 -x
+		fi
 		# Sensors on the PWM interface bank
 		# clear pins 5 and 6
 		if param compare SENS_EN_LL40LS 1
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 3a2f1d583b..1b04eea1a0 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -1523,6 +1523,9 @@ Mavlink::task_main(int argc, char *argv[])
 			} else if (strcmp(myoptarg, "osd") == 0) {
 				_mode = MAVLINK_MODE_OSD;
 
+			} else if (strcmp(myoptarg, "magic") == 0) {
+				_mode = MAVLINK_MODE_MAGIC;
+
 			} else if (strcmp(myoptarg, "config") == 0) {
 				_mode = MAVLINK_MODE_CONFIG;
 			}
@@ -1709,7 +1712,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("SYSTEM_TIME", 1.0f);
 		configure_stream("TIMESYNC", 10.0f);
 		configure_stream("ACTUATOR_CONTROL_TARGET0", 10.0f);
-		/* camera trigger is rate limited at the source, do not limit here */
+		//camera trigger is rate limited at the source, do not limit here
 		configure_stream("CAMERA_TRIGGER", 500.0f);
 		configure_stream("EXTENDED_SYS_STATE", 2.0f);
 		break;
@@ -1729,6 +1732,10 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("EXTENDED_SYS_STATE", 1.0f);
 		break;
 
+	case MAVLINK_MODE_MAGIC:
+		//stream nothing
+		break;
+
 	case MAVLINK_MODE_CONFIG:
 		// Enable a number of interesting streams we want via USB
 		configure_stream("SYS_STATUS", 1.0f);
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 95e0fdb91a..37c66b5963 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -149,6 +149,7 @@ public:
 		MAVLINK_MODE_CUSTOM,
 		MAVLINK_MODE_ONBOARD,
 		MAVLINK_MODE_OSD,
+		MAVLINK_MODE_MAGIC,
 		MAVLINK_MODE_CONFIG
 	};