Add 'safe' check-if-vehicle-is-flying arm/disarm options

ArduCopter/RC_Channel.cpp

@@ -161,6 +161,16 @@ void RC_Channel_Copter::do_aux_function_armdisarm(const AuxSwitchPos ch_flag)
     }
 }
 
+bool RC_Channel_Copter::permit_CHECKED_DISARM() const
+{
+    if (!copter.ap.land_complete) {
+        // do not permit disarm if we are considered flying
+        gcs().send_text(MAV_SEVERITY_WARNING, "Switch disarm denied; not landed");
+        return false;
+    }
+    return true;
+}
+
 // do_aux_function - implement the function invoked by auxiliary switches
 void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos ch_flag)
 {

ArduCopter/RC_Channel.h

@@ -17,6 +17,8 @@ class RC_Channel_Copter : public RC_Channel
 private:
 
     void do_aux_function_armdisarm(const AuxSwitchPos ch_flag) override;
+    bool permit_CHECKED_DISARM() const override;
+
     void do_aux_function_change_mode(const Mode::Number mode,
                                      const AuxSwitchPos ch_flag);
     void do_aux_function_change_air_mode(const AuxSwitchPos ch_flag);

ArduPlane/RC_Channel.cpp

@@ -135,6 +135,16 @@ void RC_Channel_Plane::do_aux_function_flare(AuxSwitchPos ch_flag)
         }    
 }
 
+bool RC_Channel_Plane::permit_CHECKED_DISARM() const
+{
+    if (plane.is_flying()) {
+        // do not permit disarm if we are considered flying
+        gcs().send_text(MAV_SEVERITY_WARNING, "Switch disarm denied; flying");
+        return false;
+    }
+    return true;
+}
+
 void RC_Channel_Plane::init_aux_function(const RC_Channel::aux_func_t ch_option,
                                          const RC_Channel::AuxSwitchPos ch_flag)
 {

ArduPlane/RC_Channel.h

@@ -26,6 +26,9 @@ class RC_Channel_Plane : public RC_Channel
 
     void do_aux_function_flare(AuxSwitchPos ch_flag);
 
+    // returns false if we shouldn't allow the user to disarm the
+    // vehicle using an RC channel option
+    bool permit_CHECKED_DISARM() const override;
 };
 
 class RC_Channels_Plane : public RC_Channels

Tools/autotest/ArduPlane_Tests/CheckedDisarm/flaps.txt

@@ -0,0 +1,13 @@
+QGC WPL 110
+0	0	0	16	0.000000	0.000000	0.000000	0.000000	-35.363262	149.165237	584.390015	1
+1	0	3	22	15.000000	0.000000	0.000000	0.000000	-35.361279	149.164230	30.000000	1
+2	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.361229	149.163025	80.000000	1
+3	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.364563	149.163773	80.000000	1
+4	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.364384	149.164795	80.000000	1
+5	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.361027	149.164093	80.000000	1
+6	0	0	177	2.000000	3.000000	0.000000	0.000000	0.000000	0.000000	0.000000	1
+7	0	3	189	0.000000	0.000000	0.000000	0.000000	-35.362915	149.162613	60.000000	1
+8	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.363136	149.162750	60.000000	1
+9	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.365467	149.164215	55.000000	1
+10	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.365009	149.165482	39.889999	1
+11	0	3	21	0.000000	0.000000	0.000000	1.000000	-35.363041	149.165222	0.000000	1

Tools/autotest/arducopter.py

@@ -300,8 +300,53 @@ def setGCSfailsafe(self, paramValue=0):
             self.set_parameter("SIM_SPEEDUP",4)
             self.set_parameter("FS_GCS_ENABLE", paramValue)
 
-
+    def test_checked_disarm(self):
+        self.start_subtest("Checking CHECKED_ARMDISARM")
+        self.set_parameter("RC9_OPTION", 106)
+        self.reboot_sitl()
+        self.progress("Arm/disarm without takeoff")
+        self.wait_ready_to_arm()
+        self.set_rc(9, 2000)
+        self.wait_armed()
+        self.set_rc(9, 1000)
+        self.wait_disarmed()
 
