From 6300fc4d370ee58a9ef0af62df274baa56513ae4 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Luis=20Vale=20Gonc=CC=A7alves?= <luisvale@gmail.com>
Date: Thu, 31 Aug 2017 19:19:55 +0100
Subject: [PATCH] Initial support for ArduCopter SafeRTL

---
 .../APM/APMParameterFactMetaData.Copter.3.5.xml             | 6 ++++++
 .../APM/APMParameterFactMetaData.Plane.3.7.xml              | 6 ++++++
 .../APM/APMParameterFactMetaData.Plane.3.8.xml              | 6 ++++++
 .../APM/APMParameterFactMetaData.Rover.3.2.xml              | 6 ++++++
 src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc          | 4 ++++
 src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h           | 3 ++-
 6 files changed, 30 insertions(+), 1 deletion(-)

diff --git a/src/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.5.xml b/src/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.5.xml
index 5829ed8a0..9e439ca11 100644
--- a/src/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.5.xml
+++ b/src/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.5.xml
@@ -1336,6 +1336,7 @@
 <value code="18">Throw</value>
 <value code="19">Avoid_ADSB</value>
 <value code="20">Guided_NoGPS</value>
+<value code="21">SafeRTL</value>
 </values>
 </param>
 <param humanName="Flight Mode 2" name="ArduCopter:FLTMODE2" documentation="Flight mode when Channel 5 pwm is &gt;1230, &lt;= 1360" user="Standard">
@@ -1358,6 +1359,7 @@
 <value code="18">Throw</value>
 <value code="19">Avoid_ADSB</value>
 <value code="20">Guided_NoGPS</value>
+<value code="21">SafeRTL</value>
 </values>
 </param>
 <param humanName="Flight Mode 3" name="ArduCopter:FLTMODE3" documentation="Flight mode when Channel 5 pwm is &gt;1360, &lt;= 1490" user="Standard">
@@ -1380,6 +1382,7 @@
 <value code="18">Throw</value>
 <value code="19">Avoid_ADSB</value>
 <value code="20">Guided_NoGPS</value>
+<value code="21">SafeRTL</value>
 </values>
 </param>
 <param humanName="Flight Mode 4" name="ArduCopter:FLTMODE4" documentation="Flight mode when Channel 5 pwm is &gt;1490, &lt;= 1620" user="Standard">
@@ -1402,6 +1405,7 @@
 <value code="18">Throw</value>
 <value code="19">Avoid_ADSB</value>
 <value code="20">Guided_NoGPS</value>
+<value code="21">SafeRTL</value>
 </values>
 </param>
 <param humanName="Flight Mode 5" name="ArduCopter:FLTMODE5" documentation="Flight mode when Channel 5 pwm is &gt;1620, &lt;= 1749" user="Standard">
@@ -1424,6 +1428,7 @@
 <value code="18">Throw</value>
 <value code="19">Avoid_ADSB</value>
 <value code="20">Guided_NoGPS</value>
+<value code="21">SafeRTL</value>
 </values>
 </param>
 <param humanName="Flight Mode 6" name="ArduCopter:FLTMODE6" documentation="Flight mode when Channel 5 pwm is &gt;=1750" user="Standard">
@@ -1446,6 +1451,7 @@
 <value code="18">Throw</value>
 <value code="19">Avoid_ADSB</value>
 <value code="20">Guided_NoGPS</value>
+<value code="21">SafeRTL</value>
 </values>
 </param>
 <param humanName="Simple mode bitmask" name="ArduCopter:SIMPLE" documentation="Bitmask which holds which flight modes use simple heading mode (eg bit 0 = 1 means Flight Mode 0 uses simple mode)" user="Advanced">
diff --git a/src/FirmwarePlugin/APM/APMParameterFactMetaData.Plane.3.7.xml b/src/FirmwarePlugin/APM/APMParameterFactMetaData.Plane.3.7.xml
index 07622b617..2610b0235 100644
--- a/src/FirmwarePlugin/APM/APMParameterFactMetaData.Plane.3.7.xml
+++ b/src/FirmwarePlugin/APM/APMParameterFactMetaData.Plane.3.7.xml
@@ -1094,6 +1094,7 @@
 <value code="18">Throw</value>
 <value code="19">Avoid_ADSB</value>
 <value code="20">Guided_NoGPS</value>
+<value code="21">SafeRTL</value>
 </values>
 </param>
 <param humanName="Flight Mode 2" name="ArduCopter:FLTMODE2" documentation="Flight mode when Channel 5 pwm is &gt;1230, &lt;= 1360" user="Standard">
@@ -1116,6 +1117,7 @@
 <value code="18">Throw</value>
 <value code="19">Avoid_ADSB</value>
 <value code="20">Guided_NoGPS</value>
+<value code="21">SafeRTL</value>
 </values>
 </param>
 <param humanName="Flight Mode 3" name="ArduCopter:FLTMODE3" documentation="Flight mode when Channel 5 pwm is &gt;1360, &lt;= 1490" user="Standard">
