diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py
index ee5660fe02..a6a77eb299 100644
--- a/Tools/autotest/arducopter.py
+++ b/Tools/autotest/arducopter.py
@@ -152,6 +152,47 @@ class AutoTestCopter(AutoTest):
 
     def hover(self, hover_throttle=1500):
         self.set_rc(3, hover_throttle)
+    
+    #Climb/descend to a given altitude
+    def setAlt(self, desiredAlt=50):
+        pos = self.mav.location(relative_alt=True)
+        if pos.alt > desiredAlt:
+            self.set_rc(3, 1300)
+            self.wait_altitude((desiredAlt-5), desiredAlt, relative=True)
+        if pos.alt < (desiredAlt-5):
+            self.set_rc(3, 1800)
+            self.wait_altitude((desiredAlt-5), desiredAlt, relative=True)
+        self.hover()
+    
+    # Takeoff, climb to given altitude, and fly east for 10 seconds
+    def takeoffAndMoveAway(self, dAlt=50, dDist=50):
+        self.progress("Centering sticks")
+        self.set_rc(1, 1500)
+        self.set_rc(2, 1500)
+        self.set_rc(3, 1000)
+        self.set_rc(4, 1500)
+        self.takeoff(alt_min=dAlt)
+        self.change_mode("ALT_HOLD")
+
+        self.progress("Yaw to east")
+        self.set_rc(4, 1580)
+        self.wait_heading(90)
+        self.set_rc(4, 1500)
+
+        self.progress("Fly eastbound away from home")
+        self.set_rc(2, 1800)
+        self.wait_seconds(10)
+        self.set_rc(2, 1500)
+        self.hover()
+        self.progress("Cotper staging 50 meters east of home at 50 meters altitude In mode Alt Hold")
+
+    # Start and stop the GCS heartbeat for GCS failsafe testing
+    def setHearbeat(self, beating=True):
+        if beating == False:
+            self.mavproxy.send('set heartbeat 0\n')
+        else:
+            self.mavproxy.send('set heartbeat 1\n')
+        
 
     # loiter - fly south west, then loiter within 5m position and altitude
     def loiter(self, holdtime=10, maxaltchange=5, maxdistchange=5):
@@ -238,6 +279,18 @@ class AutoTestCopter(AutoTest):
             self.wait_altitude((alt_min - 5), alt_min, relative=True)
         self.hover()
 
+    def setGCSfailsafe(self,paramValue=0):
+        # Slow down the sim rate if GCS Failsafe is in use
+        if paramValue == 0:
+            self.set_parameter("FS_GCS_ENABLE",paramValue)
+            self.set_parameter("SIM_SPEEDUP",10)
+        else:
+            self.set_parameter("SIM_SPEEDUP",4)
+            self.set_parameter("FS_GCS_ENABLE",paramValue)
+
+        
+
+
     #################################################
     #   TESTS FLY
     #################################################
@@ -415,89 +468,439 @@ class AutoTestCopter(AutoTest):
         if ex is not None:
             raise ex
 
-    def fly_throttle_failsafe(self, side=60, timeout=180):
-        """Fly east, Failsafe, return, land."""
+    # Tests all actions and logic behind the radio failsafe
+    def fly_throttle_failsafe(self, side=60, timeout=360):
+        # Trigger and RC failure with the failsafe disabled. Verify no action taken.
+        self.start_subtest("Radio failsafe disabled test: FS_THR_ENABLE=0 should take no failsafe action")
+        self.set_parameter('FS_THR_ENABLE', 0)
+        self.set_parameter('FS_OPTIONS', 0)
+        self.takeoffAndMoveAway()
+        self.set_parameter("SIM_RC_FAIL", 1)
+        self.wait_seconds(5)
+        self.wait_mode("ALT_HOLD")
+        self.set_parameter("SIM_RC_FAIL", 0)
+        self.wait_seconds(5)
+        self.wait_mode("ALT_HOLD")
+        self.end_subtest("Completed Radio failsafe disabled test")
 
