From 87127f0ab89f6b857251c5eb84533e8ff8c341dc Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Sat, 16 Nov 2013 14:46:57 +0900
Subject: [PATCH] Copter: allow battery failsafe to trigger RTL

---
 ArduCopter/Parameters.pde |  4 ++--
 ArduCopter/config.h       |  5 -----
 ArduCopter/defines.h      |  5 +++++
 ArduCopter/events.pde     | 23 +++++++++++++----------
 ArduCopter/setup.pde      |  2 +-
 5 files changed, 21 insertions(+), 18 deletions(-)

diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde
index 407ad2279d..27b4ebed80 100644
--- a/ArduCopter/Parameters.pde
+++ b/ArduCopter/Parameters.pde
@@ -98,9 +98,9 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Param: FS_BATT_ENABLE
     // @DisplayName: Battery Failsafe Enable
     // @Description: Controls whether failsafe will be invoked when battery voltage or current runs low
-    // @Values: 0:Disabled,1:Enabled
+    // @Values: 0:Disabled,1:Land,2:RTL
     // @User: Standard
-    GSCALAR(failsafe_battery_enabled, "FS_BATT_ENABLE", FS_BATTERY),
+    GSCALAR(failsafe_battery_enabled, "FS_BATT_ENABLE", FS_BATT_DISABLED),
 
     // @Param: FS_BATT_VOLTAGE
     // @DisplayName: Failsafe battery voltage
diff --git a/ArduCopter/config.h b/ArduCopter/config.h
index 6ebf6a51b6..1350aea693 100644
--- a/ArduCopter/config.h
+++ b/ArduCopter/config.h
@@ -356,11 +356,6 @@
  # define BOARD_VOLTAGE_MAX             5800        // max board voltage in milli volts for pre-arm checks
 #endif
 
-// Battery failsafe
-#ifndef FS_BATTERY
- # define FS_BATTERY              DISABLED
-#endif
-
 // GPS failsafe
 #ifndef FAILSAFE_GPS_TIMEOUT_MS
  # define FAILSAFE_GPS_TIMEOUT_MS       5000    // gps failsafe triggers after 5 seconds with no GPS
diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h
index dd458a3b95..fe3dd8f5d4 100644
--- a/ArduCopter/defines.h
+++ b/ArduCopter/defines.h
@@ -485,6 +485,11 @@ enum ap_message {
 #define FS_THR_ENABLED_CONTINUE_MISSION    2
 #define FS_THR_ENABLED_ALWAYS_LAND         3
 
+// Battery failsafe definitions (FS_BATT_ENABLE parameter)
+#define FS_BATT_DISABLED                    0       // battery failsafe disabled
+#define FS_BATT_LAND                        1       // switch to LAND mode on battery failsafe
+#define FS_BATT_RTL                         2       // switch to RTL mode on battery failsafe
+
 // GPS Failsafe definitions (FS_GPS_ENABLE parameter)
 #define FS_GPS_DISABLED                     0       // GPS failsafe disabled
 #define FS_GPS_LAND                         1       // switch to LAND mode on GPS Failsafe
diff --git a/ArduCopter/events.pde b/ArduCopter/events.pde
index cc2afb393f..58cf2fdd40 100644
--- a/ArduCopter/events.pde
+++ b/ArduCopter/events.pde
@@ -50,7 +50,7 @@ static void failsafe_radio_on_event()
             break;
         case LAND:
             // continue to land if battery failsafe is also active otherwise fall through to default handling
-            if (g.failsafe_battery_enabled && failsafe.battery) {
+            if (g.failsafe_battery_enabled == FS_BATT_LAND && failsafe.battery) {
                 break;
             }
         default:
@@ -91,7 +91,7 @@ static void failsafe_battery_event(void)
     }
 
     // failsafe check
-    if (g.failsafe_battery_enabled && motors.armed()) {
+    if (g.failsafe_battery_enabled != FS_BATT_DISABLED && motors.armed()) {
         switch(control_mode) {
             case STABILIZE:
             case ACRO:
@@ -100,23 +100,26 @@ static void failsafe_battery_event(void)
                 if (g.rc_3.control_in == 0) {
                     init_disarm_motors();
                 }else{
-                    set_mode(LAND);
+                    // set mode to RTL or LAND
+                    if (g.failsafe_battery_enabled == FS_BATT_RTL && home_distance > wp_nav.get_waypoint_radius()) {
+                        if (!set_mode(RTL)) {
+                            set_mode(LAND);
+                        }
+                    }else{
+                        set_mode(LAND);
+                    }
                 }
                 break;
-            case AUTO:
-                // set_mode to RTL or LAND
-                if (home_distance > wp_nav.get_waypoint_radius()) {
+            default:
+                // set mode to RTL or LAND
+                if (g.failsafe_battery_enabled == FS_BATT_RTL && home_distance > wp_nav.get_waypoint_radius()) {
                     if (!set_mode(RTL)) {
                         set_mode(LAND);
                     }
                 }else{
-                    // We have no GPS or are very close to home so we will land
                     set_mode(LAND);
                 }
                 break;
-            default:
-                set_mode(LAND);
-                break;
         }
     }
 
diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde
index 16e02508ed..c85c858509 100644
--- a/ArduCopter/setup.pde
+++ b/ArduCopter/setup.pde
@@ -164,7 +164,7 @@ setup_compassmot(uint8_t argc, const Menu::arg *argv)
 
     // disable throttle and battery failsafe
     g.failsafe_throttle = FS_THR_DISABLED;
-    g.failsafe_battery_enabled = false;
+    g.failsafe_battery_enabled = FS_BATT_DISABLED;
 
     // read radio
     read_radio();