@@ -1138,6 +1140,7 @@
 <value code="18">Throw</value>
 <value code="19">Avoid_ADSB</value>
 <value code="20">Guided_NoGPS</value>
+<value code="21">SafeRTL</value>
 </values>
 </param>
 <param humanName="Flight Mode 4" name="ArduCopter:FLTMODE4" documentation="Flight mode when Channel 5 pwm is &gt;1490, &lt;= 1620" user="Standard">
@@ -1160,6 +1163,7 @@
 <value code="18">Throw</value>
 <value code="19">Avoid_ADSB</value>
 <value code="20">Guided_NoGPS</value>
+<value code="21">SafeRTL</value>
 </values>
 </param>
 <param humanName="Flight Mode 5" name="ArduCopter:FLTMODE5" documentation="Flight mode when Channel 5 pwm is &gt;1620, &lt;= 1749" user="Standard">
@@ -1182,6 +1186,7 @@
 <value code="18">Throw</value>
 <value code="19">Avoid_ADSB</value>
 <value code="20">Guided_NoGPS</value>
+<value code="21">SafeRTL</value>
 </values>
 </param>
 <param humanName="Flight Mode 6" name="ArduCopter:FLTMODE6" documentation="Flight mode when Channel 5 pwm is &gt;=1750" user="Standard">
@@ -1204,6 +1209,7 @@
 <value code="18">Throw</value>
 <value code="19">Avoid_ADSB</value>
 <value code="20">Guided_NoGPS</value>
+<value code="21">SafeRTL</value>
 </values>
 </param>
 <param humanName="Simple mode bitmask" name="ArduCopter:SIMPLE" documentation="Bitmask which holds which flight modes use simple heading mode (eg bit 0 = 1 means Flight Mode 0 uses simple mode)" user="Advanced">
diff --git a/src/FirmwarePlugin/APM/APMParameterFactMetaData.Plane.3.8.xml b/src/FirmwarePlugin/APM/APMParameterFactMetaData.Plane.3.8.xml
index 91f87f90f..db7fe3b06 100644
--- a/src/FirmwarePlugin/APM/APMParameterFactMetaData.Plane.3.8.xml
+++ b/src/FirmwarePlugin/APM/APMParameterFactMetaData.Plane.3.8.xml
@@ -1013,6 +1013,7 @@
 <value code="18">Throw</value>
 <value code="19">Avoid_ADSB</value>
 <value code="20">Guided_NoGPS</value>
+<value code="21">SafeRTL</value>
 </values>
 </param>
 <param humanName="Flight Mode 2" name="ArduCopter:FLTMODE2" documentation="Flight mode when Channel 5 pwm is &gt;1230, &lt;= 1360" user="Standard">
@@ -1035,6 +1036,7 @@
 <value code="18">Throw</value>
 <value code="19">Avoid_ADSB</value>
 <value code="20">Guided_NoGPS</value>
+<value code="21">SafeRTL</value>
 </values>
 </param>
 <param humanName="Flight Mode 3" name="ArduCopter:FLTMODE3" documentation="Flight mode when Channel 5 pwm is &gt;1360, &lt;= 1490" user="Standard">
@@ -1057,6 +1059,7 @@
 <value code="18">Throw</value>
 <value code="19">Avoid_ADSB</value>
 <value code="20">Guided_NoGPS</value>
+<value code="21">SafeRTL</value>
 </values>
 </param>
 <param humanName="Flight Mode 4" name="ArduCopter:FLTMODE4" documentation="Flight mode when Channel 5 pwm is &gt;1490, &lt;= 1620" user="Standard">
@@ -1079,6 +1082,7 @@
 <value code="18">Throw</value>
 <value code="19">Avoid_ADSB</value>
 <value code="20">Guided_NoGPS</value>
+<value code="21">SafeRTL</value>
 </values>
 </param>
 <param humanName="Flight Mode 5" name="ArduCopter:FLTMODE5" documentation="Flight mode when Channel 5 pwm is &gt;1620, &lt;= 1749" user="Standard">
@@ -1101,6 +1105,7 @@
 <value code="18">Throw</value>
 <value code="19">Avoid_ADSB</value>
 <value code="20">Guided_NoGPS</value>
+<value code="21">SafeRTL</value>
 </values>
 </param>
 <param humanName="Flight Mode 6" name="ArduCopter:FLTMODE6" documentation="Flight mode when Channel 5 pwm is &gt;=1750" user="Standard">
@@ -1123,6 +1128,7 @@
 <value code="18">Throw</value>
 <value code="19">Avoid_ADSB</value>
 <value code="20">Guided_NoGPS</value>