+        self.progress("Ensure no mid-air disarming")
+        self.set_rc(9, 2000)
+        self.wait_armed()
+        self.takeoff(10, mode="ALT_HOLD")
+        good = False
+        try:
+            self.set_rc(9, 1000)
+            self.wait_disarmed(timeout=1)
+        except AutoTestTimeoutException:
+            good = True
+        if not good:
+            raise NotAchievedException("Disarmed when we shouldn't have")
+        self.do_RTL()
+        self.zero_throttle()
+
+        self.start_subtest("Checking CHECKED_DISARM")
+        self.set_parameter("RC9_OPTION", 107)
+        self.reboot_sitl()
+        self.progress("Disarm without takeoff")
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+        self.set_rc(9, 2000)
+        self.wait_disarmed()
+        self.set_rc(9, 1000)
+
+        self.progress("Ensure no mid-air disarming")
+        self.takeoff(10, mode="ALT_HOLD")
+        good = False
+        try:
+            self.set_rc(9, 2000)
+            self.wait_disarmed(timeout=1)
+        except AutoTestTimeoutException:
+            good = True
+        if not good:
+            raise NotAchievedException("Disarmed when we shouldn't have")
+        self.do_RTL()
 
     #################################################
     #   TESTS FLY
@@ -6034,6 +6079,10 @@ def tests2b(self): #this block currently around 9.5mins here
              "Test Callisto",
              self.test_callisto),
 
+            ("CheckedDisarm",
+             "Test RC Channel options for checked disarm",
+             self.test_checked_disarm),
+
             ("Replay",
              "Test Replay",
              self.test_replay),

Tools/autotest/arduplane.py

@@ -1877,6 +1877,52 @@ def fly_terrain_mission(self):
 
         self.fly_mission("ap-terrain.txt", mission_timeout=600)
 
+    def test_checked_disarm(self):
+        self.start_subtest("Checking CHECKED_ARMDISARM")
+        self.set_parameter("RC9_OPTION", 106)
+        self.reboot_sitl()
+        self.progress("Arm/disarm without takeoff")
+        self.wait_ready_to_arm()
+        self.set_rc(9, 2000)
+        self.wait_armed()
+        self.set_rc(9, 1000)
+        self.wait_disarmed()
+
+        self.progress("Ensure no mid-air disarming")
+        self.takeoff()
+        good = False
+        try:
+            self.set_rc(9, 1000)
+            self.wait_disarmed(timeout=1)
+        except AutoTestTimeoutException:
+            good = True
+        if not good:
+            raise NotAchievedException("Disarmed when we shouldn't have")
+        self.fly_home_land_and_disarm()
+        self.zero_throttle()
+
+        self.start_subtest("Checking CHECKED_DISARM")
+        self.set_parameter("RC9_OPTION", 107)
+        self.reboot_sitl()
+        self.progress("Disarm without takeoff")
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+        self.set_rc(9, 2000)
+        self.wait_disarmed()
+        self.set_rc(9, 1000)
+
+        self.progress("Ensure no mid-air disarming")
+        self.takeoff()
+        good = False
+        try:
+            self.set_rc(9, 2000)
+            self.wait_disarmed(timeout=1)
+        except AutoTestTimeoutException:
+            good = True
+        if not good:
+            raise NotAchievedException("Disarmed when we shouldn't have")
+        self.fly_home_land_and_disarm()
+
     def ekf_lane_switch(self):
 
         self.context_push()
@@ -2175,6 +2221,10 @@ def tests(self):
              "Test AirSpeed drivers",
              self.test_airspeed_drivers),
 
+            ("CheckedDisarm",
+             "Test RC Channel options for checked disarm",
+             self.test_checked_disarm),
+
             ("LogUpload",
              "Log upload",
              self.log_upload),

libraries/RC_Channel/RC_Channel.cpp