-        self.takeoff(10)
+        # Trigger an RC failure, verify radio failsafe triggers, restore radio, verify RC function by changing modes to cicle and stabilize.
+        self.start_subtest("Radio failsafe recovery test")
+        self.set_parameter('FS_THR_ENABLE', 1)
+        self.set_parameter("SIM_RC_FAIL", 1)
+        self.wait_mode("RTL")
+        self.wait_seconds(5)
+        self.set_parameter("SIM_RC_FAIL", 0)
+        self.wait_seconds(5)
+        self.set_rc(5, 1050)
+        self.wait_mode("CIRCLE")
+        self.set_rc(5, 1950)
+        self.wait_mode("STABILIZE")
+        self.end_subtest("Completed Radio failsafe recovery test")      
 
-        # switch to loiter mode temporarily to stop us from rising
-        self.change_mode('LOITER')
+        # Trigger and RC failure, verify failsafe triggers and RTL completes
+        self.start_subtest("Radio failsafe RTL with no options test: FS_THR_ENABLE=1 & FS_OPTIONS=0")
+        self.set_parameter("SIM_RC_FAIL", 1)
+        self.wait_mode("RTL")
+        self.mav.motors_disarmed_wait()
+        self.set_parameter("SIM_RC_FAIL", 0)
+        self.end_subtest("Completed Radio failsafe RTL with no options test")
 
-        # first aim east
-        self.progress("turn east")
-        self.set_rc(4, 1580)
-        self.wait_heading(135)
-        self.set_rc(4, 1500)
+        # Trigger and RC failure, verify failsafe triggers and land completes
+        self.start_subtest("Radio failsafe LAND with no options test: FS_THR_ENABLE=3 & FS_OPTIONS=0")
+        self.set_parameter('FS_THR_ENABLE', 3)
+        self.takeoffAndMoveAway()
+        self.set_parameter("SIM_RC_FAIL", 1)
+        self.wait_mode("LAND")
+        self.mav.motors_disarmed_wait()
+        self.set_parameter("SIM_RC_FAIL", 0)
+        self.end_subtest("Completed Radio failsafe LAND with no options test")
 
-        # raise throttle slightly to avoid hitting the ground
-        pos = self.mav.location(relative_alt=True)
-        if pos.alt > 25:
-            self.set_rc(3, 1300)
-            self.wait_altitude(20, 25, relative=True)
-        if pos.alt < 20:
-            self.set_rc(3, 1800)
-            self.wait_altitude(20, 25, relative=True)
-        self.hover()
+        # Trigger and RC failure, verify failsafe triggers and SmartRTL completes
+        self.start_subtest("Radio failsafe SmartRTL->RTL with no options test: FS_THR_ENABLE=4 & FS_OPTIONS=0")
+        self.set_parameter('FS_THR_ENABLE', 4)
+        self.takeoffAndMoveAway()
+        self.set_parameter("SIM_RC_FAIL", 1)
+        self.wait_mode("SMART_RTL")
+        self.mav.motors_disarmed_wait()
+        self.set_parameter("SIM_RC_FAIL", 0)
+        self.end_subtest("Completed Radio failsafe SmartRTL->RTL with no options test")
 
-        self.change_mode("STABILIZE")
+        # Trigger and RC failure, verify failsafe triggers and SmartRTL completes
+        self.start_subtest("Radio failsafe SmartRTL->Land with no options test: FS_THR_ENABLE=5 & FS_OPTIONS=0")
+        self.set_parameter('FS_THR_ENABLE', 5)
+        self.takeoffAndMoveAway()
+        self.set_parameter("SIM_RC_FAIL", 1)
+        self.wait_mode("SMART_RTL")
+        self.mav.motors_disarmed_wait()
+        self.set_parameter("SIM_RC_FAIL", 0)
+        self.end_subtest("Completed Radio failsafe SmartRTL_Land with no options test")
 
-        self.hover()
+        # Trigger a GPS failure and RC failure, verify RTL fails into land mode and completes
+        self.start_subtest("Radio failsafe RTL fails into land mode due to bad position.")
+        self.set_parameter('FS_THR_ENABLE', 1)
+        self.takeoffAndMoveAway()
+        self.set_parameter('SIM_GPS_DISABLE', 1)
+        self.wait_seconds(5)
+        self.set_parameter("SIM_RC_FAIL", 1)
+        self.wait_mode("LAND")
+        self.mav.motors_disarmed_wait()
+        self.set_parameter("SIM_RC_FAIL", 0)
+        self.set_parameter('SIM_GPS_DISABLE', 0)
+        self.wait_ekf_happy()
+        self.end_subtest("Completed Radio failsafe RTL fails into land mode due to bad position.")
 
