From 5f1ba85ce49d172373b332019c72b70f24cb7b7b Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 2 Oct 2013 17:45:10 +0900
Subject: [PATCH] AutoTest: add copter battery failsafe test

---
 Tools/autotest/arducopter.py | 41 ++++++++++++++++++++++++++++++++++++
 Tools/autotest/common.py     |  6 +++---
 2 files changed, 44 insertions(+), 3 deletions(-)

diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py
index 448a265326..8f14672bac 100644
--- a/Tools/autotest/arducopter.py
+++ b/Tools/autotest/arducopter.py
@@ -282,6 +282,36 @@ def fly_throttle_failsafe(mavproxy, mav, side=60, timeout=180):
     wait_mode(mav, 'STABILIZE')
     return False
 
+def fly_battery_failsafe(mavproxy, mav, timeout=30):
+    # assume failure
+    success = False
+
+    # switch to loiter mode so that we hold position
+    mavproxy.send('switch 5\n')
+    wait_mode(mav, 'LOITER')
+    mavproxy.send("rc 3 1500\n")
+
+    # enable battery failsafe
+    mavproxy.send("param set FS_BATT_ENABLE 1\n")
+
+    # trigger low voltage
+    mavproxy.send('param set SIM_BATT_VOLTAGE 10\n')
+
+    # wait for LAND mode
+    if wait_mode(mav, 'LAND', timeout) == 'LAND':
+        success = True
+
+    # disable battery failsafe
+    mavproxy.send('param set FS_BATT_ENABLE 0\n')
+
+    # return status
+    if success:
+        print("Successfully entered LAND mode after battery failsafe")
+    else:
+        print("Failed to neter LAND mode after battery failsafe")
+
+    return success
+
 # fly_stability_patch - fly south, then hold loiter within 5m position and altitude and reduce 1 motor to 60% efficiency
 def fly_stability_patch(mavproxy, mav, holdtime=30, maxaltchange=5, maxdistchange=10):
     '''hold loiter position'''
@@ -859,6 +889,17 @@ def fly_ArduCopter(viewerip=None, map=False):
             print("takeoff failed")
             failed = True
 
+        # Battery failsafe
+        if not fly_battery_failsafe(mavproxy, mav):
+            print("battery failsafe failed")
+            failed = True
+
+        # Takeoff
+        print("# Takeoff")
+        if not takeoff(mavproxy, mav, 10):
+            print("takeoff failed")
+            failed = True
+
         # Stability patch
         print("#")
         print("########## Test Stability Patch ##########")
diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py
index 743c4038b8..bedae89b61 100644
--- a/Tools/autotest/common.py
+++ b/Tools/autotest/common.py
@@ -211,11 +211,11 @@ def save_wp(mavproxy, mav):
     mavproxy.send('rc 7 1000\n')
     mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==1000', blocking=True)
 
-def wait_mode(mav, mode):
-    '''wait for a flight mode to be engaged'''
+def wait_mode(mav, mode, timeout=None):
     print("Waiting for mode %s" % mode)
-    mav.recv_match(condition='MAV.flightmode.upper()=="%s".upper()' % mode, blocking=True)
+    mav.recv_match(condition='MAV.flightmode.upper()=="%s".upper()' % mode, timeout=timeout, blocking=True)
     print("Got mode %s" % mode)
+    return mav.flightmode
 
 def mission_count(filename):
     '''load a mission from a file and return number of waypoints'''