+<value code="21">SafeRTL</value>
 </values>
 </param>
 <param humanName="Simple mode bitmask" name="ArduCopter:SIMPLE" documentation="Bitmask which holds which flight modes use simple heading mode (eg bit 0 = 1 means Flight Mode 0 uses simple mode)" user="Advanced">
diff --git a/src/FirmwarePlugin/APM/APMParameterFactMetaData.Rover.3.2.xml b/src/FirmwarePlugin/APM/APMParameterFactMetaData.Rover.3.2.xml
index 91f87f90f..db7fe3b06 100644
--- a/src/FirmwarePlugin/APM/APMParameterFactMetaData.Rover.3.2.xml
+++ b/src/FirmwarePlugin/APM/APMParameterFactMetaData.Rover.3.2.xml
@@ -1013,6 +1013,7 @@
 <value code="18">Throw</value>
 <value code="19">Avoid_ADSB</value>
 <value code="20">Guided_NoGPS</value>
+<value code="21">SafeRTL</value>
 </values>
 </param>
 <param humanName="Flight Mode 2" name="ArduCopter:FLTMODE2" documentation="Flight mode when Channel 5 pwm is &gt;1230, &lt;= 1360" user="Standard">
@@ -1035,6 +1036,7 @@
 <value code="18">Throw</value>
 <value code="19">Avoid_ADSB</value>
 <value code="20">Guided_NoGPS</value>
+<value code="21">SafeRTL</value>
 </values>
 </param>
 <param humanName="Flight Mode 3" name="ArduCopter:FLTMODE3" documentation="Flight mode when Channel 5 pwm is &gt;1360, &lt;= 1490" user="Standard">
@@ -1057,6 +1059,7 @@
 <value code="18">Throw</value>
 <value code="19">Avoid_ADSB</value>
 <value code="20">Guided_NoGPS</value>
+<value code="21">SafeRTL</value>
 </values>
 </param>
 <param humanName="Flight Mode 4" name="ArduCopter:FLTMODE4" documentation="Flight mode when Channel 5 pwm is &gt;1490, &lt;= 1620" user="Standard">
@@ -1079,6 +1082,7 @@
 <value code="18">Throw</value>
 <value code="19">Avoid_ADSB</value>
 <value code="20">Guided_NoGPS</value>
+<value code="21">SafeRTL</value>
 </values>
 </param>
 <param humanName="Flight Mode 5" name="ArduCopter:FLTMODE5" documentation="Flight mode when Channel 5 pwm is &gt;1620, &lt;= 1749" user="Standard">
@@ -1101,6 +1105,7 @@
 <value code="18">Throw</value>
 <value code="19">Avoid_ADSB</value>
 <value code="20">Guided_NoGPS</value>
+<value code="21">SafeRTL</value>
 </values>
 </param>
 <param humanName="Flight Mode 6" name="ArduCopter:FLTMODE6" documentation="Flight mode when Channel 5 pwm is &gt;=1750" user="Standard">
@@ -1123,6 +1128,7 @@
 <value code="18">Throw</value>
 <value code="19">Avoid_ADSB</value>
 <value code="20">Guided_NoGPS</value>
+<value code="21">SafeRTL</value>
 </values>
 </param>
 <param humanName="Simple mode bitmask" name="ArduCopter:SIMPLE" documentation="Bitmask which holds which flight modes use simple heading mode (eg bit 0 = 1 means Flight Mode 0 uses simple mode)" user="Advanced">
diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
index 9a8fb896d..3d9b9c515 100644
--- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
+++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
@@ -41,6 +41,8 @@ APMCopterMode::APMCopterMode(uint32_t mode, bool settable) :
     enumToString.insert(THROW,     "Throw");
     enumToString.insert(AVOID_ADSB,"Avoid ADSB");
     enumToString.insert(GUIDED_NOGPS,"Guided No GPS");
+    enumToString.insert(SMART_RTL,"Smart RTL");
+
 
     setEnumToStringMapping(enumToString);
 }
@@ -66,6 +68,8 @@ ArduCopterFirmwarePlugin::ArduCopterFirmwarePlugin(void)
     supportedFlightModes << APMCopterMode(APMCopterMode::THROW     ,true);
     supportedFlightModes << APMCopterMode(APMCopterMode::AVOID_ADSB,true);
     supportedFlightModes << APMCopterMode(APMCopterMode::GUIDED_NOGPS,true);
+    supportedFlightModes << APMCopterMode(APMCopterMode::SMART_RTL,true);
+
 
 
     setSupportedModes(supportedFlightModes);
diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h
index e21d4f6d0..7446236c2 100644
--- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h
+++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h
@@ -41,8 +41,9 @@ public:
         THROW       = 18,
         AVOID_ADSB  = 19,
         GUIDED_NOGPS= 20,
+        SMART_RTL   = 21,   //Smart Return to Launch
     };
-    static const int modeCount = 21;
+    static const int modeCount = 22;
 
     APMCopterMode(uint32_t mode, bool settable);
 };
-- 
2.22.0