-        # fly east 60 meters
-        self.progress("# Going forward %u meters" % side)
-        self.set_rc(2, 1350)
-        self.wait_distance(side, 5, 60)
-        self.set_rc(2, 1500)
+        # Trigger a GPS failure and RC failure, verify SmartRTL fails into land mode and completes
+        self.start_subtest("Radio failsafe SmartRTL->RTL fails into land mode due to bad position.")
+        self.set_parameter('FS_THR_ENABLE', 4)
+        self.takeoffAndMoveAway()
+        self.set_parameter('SIM_GPS_DISABLE', 1)
+        self.wait_seconds(5)
+        self.set_parameter("SIM_RC_FAIL", 1)
+        self.wait_mode("LAND")
+        self.mav.motors_disarmed_wait()
+        self.set_parameter("SIM_RC_FAIL", 0)
+        self.set_parameter('SIM_GPS_DISABLE', 0)
+        self.wait_ekf_happy()
+        self.end_subtest("Completed Radio failsafe SmartRTL->RTL fails into land mode due to bad position.")
 
-        # pull throttle low
-        self.progress("# Enter Failsafe by setting very low throttle")
-        self.set_rc(3, 900)
+        # Trigger a GPS failure and RC failure, verify SmartRTL fails into land mode and completes
+        self.start_subtest("Radio failsafe SmartRTL->LAND fails into land mode due to bad position.")
+        self.set_parameter('FS_THR_ENABLE', 5)
+        self.takeoffAndMoveAway()
+        self.set_parameter('SIM_GPS_DISABLE', 1)
+        self.wait_seconds(5)
+        self.set_parameter("SIM_RC_FAIL", 1)
+        self.wait_mode("LAND")
+        self.mav.motors_disarmed_wait()
+        self.set_parameter("SIM_RC_FAIL", 0)
+        self.set_parameter('SIM_GPS_DISABLE', 0)
+        self.wait_ekf_happy()
+        self.end_subtest("Completed Radio failsafe SmartRTL->LAND fails into land mode due to bad position.")
 
-        tstart = self.get_sim_time()
-        homeloc = self.poll_home_position()
-        home = mavutil.location(homeloc.latitude/1e7,
-                                homeloc.longitude/1e7,
-                                homeloc.altitude/1e3,
-                                0)
-        while self.get_sim_time_cached() < tstart + timeout:
-            m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
-            alt = m.relative_alt / 1000.0 # mm -> m
-            pos = self.mav.location()
-            home_distance = self.get_distance(home, pos )
-            self.progress("Alt: %.02f  HomeDist: %.0f" % (alt, home_distance))
-            # check if we've reached home
-            if alt <= 1 and home_distance < 10:
-                # reduce throttle
-                self.set_rc(3, 1100)
-                # switch back to stabilize
-                self.change_mode("LAND")
-                self.progress("Waiting for disarm")
-                self.mav.motors_disarmed_wait()
-                self.progress("Reached failsafe home OK")
-                self.change_mode('STABILIZE')
-                self.zero_throttle()
-                return
+        # Trigger a GPS failure, then restore the GPS. Trigger an RC failure, verify SmartRTL fails into RTL and completes
+        self.start_subtest("Radio failsafe SmartRTL->RTL fails into RTL mode due to no path.")
+        self.set_parameter('FS_THR_ENABLE', 4)
+        self.takeoffAndMoveAway()
+        self.set_parameter('SIM_GPS_DISABLE', 1)
+        self.mavproxy.expect("SmartRTL deactivated: bad position")
+        self.set_parameter('SIM_GPS_DISABLE', 0)
+        self.wait_ekf_happy()
+        self.wait_seconds(5)
+        self.set_parameter("SIM_RC_FAIL", 1)
+        self.wait_mode("RTL")
+        self.mav.motors_disarmed_wait()
+        self.set_parameter("SIM_RC_FAIL", 0)
+        self.end_subtest("Completed Radio failsafe SmartRTL->RTL fails into RTL mode due to no path.")
 
