Plane: add arming switch safety (ArduPilot#30)

ArduPlane/AP_Arming.cpp

@@ -298,6 +298,15 @@ void AP_Arming_Plane::change_arm_state(void)
 
 bool AP_Arming_Plane::arm(const AP_Arming::Method method, const bool do_arming_checks)
 {
+    if (throttle_cut) {
+        if (throttle_cut_prev_mode && plane.control_mode == &plane.mode_fbwa) {
+            plane.set_mode(*throttle_cut_prev_mode, ModeReason::RC_COMMAND);
+        }
+        set_throttle_cut(false);
+        gcs().send_text(MAV_SEVERITY_INFO , "Rearmed");
+        return true;
+    }
+
     if (!AP_Arming::arm(method, do_arming_checks)) {
         return false;
     }
@@ -312,26 +321,47 @@ bool AP_Arming_Plane::arm(const AP_Arming::Method method, const bool do_arming_c
     return true;
 }
 
+void AP_Arming_Plane::set_throttle_cut(bool status)
+{
+    throttle_cut = status;
+    AP_Notify::flags.throttle_cut = status;
+}
+
 /*
   disarm motors
  */
 bool AP_Arming_Plane::disarm(const AP_Arming::Method method, bool do_disarm_checks)
 {
-    if (do_disarm_checks &&
-        (method == AP_Arming::Method::MAVLINK ||
-         method == AP_Arming::Method::RUDDER)) {
+    if (do_disarm_checks) {
+
+        // don't allow disarming in flight, cut the throttle instead and change mode to FBWA if in auto throttle mode
+#if CONFIG_HAL_BOARD != HAL_BOARD_SITL
         if (plane.is_flying()) {
-            // don't allow mavlink or rudder disarm while flying
+            if (method == AP_Arming::Method::AUXSWITCH) {
+                set_throttle_cut(true);
+                if (plane.control_mode->does_auto_throttle()) {
+                    throttle_cut_prev_mode = plane.control_mode;
+                    plane.set_mode(plane.mode_fbwa, ModeReason::RC_COMMAND);
+                } else {
+                    throttle_cut_prev_mode = NULL;
+                }
+                gcs().send_text(MAV_SEVERITY_INFO , "Throttle cut by arm switch");
+            }
+            // obviously this could happen in-flight so we can't warn about it
             return false;
         }
-    }
-
-    if (do_disarm_checks && method == AP_Arming::Method::RUDDER) {
+#else
+        if (plane.is_flying() && (method == AP_Arming::Method::RUDDER || method == AP_Arming::Method::MAVLINK)) {
+            return false;
+        }
+#endif
+
         // option must be enabled:
-        if (get_rudder_arming_type() != AP_Arming::RudderArming::ARMDISARM) {
+        if (method == AP_Arming::Method::RUDDER && get_rudder_arming_type() != AP_Arming::RudderArming::ARMDISARM) {
             gcs().send_text(MAV_SEVERITY_INFO, "Rudder disarm: disabled");
             return false;
         }
+
     }
 
     if (!AP_Arming::disarm(method, do_disarm_checks)) {
@@ -354,6 +384,7 @@ bool AP_Arming_Plane::disarm(const AP_Arming::Method method, bool do_disarm_chec
 
     //only log if disarming was successful
     change_arm_state();
+    set_throttle_cut(false);
 
 #if QAUTOTUNE_ENABLED
     //save qautotune gains if enabled and success
@@ -373,6 +404,13 @@ bool AP_Arming_Plane::disarm(const AP_Arming::Method method, bool do_disarm_chec
     return true;
 }
 
+void AP_Arming_Plane::disarm_if_requested()
+{
+    if (throttle_cut) {
+        disarm(AP_Arming::Method::AUXSWITCH, false);
+    }
+}
+
 void AP_Arming_Plane::update_soft_armed()
 {
     bool _armed = is_armed();

ArduPlane/AP_Arming.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <AP_Arming/AP_Arming.h>
+#include "mode.h"
 
 /*
   a plane specific arming class
@@ -23,12 +24,16 @@ class AP_Arming_Plane : public AP_Arming
     // var_info for holding Parameter information
     static const struct AP_Param::GroupInfo var_info[];
 
+    void disarm_if_requested();
     bool disarm(AP_Arming::Method method, bool do_disarm_checks=true) override;
     bool arm(AP_Arming::Method method, bool do_arming_checks=true) override;
 
     void update_soft_armed();
     bool get_delay_arming() const { return delay_arming; };
 
+    void set_throttle_cut(bool status);
+    bool get_throttle_cut() const { return throttle_cut; };
+
 protected:
     bool ins_checks(bool report) override;
     bool terrain_database_required() const override;
@@ -39,6 +44,12 @@ class AP_Arming_Plane : public AP_Arming
 private:
     void change_arm_state(void);
 
+    // throttle cut when trying to disarm but the plane is still flying
+    bool throttle_cut = false;
+
+    // mode the plane was in when throttle cut was enabled
+    Mode *throttle_cut_prev_mode;
+
     // oneshot with duration AP_ARMING_DELAY_MS used by quadplane to delay spoolup after arming:
     // ignored unless OPTION_DELAY_ARMING or OPTION_TILT_DISARMED is set
     bool delay_arming;

ArduPlane/RC_Channel.cpp

@@ -51,6 +51,22 @@ void RC_Channel_Plane::do_aux_function_change_mode(const Mode::Number number,
     }
 }
 
+void RC_Channel_Plane::do_aux_function_armdisarm(const AuxSwitchPos ch_flag)
+{
+    // arm or disarm the vehicle
+    switch (ch_flag) {
+    case AuxSwitchPos::HIGH:
+        plane.arming.arm(AP_Arming::Method::AUXSWITCH, true);
+        break;
+    case AuxSwitchPos::MIDDLE:
+        // nothing
+        break;
+    case AuxSwitchPos::LOW:
+        plane.arming.disarm(AP_Arming::Method::AUXSWITCH , true);
+        break;
+    }
+}
+
 #if HAL_QUADPLANE_ENABLED
 void RC_Channel_Plane::do_aux_function_q_assist_state(AuxSwitchPos ch_flag)
 {
@@ -403,6 +419,10 @@ bool RC_Channel_Plane::do_aux_function(const aux_func_t ch_option, const AuxSwit
         plane.autotune_enable(ch_flag == AuxSwitchPos::HIGH);
         break;
 
+    case AUX_FUNC::ARMDISARM:
+        do_aux_function_armdisarm(ch_flag);
+        break;
+
     default:
         return RC_Channel::do_aux_function(ch_option, ch_flag);
     }

ArduPlane/RC_Channel.h

@@ -28,6 +28,8 @@ class RC_Channel_Plane : public RC_Channel
 
     void do_aux_function_flare(AuxSwitchPos ch_flag);
 
+    void do_aux_function_armdisarm(const AuxSwitchPos ch_flag);
+
 };
 
 class RC_Channels_Plane : public RC_Channels

ArduPlane/is_flying.cpp

@@ -177,6 +177,10 @@ void Plane::update_is_flying_5Hz(void)
 
     // conservative ground mode value for rate D suppression
     ground_mode = !is_flying() && !hal.util->get_soft_armed();
+
+    if (!is_flying()) {
+        arming.disarm_if_requested();
+    }
 }
 
 /*

ArduPlane/servos.cpp

@@ -73,6 +73,11 @@ void Plane::throttle_slew_limit(SRV_Channel::Aux_servo_function_t func)
 */
 bool Plane::suppress_throttle(void)
 {
+
+    if (arming.get_throttle_cut()) {
+        return true;
+    }
+
 #if PARACHUTE == ENABLED
     if (control_mode->does_auto_throttle() && parachute.release_initiated()) {
         // throttle always suppressed in auto-throttle modes after parachute release initiated

libraries/AP_Notify/AP_Notify.h

@@ -113,6 +113,7 @@ class AP_Notify
         bool powering_off;        // true when the vehicle is powering off
         bool video_recording;     // true when the vehicle is recording video
         bool temp_cal_running;    // true if a temperature calibration is running
+        bool throttle_cut;        // true if the throttle has been cut by an attempt to disarm with aux channel while flying
     };
 
     /// notify_events_type - bitmask of active events.

libraries/AP_OSD/AP_OSD_Screen.cpp

@@ -1493,13 +1493,14 @@ void AP_OSD_Screen::draw_fltmode(uint8_t x, uint8_t y)
 {
     AP_Notify * notify = AP_Notify::get_singleton();
     char arm;
-    if (AP_Notify::flags.armed) {
+    if (AP_Notify::flags.armed && !AP_Notify::flags.throttle_cut) {
         arm = SYMBOL(SYM_ARMED);
     } else {
         arm = SYMBOL(SYM_DISARMED);
     }
     if (notify) {
-        backend->write(x, y, false, "%s%c", notify->get_flight_mode_str(), arm);
+        backend->write(x, y, false, "%s", notify->get_flight_mode_str());
+        backend->write(x+4, y, AP_Notify::flags.throttle_cut, "%c", arm);
     }
 }
 
@@ -1746,7 +1747,7 @@ void AP_OSD_Screen::draw_throttle_value(uint8_t x, uint8_t y, float throttle_v)
     if (signbit(throttle_v)) {
         spaces -= 1;
     }
-    backend->write(x + spaces, y, false, format, throttle_v, SYMBOL(SYM_PCNT));
+    backend->write(x + spaces, y, AP_Notify::flags.throttle_cut, format, throttle_v, SYMBOL(SYM_PCNT));
 }
 
 void AP_OSD_Screen::draw_throttle(uint8_t x, uint8_t y)