Copter: Thr and Batt failsafe disarm when landed

When throttle/radio failsafe or battery failsafe are triggered, the
vehicle will disarm if it is landed even if the throttle is not at zero.
Auto mode will disarm if landed and mission has not started.  This
ensures that the vehicle does not disarm during a mission land command
that appears mid way through a mission.

ArduCopter/events.pde

@@ -15,25 +15,37 @@ static void failsafe_radio_on_event()
     switch(control_mode) {
         case STABILIZE:
         case ACRO:
-        case SPORT:
+            // if throttle is zero OR vehicle is landed disarm motors
-            // if throttle is zero disarm motors
+            if (g.rc_3.control_in == 0 || ap.land_complete) {
-            if (g.rc_3.control_in == 0) {
                 init_disarm_motors();
+
+            // if failsafe_throttle is FS_THR_ENABLED_ALWAYS_LAND then land immediately
             }else if(g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
-                // if failsafe_throttle is 3 (i.e. FS_THR_ENABLED_ALWAYS_LAND) land immediately
                 set_mode_land_with_pause();
+
+            // if far from home then RTL
             }else if(home_distance > wp_nav.get_wp_radius()) {
                 if (!set_mode(RTL)) {
                     set_mode_land_with_pause();
                 }
+
+            // We have no GPS or are very close to home so we will land
             }else{
-                // We have no GPS or are very close to home so we will land
                 set_mode_land_with_pause();
             }
             break;
+
         case AUTO:
-            // failsafe_throttle is 1 do RTL, 2 means continue with the mission
+            // if mission has not started AND vehicle is landed, disarm motors
-            if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_RTL) {
+            if (!ap.auto_armed && ap.land_complete) {
+                init_disarm_motors();
+
+            // if failsafe_throttle is FS_THR_ENABLED_ALWAYS_LAND then land immediately
+            } else if(g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
+                set_mode_land_with_pause();
+
+            // if failsafe_throttle is FS_THR_ENABLED_ALWAYS_RTL do RTL
+            } else if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_RTL) {
                 if(home_distance > wp_nav.get_wp_radius()) {
                     if (!set_mode(RTL)) {
                         set_mode_land_with_pause();
@@ -42,30 +54,10 @@ static void failsafe_radio_on_event()
                     // We are very close to home so we will land
                     set_mode_land_with_pause();
                 }
-            }else if(g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
-                // if failsafe_throttle is 3 (i.e. FS_THR_ENABLED_ALWAYS_LAND) land immediately
-            	set_mode_land_with_pause();
-            }
-            // if failsafe_throttle is 2 (i.e. FS_THR_ENABLED_CONTINUE_MISSION) no need to do anything
-            break;
-        case LOITER:
-        case ALT_HOLD:
-        case POSHOLD:
-            // if landed with throttle at zero disarm, otherwise do the regular thing
-            if (g.rc_3.control_in == 0 && ap.land_complete) {
-                init_disarm_motors();
-            }else if(g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
-                // if failsafe_throttle is 3 (i.e. FS_THR_ENABLED_ALWAYS_LAND) land immediately
-                set_mode_land_with_pause();
-            }else if(home_distance > wp_nav.get_wp_radius()) {
-                if (!set_mode(RTL)) {
-                    set_mode_land_with_pause();
-                }
-            }else{
-                // We have no GPS or are very close to home so we will land
-                set_mode_land_with_pause();
             }
+            // failsafe_throttle must be FS_THR_ENABLED_CONTINUE_MISSION so no need to do anything
             break;
+
         case LAND:
             // continue to land if battery failsafe is also active otherwise fall through to default handling
             if (g.failsafe_battery_enabled == FS_BATT_LAND && failsafe.battery) {
@@ -73,11 +65,19 @@ static void failsafe_radio_on_event()
             }
             // no break
         default:
-            if(g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
+            // used for AltHold, Guided, Loiter, RTL, Circle, OF_Loiter, Drift, Sport, Flip, Autotune, PosHold
-                // if failsafe_throttle is 3 (i.e. FS_THR_ENABLED_ALWAYS_LAND) land immediately
+            // if landed disarm
+            if (ap.land_complete) {
+                init_disarm_motors();
+
+            // if failsafe_throttle is FS_THR_ENABLED_ALWAYS_LAND then land immediately
+            } else if(g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
                 set_mode_land_with_pause();
+
+            // if far from home then RTL
             }else if(home_distance > wp_nav.get_wp_radius()) {
                 if (!set_mode(RTL)){
+                    // if RTL fails because of no GPS, then LAND
                     set_mode_land_with_pause();
                 }
             }else{
@@ -114,9 +114,8 @@ static void failsafe_battery_event(void)
         switch(control_mode) {
             case STABILIZE:
             case ACRO:
-            case SPORT:
+                // if throttle is zero OR vehicle is landed disarm motors
-                // if throttle is zero disarm motors
+                if (g.rc_3.control_in == 0 || ap.land_complete) {
-                if (g.rc_3.control_in == 0) {
                     init_disarm_motors();
                 }else{
                     // set mode to RTL or LAND
@@ -130,31 +129,31 @@ static void failsafe_battery_event(void)
                 }
                 break;
             case AUTO:
+                // if mission has not started AND vehicle is landed, disarm motors
+                if (!ap.auto_armed && ap.land_complete) {
+                    init_disarm_motors();
+
                 // set mode to RTL or LAND
-                if (home_distance > wp_nav.get_wp_radius()) {
+                } else if (home_distance > wp_nav.get_wp_radius()) {
                     if (!set_mode(RTL)) {
                         set_mode_land_with_pause();
                     }
-                }else{
+                } else {
                     set_mode_land_with_pause();
                 }
                 break;
-            case LOITER:
-            case ALT_HOLD:
-            case POSHOLD:
-                // if landed with throttle at zero disarm, otherwise fall through to default handling
-                if (g.rc_3.control_in == 0 && ap.land_complete) {
-                    init_disarm_motors();
-                    break;
-                }
-                // no break
             default:
+                // used for AltHold, Guided, Loiter, RTL, Circle, OF_Loiter, Drift, Sport, Flip, Autotune, PosHold
+                // if landed disarm
+                if (ap.land_complete) {
+                    init_disarm_motors();
+
                 // set mode to RTL or LAND
-                if (g.failsafe_battery_enabled == FS_BATT_RTL && home_distance > wp_nav.get_wp_radius()) {
+                } else if (g.failsafe_battery_enabled == FS_BATT_RTL && home_distance > wp_nav.get_wp_radius()) {
                     if (!set_mode(RTL)) {
                         set_mode_land_with_pause();
                     }
-                }else{
+                } else {
                     set_mode_land_with_pause();
                 }
                 break;