+        # Trigger a GPS failure, then restore the GPS. Trigger an RC failure, verify SmartRTL fails into Land and completes
+        self.start_subtest("Radio failsafe SmartRTL->LAND fails into land mode due to no path.")
+        self.set_parameter('FS_THR_ENABLE', 5)
+        self.takeoffAndMoveAway()
+        self.set_parameter('SIM_GPS_DISABLE', 1)
+        self.mavproxy.expect("SmartRTL deactivated: bad position")
+        self.set_parameter('SIM_GPS_DISABLE', 0)
+        self.wait_ekf_happy()
+        self.wait_seconds(5)
+        self.set_parameter("SIM_RC_FAIL", 1)
+        self.wait_mode("LAND")
+        self.mav.motors_disarmed_wait()
+        self.set_parameter("SIM_RC_FAIL", 0)
+        self.end_subtest("Completed Radio failsafe SmartRTL->LAND fails into land mode due to no path.")
+
+        # Trigger an RC failure in guided mode with the option enabled to continue in guided. Verify no failsafe action takes place
+        self.start_subtest("Radio failsafe with option to continue in guided mode: FS_THR_ENABLE=1 & FS_OPTIONS=4")
+        self.setGCSfailsafe(1)
+        self.set_parameter('FS_THR_ENABLE', 1)
+        self.set_parameter('FS_OPTIONS', 4)
+        self.takeoffAndMoveAway()
+        self.change_mode("GUIDED")
+        self.set_parameter("SIM_RC_FAIL", 1)
+        self.wait_seconds(5)
+        self.wait_mode("GUIDED")
+        self.set_parameter("SIM_RC_FAIL", 0)
+        self.wait_seconds(5)
+        self.change_mode("ALT_HOLD")
+        self.setGCSfailsafe(0)
+        # self.change_mode("RTL")
+        # self.mav.motors_disarmed_wait()
+        self.end_subtest("Completed Radio failsafe with option to continue in guided mode")
+
+        # Trigger an RC failure in AUTO mode with the option enabled to continue the mission. Verify no failsafe action takes place
+        self.start_subtest("Radio failsafe RTL with option to continue mission: FS_THR_ENABLE=1 & FS_OPTIONS=1")
+        self.set_parameter('FS_OPTIONS', 1)
+        self.progress("# Load copter_mission")
+        num_wp = self.load_mission("copter_mission.txt")
+        if not num_wp:
+            raise NotAchievedException("load copter_mission failed")
+        self.takeoffAndMoveAway()
+        self.change_mode("AUTO")
+        self.set_parameter("SIM_RC_FAIL", 1)
+        self.wait_seconds(5)
+        self.wait_mode("AUTO")
+        self.set_parameter("SIM_RC_FAIL", 0)
+        self.wait_seconds(5)
+        self.wait_mode("AUTO")
+        # self.change_mode("RTL")
+        # self.mav.motors_disarmed_wait()
+        self.end_subtest("Completed Radio failsafe RTL with option to continue mission")
+
+        # Trigger an RC failure in AUTO mode without the option enabled to continue. Verify failsafe triggers and RTL completes
+        self.start_subtest("Radio failsafe RTL in mission without option to continue should RTL: FS_THR_ENABLE=1 & FS_OPTIONS=0")
+        self.set_parameter('FS_OPTIONS', 0)
+        self.set_parameter("SIM_RC_FAIL", 1)
+        self.wait_mode("RTL")
+        self.mav.motors_disarmed_wait()
+        self.clear_mission_using_mavproxy()
+        self.set_parameter("SIM_RC_FAIL", 0)
+        self.end_subtest("Completed Radio failsafe RTL in mission without option to continue")
+
+        self.progress("All radio failsafe tests complete")
+        self.set_parameter('FS_THR_ENABLE', 0)
+        self.reboot_sitl()
+        
+    # Tests all actions and logic behind the GCS failsafe
+    def fly_gcs_failsafe(self, side=60, timeout=360):
+        # Trigger telemety loss with failsafe disabled. Verify no action taken.
+        self.start_subtest("GCS failsafe disabled test: FS_GCS_ENABLE=0 should take no failsafe action")
+        self.setGCSfailsafe(0)
+        self.takeoffAndMoveAway()
+        self.setHearbeat(False)
+        self.wait_seconds(5)
+        self.wait_mode("ALT_HOLD")
+        self.setHearbeat()
+        self.wait_seconds(5)
+        self.wait_mode("ALT_HOLD")
+        self.end_subtest("Completed GCS failsafe disabled test")
+
+        # Trigger telemetry loss with failsafe enabled. Verify failsafe tirggers to RTL. Restory telemety, verify failsafe clears, and change modes.
+        self.start_subtest("GCS failsafe recovery test")
+        self.setGCSfailsafe(1)
+        self.setHearbeat(False)
+        self.wait_mode("RTL")
+        self.setHearbeat()
+        self.mavproxy.expect("GCS Failsafe Cleared")
+        self.change_mode("LOITER")
+        self.end_subtest("Completed GCS failsafe recovery test")
+
+        # Trigger telemetry loss with failsafe enabled. Verify failsafe tirggers and RTL completes
+        self.start_subtest("GCS failsafe RTL with no options test: FS_GCS_ENABLE=1 & FS_OPTIONS=0")
+        self.setHearbeat(False)
+        self.wait_mode("RTL")
+        self.mav.motors_disarmed_wait()
+        self.setHearbeat()
+        self.mavproxy.expect("GCS Failsafe Cleared")
+        self.end_subtest("Completed GCS failsafe RTL with no options test")
+
+        # Trigger telemetry loss with failsafe enabled. Verify failsafe tirggers and land completes
+        self.start_subtest("GCS failsafe LAND with no options test: FS_GCS_ENABLE=5 & FS_OPTIONS=0")
+        self.setGCSfailsafe(5)
+        self.takeoffAndMoveAway()
+        self.setHearbeat(False)
+        self.wait_mode("LAND")
+        self.mav.motors_disarmed_wait()
+        self.setHearbeat()
+        self.mavproxy.expect("GCS Failsafe Cleared")
+        self.end_subtest("Completed GCS failsafe land with no options test")
+
+        # Trigger telemetry loss with failsafe enabled. Verify failsafe tirggers and SmartRTL completes
+        self.start_subtest("GCS failsafe SmartRTL->RTL with no options test: FS_GCS_ENABLE=3 & FS_OPTIONS=0")
+        self.setGCSfailsafe(3)
+        self.takeoffAndMoveAway()
+        self.setHearbeat(False)
+        self.wait_mode("SMART_RTL")
+        self.mav.motors_disarmed_wait()
+        self.setHearbeat()
+        self.mavproxy.expect("GCS Failsafe Cleared")
+        self.end_subtest("Completed GCS failsafe SmartRTL->RTL with no options test")
+
+        # Trigger telemetry loss with failsafe enabled. Verify failsafe tirggers and SmartRTL completes
+        self.start_subtest("GCS failsafe SmartRTL->Land with no options test: FS_GCS_ENABLE=4 & FS_OPTIONS=0")
+        self.setGCSfailsafe(4)
+        self.takeoffAndMoveAway()
+        self.setHearbeat(False)
+        self.wait_mode("SMART_RTL")
+        self.mav.motors_disarmed_wait()
+        self.setHearbeat()
+        self.mavproxy.expect("GCS Failsafe Cleared")
+        self.end_subtest("Completed GCS failsafe SmartRTL->Land with no options test")
+
+        # Trigger telemetry loss with an invalid failsafe value. Verify failsafe tirggers and RTL completes
+        self.start_subtest("GCS failsafe invalid value with no options test: FS_GCS_ENABLE=99 & FS_OPTIONS=0")
+        self.setGCSfailsafe(99)
+        self.takeoffAndMoveAway()
+        self.setHearbeat(False)
+        self.wait_mode("RTL")
+        self.mav.motors_disarmed_wait()
+        self.setHearbeat()
+        self.mavproxy.expect("GCS Failsafe Cleared")
+        self.end_subtest("Completed GCS failsafe invalid value with no options test")
+
+        # Trigger telemetry loss with failsafe enabled to test FS_OPTIONS settings
+        self.start_subtest("GCS failsafe with option bit tests: FS_GCS_ENABLE=1 & FS_OPTIONS=64/2/16")
+        num_wp = self.load_mission("copter_mission.txt")
+        if not num_wp:
+            raise NotAchievedException("load copter_mission failed")
+        self.setGCSfailsafe(1)
+        self.set_parameter('FS_OPTIONS', 16)
+        self.takeoffAndMoveAway()
+        self.progress("Testing continue in pilot controlled modes")
+        self.setHearbeat(False)
+        self.mavproxy.expect("GCS Failsafe - Continuing Pilot Control")
+        self.wait_seconds(5)
+        self.wait_mode("ALT_HOLD")
+        self.setHearbeat()
+        self.mavproxy.expect("GCS Failsafe Cleared")
+
+        self.progress("Testing continue in auto mission")
+        self.set_parameter('FS_OPTIONS', 2)
+        self.change_mode("AUTO")
+        self.wait_seconds(5)
+        self.setHearbeat(False)
+        self.mavproxy.expect("GCS Failsafe - Continuing Auto Mode")
+        self.wait_seconds(5)
+        self.wait_mode("AUTO")
+        self.setHearbeat()
+        self.mavproxy.expect("GCS Failsafe Cleared")
+
+        self.progress("Testing continue landing in land mode")
+        self.set_parameter('FS_OPTIONS', 8)
+        self.change_mode("LAND")
+        self.wait_seconds(5)
+        self.setHearbeat(False)
+        self.mavproxy.expect("GCS Failsafe - Continuing Landing")
+        self.wait_seconds(5)
+        self.wait_mode("LAND")
+        self.mav.motors_disarmed_wait()
+        self.setHearbeat()
+        self.mavproxy.expect("GCS Failsafe Cleared")
+        self.end_subtest("Completed GCS failsafe with option bits")
+
+        self.setGCSfailsafe(0)
+        self.set_parameter('FS_OPTIONS', 0)
+        self.progress("All GCS failsafe tests complete")
         self.reboot_sitl()
 