@@ -93,9 +93,9 @@ const AP_Param::GroupInfo RC_Channel::var_info[] = {
     // @Param: OPTION
     // @DisplayName: RC input option
     // @Description: Function assigned to this RC channel
-    // @Values{Copter}: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 37:Throw, 38:ADSB Avoidance En, 39:PrecLoiter, 40:Proximity Avoidance, 41:ArmDisarm, 42:SmartRTL, 43:InvertedFlight, 46:RC Override Enable, 47:User Function 1, 48:User Function 2, 49:User Function 3, 52:Acro, 55:Guided, 56:Loiter, 57:Follow, 58:Clear Waypoints, 60:ZigZag, 61:ZigZag SaveWP, 62:Compass Learn, 65:GPS Disable, 66:Relay5, 67:Relay6, 68:Stabilize, 69:PosHold, 70:AltHold, 71:FlowHold, 72:Circle, 73:Drift, 75:SurfaceTrackingUpDown, 76:Standby Mode, 78:RunCam Control, 79:RunCam OSD Control, 80:Viso Align, 81:Disarm, 83:ZigZag Auto, 84:Air Mode, 85:Generator, 90:EKF Pos Source, 100:KillIMU1, 101:KillIMU2, 102:Camera Mode Toggle, 105:GPS Disable Yaw, 300:Scripting1, 301:Scripting2, 302:Scripting3, 303:Scripting4, 304:Scripting5, 305:Scripting6, 306:Scripting7, 307:Scripting8
+    // @Values{Copter}: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 37:Throw, 38:ADSB Avoidance En, 39:PrecLoiter, 40:Proximity Avoidance, 41:ArmDisarm, 42:SmartRTL, 43:InvertedFlight, 46:RC Override Enable, 47:User Function 1, 48:User Function 2, 49:User Function 3, 52:Acro, 55:Guided, 56:Loiter, 57:Follow, 58:Clear Waypoints, 60:ZigZag, 61:ZigZag SaveWP, 62:Compass Learn, 65:GPS Disable, 66:Relay5, 67:Relay6, 68:Stabilize, 69:PosHold, 70:AltHold, 71:FlowHold, 72:Circle, 73:Drift, 75:SurfaceTrackingUpDown, 76:Standby Mode, 78:RunCam Control, 79:RunCam OSD Control, 80:Viso Align, 81:Disarm, 83:ZigZag Auto, 84:Air Mode, 85:Generator, 90:EKF Pos Source, 100:KillIMU1, 101:KillIMU2, 102:Camera Mode Toggle, 105:GPS Disable Yaw, 106:Checked Arm/Disarm, 107:Checked Disarm, 300:Scripting1, 301:Scripting2, 302:Scripting3, 303:Scripting4, 304:Scripting5, 305:Scripting6, 306:Scripting7, 307:Scripting8
     // @Values{Rover}: 0:Do Nothing, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 11:Fence, 16:Auto, 19:Gripper, 24:Auto Mission Reset, 27:Retract Mount, 28:Relay On/Off, 30:Lost Rover Sound, 31:Motor Emergency Stop, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 40:Proximity Avoidance, 41:ArmDisarm, 42:SmartRTL, 46:RC Override Enable, 50:LearnCruise, 51:Manual, 52:Acro, 53:Steering, 54:Hold, 55:Guided, 56:Loiter, 57:Follow, 58:Clear Waypoints, 59:Simple Mode, 62:Compass Learn, 63:Sailboat Tack, 65:GPS Disable, 66:Relay5, 67:Relay6, 74:Sailboat motoring 3pos, 78:RunCam Control, 79:RunCam OSD Control, 80:Viso Align, 81:Disarm, 90:EKF Pos Source, 100:KillIMU1, 101:KillIMU2, 102:Camera Mode Toggle, 105:GPS Disable Yaw, 201:Roll, 202:Pitch, 203:Walking Height, 207:MainSail, 208:Flap, 300:Scripting1, 301:Scripting2, 302:Scripting3, 303:Scripting4, 304:Scripting5, 305:Scripting6, 306:Scripting7, 307:Scripting8
-    // @Values{Plane}: 0:Do Nothing, 4:ModeRTL, 9:Camera Trigger, 16:ModeAuto, 24:Auto Mission Reset, 27:Retract Mount, 28:Relay On/Off, 29:Landing Gear, 30:Lost Plane Sound, 31:Motor Emergency Stop, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 38:ADSB Avoidance En, 41:ArmDisarm, 43:InvertedFlight, 46:RC Override Enable, 51:ModeManual, 55:ModeGuided, 56:ModeLoiter, 58:Clear Waypoints, 62:Compass Learn, 64:Reverse Throttle, 65:GPS Disable, 66:Relay5, 67:Relay6, 72:ModeCircle, 77:ModeTakeoff, 78:RunCam Control, 79:RunCam OSD Control, 81:Disarm, 82:QAssist 3pos, 84:Air Mode, 85:Generator, 86: Non Auto Terrain Follow Disable, 87:Crow Select, 88:Soaring Enable, 89:Landing Flare, 90:EKF Pos Source, 91:Airspeed Ratio Calibration, 92:FBWA, 100:KillIMU1, 101:KillIMU2, 102:Camera Mode Toggle, 105:GPS Disable Yaw, 208:Flap, 209: Forward Throttle, 300:Scripting1, 301:Scripting2, 302:Scripting3, 303:Scripting4, 304:Scripting5, 305:Scripting6, 306:Scripting7, 307:Scripting8
+    // @Values{Plane}: 0:Do Nothing, 4:ModeRTL, 9:Camera Trigger, 16:ModeAuto, 24:Auto Mission Reset, 27:Retract Mount, 28:Relay On/Off, 29:Landing Gear, 30:Lost Plane Sound, 31:Motor Emergency Stop, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 38:ADSB Avoidance En, 41:ArmDisarm, 43:InvertedFlight, 46:RC Override Enable, 51:ModeManual, 55:ModeGuided, 56:ModeLoiter, 58:Clear Waypoints, 62:Compass Learn, 64:Reverse Throttle, 65:GPS Disable, 66:Relay5, 67:Relay6, 72:ModeCircle, 77:ModeTakeoff, 78:RunCam Control, 79:RunCam OSD Control, 81:Disarm, 82:QAssist 3pos, 84:Air Mode, 85:Generator, 86: Non Auto Terrain Follow Disable, 87:Crow Select, 88:Soaring Enable, 89:Landing Flare, 90:EKF Pos Source, 91:Airspeed Ratio Calibration, 92:FBWA, 100:KillIMU1, 101:KillIMU2, 102:Camera Mode Toggle, 105:GPS Disable Yaw, 106:Checked Arm/Disarm, 107:Checked Disarm, 208:Flap, 209: Forward Throttle, 300:Scripting1, 301:Scripting2, 302:Scripting3, 303:Scripting4, 304:Scripting5, 305:Scripting6, 306:Scripting7, 307:Scripting8
     // @User: Standard
     AP_GROUPINFO_FRAME("OPTION",  6, RC_Channel, option, 0, AP_PARAM_FRAME_COPTER|AP_PARAM_FRAME_ROVER|AP_PARAM_FRAME_PLANE),
 
@@ -457,10 +457,12 @@ void RC_Channel::init_aux_function(const aux_func_t ch_option, const AuxSwitchPo
     switch(ch_option) {
     // the following functions do not need to be initialised:
     case AUX_FUNC::ARMDISARM:
+    case AUX_FUNC::CHECKED_ARMDISARM:
     case AUX_FUNC::CAMERA_TRIGGER:
     case AUX_FUNC::CLEAR_WP:
     case AUX_FUNC::COMPASS_LEARN:
     case AUX_FUNC::DISARM:
+    case AUX_FUNC::CHECKED_DISARM:
     case AUX_FUNC::DO_NOTHING:
     case AUX_FUNC::LANDING_GEAR:
     case AUX_FUNC::LOST_VEHICLE_SOUND:
@@ -631,6 +633,13 @@ void RC_Channel::do_aux_function_armdisarm(const AuxSwitchPos ch_flag)
     }
 }
 
+void RC_Channel::do_aux_function_disarm(const AuxSwitchPos ch_flag)
+{
+    if (ch_flag == AuxSwitchPos::HIGH) {
+        AP::arming().disarm(AP_Arming::Method::AUXSWITCH);
+    }
+}
+
 void RC_Channel::do_aux_function_avoid_adsb(const AuxSwitchPos ch_flag)
 {
 #if HAL_ADSB_ENABLED
@@ -865,6 +874,25 @@ void RC_Channel::do_aux_function_mission_reset(const AuxSwitchPos ch_flag)
     mission->reset();
 }
 
+void RC_Channel::do_aux_function_checked_armdisarm(const AuxSwitchPos ch_flag)
+{
+    if (ch_flag == AuxSwitchPos::LOW &&
+        !permit_CHECKED_DISARM()) {
+        return;
+    }
+    do_aux_function_armdisarm(ch_flag);
+}
+
+void RC_Channel::do_aux_function_checked_disarm(const AuxSwitchPos ch_flag)
+{
+    // do not permit disarm unless we are considered landed
+    if (!permit_CHECKED_DISARM()) {
+        return;
+    }
+    do_aux_function_disarm(ch_flag);
+}
+
+
 void RC_Channel::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos ch_flag)
 {
     switch(ch_option) {
@@ -945,10 +973,16 @@ void RC_Channel::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos
         do_aux_function_armdisarm(ch_flag);
         break;
 
+    case AUX_FUNC::CHECKED_ARMDISARM:
+        do_aux_function_checked_armdisarm(ch_flag);
+        break;
+
     case AUX_FUNC::DISARM:
-        if (ch_flag == AuxSwitchPos::HIGH) {
-            AP::arming().disarm(AP_Arming::Method::AUXSWITCH);
-        }
+        do_aux_function_disarm(ch_flag);
+        break;
+
+    case AUX_FUNC::CHECKED_DISARM:
+        do_aux_function_checked_disarm(ch_flag);
         break;
 
     case AUX_FUNC::COMPASS_LEARN:

libraries/RC_Channel/RC_Channel.h

@@ -199,6 +199,8 @@ class RC_Channel {
         EKF_LANE_SWITCH =    103, // trigger lane switch attempt
         EKF_YAW_RESET =      104, // trigger yaw reset attempt
         GPS_DISABLE_YAW =    105, // disable GPS yaw for testing
+        CHECKED_ARMDISARM =  106, // as ARMDISARM but disarm checks are done
+        CHECKED_DISARM =     107, // as DISARM but disarm checks are done
         // if you add something here, make sure to update the documentation of the parameter in RC_Channel.cpp!
         // also, if you add an option >255, you will need to fix duplicate_options_exist
 
@@ -249,6 +251,8 @@ class RC_Channel {
     virtual void init_aux_function(aux_func_t ch_option, AuxSwitchPos);
 
     virtual void do_aux_function_armdisarm(const AuxSwitchPos ch_flag);
+    void do_aux_function_disarm(const AuxSwitchPos ch_flag);
+    virtual bool permit_CHECKED_DISARM() const { return true; }
     void do_aux_function_avoid_adsb(const AuxSwitchPos ch_flag);
     void do_aux_function_avoid_proximity(const AuxSwitchPos ch_flag);
     void do_aux_function_camera_trigger(const AuxSwitchPos ch_flag);
@@ -309,6 +313,9 @@ class RC_Channel {
     void read_mode_switch();
     bool debounce_completed(int8_t position);
 
+    void do_aux_function_checked_armdisarm(const AuxSwitchPos ch_flag);
+    void do_aux_function_checked_disarm(const AuxSwitchPos ch_flag);
+
 #if !HAL_MINIMIZE_FEATURES
     // Structure to lookup switch change announcements
     struct LookupTable{