From a9e9ff8d082aa628cfb324135da6510d3b070978 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Wed, 11 Nov 2015 12:44:03 +0100
Subject: [PATCH] British Safety Rules: Failsafe on RC loss in all cases

---
 conf/airframes/TUDelft/airframes/logo600.xml | 1 +
 sw/airborne/firmwares/rotorcraft/main_fbw.c  | 8 +++++++-
 sw/airborne/firmwares/rotorcraft/main_fbw.h  | 7 +++++++
 3 files changed, 15 insertions(+), 1 deletion(-)

diff --git a/conf/airframes/TUDelft/airframes/logo600.xml b/conf/airframes/TUDelft/airframes/logo600.xml
index a13ecdb783..39868320fe 100644
--- a/conf/airframes/TUDelft/airframes/logo600.xml
+++ b/conf/airframes/TUDelft/airframes/logo600.xml
@@ -45,6 +45,7 @@
       <configure name="ACTUATORS_SPEKTRUM_DEV2" value="UART6"/>
     </subsystem>
     <define name="RC_LOST_FBW_MODE" value="FBW_MODE_FAILSAFE"/><!-- Switch to Failsafe or to Autopilot on RC loss? -->
+    <define name="RC_LOST_IN_AUTO_FBW_MODE" value="FBW_MODE_FAILSAFE"/><!-- Switch to Failsafe with a working autopilot on RC loss? -->
     <define name="AP_LOST_FBW_MODE" value="FBW_MODE_MANUAL"/><!-- Switch to Failsafe or to Manual on AP loss? -->
 
     <subsystem name="intermcu"     type="uart">
diff --git a/sw/airborne/firmwares/rotorcraft/main_fbw.c b/sw/airborne/firmwares/rotorcraft/main_fbw.c
index 7ffa741a90..e57c84e647 100644
--- a/sw/airborne/firmwares/rotorcraft/main_fbw.c
+++ b/sw/airborne/firmwares/rotorcraft/main_fbw.c
@@ -159,7 +159,13 @@ STATIC_INLINE void main_periodic(void)
       if (fbw_mode == FBW_MODE_MANUAL) {
         fbw_mode = RC_LOST_FBW_MODE;
       } else {
-        // No change: failsafe stays failsafe, auto stays auto
+        if (fbw_mode == FBW_MODE_FAILSAFE)
+        {
+          // No change: failsafe stays failsafe
+        } else {
+          // Lost RC while in working Auto mode
+          fbw_mode = RC_LOST_IN_AUTO_FBW_MODE;
+        }
       }
     }
   } else { // rc_is_good
diff --git a/sw/airborne/firmwares/rotorcraft/main_fbw.h b/sw/airborne/firmwares/rotorcraft/main_fbw.h
index 006f35cc52..9af195a4f2 100644
--- a/sw/airborne/firmwares/rotorcraft/main_fbw.h
+++ b/sw/airborne/firmwares/rotorcraft/main_fbw.h
@@ -47,11 +47,18 @@
 #define RC_LOST_FBW_MODE FBW_MODE_FAILSAFE
 #endif
 
+/** mode to enter when AP is lost while using autopilot */
+#ifndef RC_LOST_IN_AUTO_FBW_MODE
+#define RC_LOST_IN_AUTO_FBW_MODE FBW_MODE_AUTO
+#endif
+
 /** mode to enter when AP is lost while using autopilot */
 #ifndef AP_LOST_FBW_MODE
 #define AP_LOST_FBW_MODE FBW_MODE_FAILSAFE
 #endif
 
+
+
 /** Switching between FBW and autopilot is done with RADIO_FBW_MODE: default is to re-use RADIO_MODE */
 
 #ifndef RADIO_FBW_MODE