-        raise AutoTestTimeoutException(
-            ("Failed to land and disarm on failsafe RTL - "
-             "timed out after %u seconds" % timeout))
-
+    # Tests all actions and logic behind the battery failsafe
     def fly_battery_failsafe(self, timeout=300):
-        self.takeoff(10, mode='LOITER')
+        self.progress("Configure battery failsafe parameters")
+        self.set_parameter('SIM_SPEEDUP', 4)
+        self.set_parameter('BATT_LOW_VOLT', 11.5)
+        self.set_parameter('BATT_CRT_VOLT', 10.1)
+        self.set_parameter('BATT_FS_LOW_ACT', 0)
+        self.set_parameter('BATT_FS_CRT_ACT', 0)
+        self.set_parameter('FS_OPTIONS', 0)
+        self.set_parameter('SIM_BATT_VOLTAGE', 12.5)
 
-        # enable battery failsafe
-        self.set_parameter('BATT_FS_LOW_ACT', 1)
-
-        # trigger low voltage
-        self.set_parameter('SIM_BATT_VOLTAGE', 10)
-
-        # wait for LAND mode. If unsuccessful an exception will be raised
-        self.wait_mode('LAND', timeout=timeout)
-
-        self.progress("Successfully entered LAND after battery failsafe")
+        # Trigger low battery condition with failsafe disabled. Verify no action taken.
+        self.start_subtest("Batt failsafe disabled test")
+        self.takeoffAndMoveAway()
+        self.set_parameter('SIM_BATT_VOLTAGE', 11.4)
+        self.mavproxy.expect("Battery 1 is low")
+        self.wait_seconds(5)
+        self.wait_mode("ALT_HOLD")
+        self.set_parameter('SIM_BATT_VOLTAGE', 10.0)
+        self.mavproxy.expect("Battery 1 is critical")
+        self.wait_seconds(5)
+        self.wait_mode("ALT_HOLD")
+        self.change_mode("RTL")
         self.mav.motors_disarmed_wait()
+        self.set_parameter('SIM_BATT_VOLTAGE', 12.5)
+        self.mavproxy.send('reboot\n')
+        self.end_subtest("Completed Batt failsafe disabled test")
+
+        # TWO STAGE BATTERY FAILSAFE: Trigger low battery condition, then critical battery condition. Verify RTL and Land actions complete.
+        self.start_subtest("Two stage battery failsafe test with RTL and Land")
+        self.takeoffAndMoveAway()
+        self.wait_seconds(3)
+        self.set_parameter('BATT_FS_LOW_ACT', 2)
+        self.set_parameter('BATT_FS_CRT_ACT', 1)
+        self.set_parameter('SIM_BATT_VOLTAGE', 11.4)
+        self.mavproxy.expect("Battery 1 is low")
+        self.wait_seconds(5)
+        self.wait_mode("RTL")
+        self.wait_seconds(10)
+        self.set_parameter('SIM_BATT_VOLTAGE', 10.0)
+        self.mavproxy.expect("Battery 1 is critical")
+        self.wait_seconds(5)
+        self.wait_mode("LAND")
+        self.mav.motors_disarmed_wait()
+        self.set_parameter('SIM_BATT_VOLTAGE', 12.5)
+        self.mavproxy.send('reboot\n')
+        self.end_subtest("Completed two stage battery failsafe test with RTL and Land")
+
+        # TWO STAGE BATTERY FAILSAFE: Trigger low battery condition, then critical battery condition. Verify both SmartRTL actions complete
+        self.start_subtest("Two stage battery failsafe test with SmartRTL")
+        self.takeoffAndMoveAway()
+        self.set_parameter('BATT_FS_LOW_ACT', 3)
+        self.set_parameter('BATT_FS_CRT_ACT', 4)
+        self.wait_seconds(10)
+        self.set_parameter('SIM_BATT_VOLTAGE', 11.4)
+        self.mavproxy.expect("Battery 1 is low")
+        self.wait_seconds(5)
+        self.wait_mode("SMART_RTL")
+        self.change_mode("LOITER")
+        self.wait_seconds(10)
+        self.set_parameter('SIM_BATT_VOLTAGE', 10.0)
+        self.mavproxy.expect("Battery 1 is critical")
+        self.wait_seconds(5)
+        self.wait_mode("SMART_RTL")
+        self.mav.motors_disarmed_wait()
+        self.set_parameter('SIM_BATT_VOLTAGE', 12.5)
+        self.mavproxy.send('reboot\n')
+        self.end_subtest("Completed two stage battery failsafe test with SmartRTL")
+
+        # Trigger low battery condition in land mode with FS_OPTIONS set to allow land mode to continue. Verify landing completes uninterupted.
+        self.start_subtest("Battery failsafe with FS_OPTIONS set to continue landing")
+        self.takeoffAndMoveAway()
+        self.set_parameter('FS_OPTIONS', 8)
+        self.change_mode("LAND")
+        self.wait_seconds(5)
+        self.set_parameter('SIM_BATT_VOLTAGE', 11.4)
+        self.mavproxy.expect("Battery 1 is low")
+        self.wait_seconds(5)
+        self.wait_mode("LAND")
+        self.mav.motors_disarmed_wait()
+        self.set_parameter('SIM_BATT_VOLTAGE', 12.5)
+        self.mavproxy.send('reboot\n')
+        self.end_subtest("Completed battery failsafe with FS_OPTIONS set to continue landing")
+
+        # Trigger a critical battery condition, which triggers a land mode failsafe. Trigger an RC failure. Verify the RC failsafe is prevented from stopping the low battery landing.
+        self.start_subtest("Battery failsafe critical landing")
+        self.takeoffAndMoveAway(100,50)
+        self.set_parameter('FS_OPTIONS', 0)
+        self.set_parameter('BATT_FS_LOW_ACT', 1)
+        self.set_parameter('BATT_FS_CRT_ACT', 1)
+        self.set_parameter('FS_THR_ENABLE', 1)
+        self.wait_seconds(5)
+        self.set_parameter('SIM_BATT_VOLTAGE', 10.0)
+        self.mavproxy.expect("Battery 1 is critical")
+        self.wait_mode("LAND")
+        self.wait_seconds(10)
+        self.set_parameter("SIM_RC_FAIL", 1)
+        self.wait_seconds(10)
+        self.wait_mode("LAND")
+        self.mav.motors_disarmed_wait()
+        self.set_parameter('SIM_BATT_VOLTAGE', 12.5)
+        self.set_parameter("SIM_RC_FAIL", 0)
+        self.mavproxy.send('reboot\n')
+        self.end_subtest("Completed battery failsafe critical landing")
+
+        # Trigger low battery condition with failsafe set to terminate. Copter will disarm and crash.
+        self.start_subtest("Battery failsafe terminate")
+        self.takeoffAndMoveAway()
+        self.set_parameter('BATT_FS_LOW_ACT', 5)
+        self.wait_seconds(10)
+        self.set_parameter('SIM_BATT_VOLTAGE', 11.4)
+        self.mavproxy.expect("Battery 1 is low")
+        self.mav.motors_disarmed_wait()
+        self.end_subtest("Completed terminate failsafe test")
+
+        self.progress("All Battery failsafe tests complete")
+        self.set_parameter('BATT_LOW_VOLT', 0)
+        self.set_parameter('BATT_CRT_VOLT', 0)
+        self.set_parameter('BATT_FS_LOW_ACT', 0)
+        self.set_parameter('BATT_FS_CRT_ACT', 0)
+        self.set_parameter('FS_OPTIONS', 0)
         self.reboot_sitl()
 
     # fly_stability_patch - fly south, then hold loiter within 5m
@@ -569,12 +972,12 @@ class AutoTestCopter(AutoTest):
                 self.progress("Armed")
                 return
 
-    def wait_distance_from_home(self, distance_min, distance_max, timeout=10):
+    def wait_distance_from_home(self, distance_min, distance_max, timeout=10, use_cached_home=True):
         tstart = self.get_sim_time()
         while True:
             if self.get_sim_time() - tstart > timeout:
                 raise NotAchievedException("Did not achieve distance from home")
-            distance = self.distance_to_home(use_cached_home=True)
+            distance = self.distance_to_home(use_cached_home)
             self.progress("Distance from home: now=%f %f<want<%f" %
                           (distance, distance_min, distance_max))
             if distance >= distance_min and distance <= distance_max:
@@ -3597,23 +4000,6 @@ class AutoTestCopter(AutoTest):
         self.wait_current_waypoint(0, timeout=10)
         self.set_rc(7, 1000)
 
-    def test_radio_failsafe(self):
-        self.start_subtest("If you haven't taken off yet RC failure should be instant disarm")
-        self.change_mode("STABILIZE")
-        self.set_parameter("DISARM_DELAY", 0)
-        self.arm_vehicle()
-        self.set_parameter("SIM_RC_FAIL", 1)
-        self.disarm_wait(timeout=1)
-        self.set_parameter("SIM_RC_FAIL", 0)
-        self.set_parameter("DISARM_DELAY", 10)
-
-        self.start_subtest("Default behavour from loiter should be RTL")
-        self.takeoff(10, mode="LOITER")
-        self.set_parameter("SIM_RC_FAIL", 1)
-        self.wait_mode("RTL")
-        self.disarm_wait(timeout=100)
-
-
     def tests(self):
         '''return list of all tests'''
         ret = super(AutoTestCopter, self).tests()
@@ -3677,6 +4063,10 @@ class AutoTestCopter(AutoTest):
              "Test Throttle Failsafe",
              self.fly_throttle_failsafe),
 
+            ("GCSFailsafe",
+             "Test GCS Failsafe",
+             self.fly_gcs_failsafe),
+
             ("BatteryFailsafe",
              "Fly Battery Failsafe",
              self.fly_battery_failsafe),
@@ -3725,10 +4115,6 @@ class AutoTestCopter(AutoTest):
              "Test Loiter Mode",
              self.loiter),
 
-            ("RadioFailsafe",
-             "Test radio failsafes",
-             self.test_radio_failsafe),
-
             ("SimpleMode",
              "Fly in SIMPLE mode",
              self.fly_simple),
diff --git a/Tools/autotest/default_params/copter.parm b/Tools/autotest/default_params/copter.parm
index 32043171ba..cae431bb2e 100644
--- a/Tools/autotest/default_params/copter.parm
+++ b/Tools/autotest/default_params/copter.parm
@@ -45,6 +45,7 @@ FLTMODE3        6
 FLTMODE4        3
 FLTMODE5        5
 FLTMODE6		0
+FS_GCS_ENABLE   0
 SUPER_SIMPLE	0
 SIM_GPS_DELAY   1
 SIM_ACC_RND     0