Move handling of parachute mission commands into AP_Mission_Commands

ArduCopter/crash_check.cpp

@@ -209,10 +209,6 @@ void Copter::parachute_check()
 // parachute_release - trigger the release of the parachute, disarm the motors and notify the user
 void Copter::parachute_release()
 {
-    // send message to gcs and dataflash
-    gcs().send_text(MAV_SEVERITY_INFO,"Parachute: Released");
-    Log_Write_Event(DATA_PARACHUTE_RELEASED);
-
     // disarm motors
     init_disarm_motors();
 

ArduCopter/mode_auto.cpp

@@ -485,12 +485,6 @@ bool Copter::ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
 #endif //AC_FENCE == ENABLED
         break;
 
-#if PARACHUTE == ENABLED
-    case MAV_CMD_DO_PARACHUTE:                          // Mission command to configure or release parachute
-        do_parachute(cmd);
-        break;
-#endif
-
 #if NAV_GUIDED == ENABLED
     case MAV_CMD_DO_GUIDED_LIMITS:                      // 220  accept guided mode limits
         do_guided_limits(cmd);
@@ -679,7 +673,6 @@ bool Copter::ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd)
     case MAV_CMD_DO_SET_HOME:
     case MAV_CMD_DO_SET_ROI:
     case MAV_CMD_DO_MOUNT_CONTROL:
-    case MAV_CMD_DO_PARACHUTE:  // assume parachute was released successfully
     case MAV_CMD_DO_GUIDED_LIMITS:
     case MAV_CMD_DO_FENCE_ENABLE:
     case MAV_CMD_DO_WINCH:
@@ -1393,29 +1386,6 @@ void Copter::ModeAuto::do_mount_control(const AP_Mission::Mission_Command& cmd)
 #endif
 }
 
-#if PARACHUTE == ENABLED
-// do_parachute - configure or release parachute
-void Copter::ModeAuto::do_parachute(const AP_Mission::Mission_Command& cmd)
-{
-    switch (cmd.p1) {
-        case PARACHUTE_DISABLE:
-            copter.parachute.enabled(false);
-            Log_Write_Event(DATA_PARACHUTE_DISABLED);
-            break;
-        case PARACHUTE_ENABLE:
-            copter.parachute.enabled(true);
-            Log_Write_Event(DATA_PARACHUTE_ENABLED);
-            break;
-        case PARACHUTE_RELEASE:
-            copter.parachute_release();
-            break;
-        default:
-            // do nothing
-            break;
-    }
-}
-#endif
-
 #if WINCH_ENABLED == ENABLED
 // control winch based on mission command
 void Copter::ModeAuto::do_winch(const AP_Mission::Mission_Command& cmd)

ArduCopter/wscript

@@ -19,7 +19,6 @@ def build(bld):
             'AP_IRLock',
             'AP_InertialNav',
             'AP_Motors',
-            'AP_Parachute',
             'AP_RCMapper',
             'AP_Avoidance',
             'AP_AdvancedFailsafe',

ArduPlane/commands_logic.cpp

@@ -159,12 +159,6 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
         autotune_enable(cmd.p1);
         break;
 
-#if PARACHUTE == ENABLED
-    case MAV_CMD_DO_PARACHUTE:
-        do_parachute(cmd);
-        break;
-#endif
-
 #if MOUNT == ENABLED
     // Sets the region of interest (ROI) for a sensor set or the
     // vehicle itself. This can then be used by the vehicles control
@@ -286,12 +280,6 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd)        // Ret
     case MAV_CMD_CONDITION_DISTANCE:
         return verify_within_distance();
 
-#if PARACHUTE == ENABLED
-    case MAV_CMD_DO_PARACHUTE:
-        // assume parachute was released successfully
-        return true;
-#endif
-
     // do commands (always return true)
     case MAV_CMD_DO_CHANGE_SPEED:
     case MAV_CMD_DO_SET_HOME:
@@ -920,27 +908,6 @@ void Plane::do_set_home(const AP_Mission::Mission_Command& cmd)
     }
 }
 
-#if PARACHUTE == ENABLED
-// do_parachute - configure or release parachute
-void Plane::do_parachute(const AP_Mission::Mission_Command& cmd)
-{
-    switch (cmd.p1) {
-        case PARACHUTE_DISABLE:
-            parachute.enabled(false);
-            break;
-        case PARACHUTE_ENABLE:
-            parachute.enabled(true);
-            break;
-        case PARACHUTE_RELEASE:
-            parachute_release();
-            break;
-        default:
-            // do nothing
-            break;
-    }
-}
-#endif
-
 // start_command_callback - callback function called from ap-mission when it begins a new mission command
 //      we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode
 bool Plane::start_command_callback(const AP_Mission::Mission_Command &cmd)

ArduPlane/wscript

@@ -16,7 +16,6 @@ def build(bld):
             'AP_Camera',
             'AP_L1_Control',
             'AP_Navigation',
-            'AP_Parachute',
             'AP_RCMapper',
             'AP_SpdHgtControl',
             'AP_TECS',

Tools/ardupilotwaf/ardupilotwaf.py

@@ -65,6 +65,7 @@
     'AP_Volz_Protocol',
     'AP_SBusOut',
     'AP_IOMCU',
+    'AP_Parachute',
     'AP_RAMTRON',
     'AP_RCProtocol',
     'AP_Radio',

Tools/autotest/arducopter.py

@@ -1640,6 +1640,29 @@ def test_parachute(self):
         self.set_parameter("SIM_PARA_ENABLE", 1)
         self.set_parameter("SIM_PARA_PIN", 9)
 
+        self.progress("Test triggering parachute in mission")
+        self.load_mission("copter_parachute_mission.txt")
+        self.change_mode('LOITER')
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+        self.change_mode('AUTO')
+        self.set_rc(3, 1600)
+        self.mavproxy.expect('BANG')
+        self.reboot_sitl()
+
+        self.progress("Test triggering with mavlink message")
+        self.takeoff(20)
+        self.run_cmd(mavutil.mavlink.MAV_CMD_DO_PARACHUTE,
+                     2, # release
+                     0,
+                     0,
+                     0,
+                     0,
+                     0,
+                     0)
+        self.mavproxy.expect('BANG')
+        self.reboot_sitl()
+
         self.progress("Testing three-position switch")
         self.set_parameter("RC9_OPTION", 23) # parachute 3pos
 

Tools/autotest/arduplane.py

@@ -700,6 +700,22 @@ def test_gripper_mission(self):
         if ex is not None:
             raise ex
 
+    def test_parachute(self):
+        self.set_rc(9, 1000)
+        self.set_parameter("CHUTE_ENABLED", 1)
+        self.set_parameter("CHUTE_TYPE", 10)
+        self.set_parameter("SERVO9_FUNCTION", 27)
+        self.set_parameter("SIM_PARA_ENABLE", 1)
+        self.set_parameter("SIM_PARA_PIN", 9)
+
+        self.load_mission("plane-parachute-mission.txt")
+        self.mavproxy.send("wp set 1\n")
+        self.change_mode('AUTO')
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+        self.mavproxy.expect("BANG")
+        self.reboot_sitl()
+
     def run_subtest(self, desc, func):
         self.start_subtest(desc)
         func()
@@ -772,6 +788,8 @@ def tests(self):
              "Test Gripper mission items",
              self.test_gripper_mission),
 
+            ("Parachute", "Test Parachute", self.test_parachute),
+
             ("LogDownLoad",
              "Log download",
              lambda: self.log_download(

Tools/autotest/copter_parachute_mission.txt

@@ -0,0 +1,4 @@
+QGC WPL 110
+0	1	0	16	0.000000	0.000000	0.000000	0.000000	-35.362881	149.165222	582.000000	1
+1	0	3	22	0.000000	0.000000	0.000000	0.000000	  0.000000        0.000000	20.000000	1
+2	0	0      208	2.000000	0.000000	0.000000	0.000000	  0.000000	  0.000000	0.000000	1

Tools/autotest/plane-parachute-mission.txt

@@ -0,0 +1,5 @@
+QGC WPL 110
+0	0	0	16	0.000000	0.000000	0.000000	0.000000	-35.362045	149.165878	584.280029	1
+1	0	3	22	0.000000	0.000000	0.000000	0.000000	-35.362125	149.165039	10.000000	1
+2	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.364845	149.163452	11.000000	1
+3	0	0      208	2.000000	0.000000	0.000000	0.000000	  0.000000	  0.000000	0.000000	1

libraries/AP_Mission/AP_Mission.cpp

@@ -269,6 +269,7 @@ bool AP_Mission::verify_command(const Mission_Command& cmd)
     case MAV_CMD_DO_DIGICAM_CONFIGURE:
     case MAV_CMD_DO_DIGICAM_CONTROL:
     case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
+    case MAV_CMD_DO_PARACHUTE:
         return true;
     default:
         return _cmd_verify_fn(cmd);
@@ -290,6 +291,8 @@ bool AP_Mission::start_command(const Mission_Command& cmd)
     case MAV_CMD_DO_DIGICAM_CONTROL:
     case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
         return start_command_camera(cmd);
+    case MAV_CMD_DO_PARACHUTE:
+        return start_command_parachute(cmd);
     default:
         return _cmd_start_fn(cmd);
     }

libraries/AP_Mission/AP_Mission.h

@@ -583,6 +583,7 @@ class AP_Mission {
     bool start_command_do_gripper(const AP_Mission::Mission_Command& cmd);
     bool start_command_do_servorelayevents(const AP_Mission::Mission_Command& cmd);
     bool start_command_camera(const AP_Mission::Mission_Command& cmd);
+    bool start_command_parachute(const AP_Mission::Mission_Command& cmd);
 };
 
 namespace AP {

libraries/AP_Mission/AP_Mission_Commands.cpp

@@ -2,6 +2,7 @@
 
 #include <GCS_MAVLink/GCS.h>
 #include <AP_Gripper/AP_Gripper.h>
+#include <AP_Parachute/AP_Parachute.h>
 #include <AP_ServoRelayEvents/AP_ServoRelayEvents.h>
 
 bool AP_Mission::start_command_do_gripper(const AP_Mission::Mission_Command& cmd)
@@ -110,3 +111,27 @@ bool AP_Mission::start_command_camera(const AP_Mission::Mission_Command& cmd)
     }
 }
 
+bool AP_Mission::start_command_parachute(const AP_Mission::Mission_Command& cmd)
+{
+    AP_Parachute *parachute = AP::parachute();
+    if (parachute == nullptr) {
+        return false;
+    }
+
+    switch (cmd.p1) {
+    case PARACHUTE_DISABLE:
+        parachute->enabled(false);
+        break;
+    case PARACHUTE_ENABLE:
+        parachute->enabled(true);
+        break;
+    case PARACHUTE_RELEASE:
+        parachute->release();
+        break;
+    default:
+        // do nothing
+        return false;
+    }
+
+    return true;
+}

libraries/AP_Parachute/AP_Parachute.cpp

@@ -5,6 +5,8 @@
 #include <SRV_Channel/SRV_Channel.h>
 #include <AP_Notify/AP_Notify.h>
 #include <AP_HAL/AP_HAL.h>
+#include <AP_Logger/AP_Logger.h>
+#include <GCS_MAVLink/GCS.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -70,6 +72,8 @@ void AP_Parachute::enabled(bool on_off)
 
     // clear release_time
     _release_time = 0;
+
+    AP::logger().Write_Event(_enabled ? DATA_PARACHUTE_ENABLED : DATA_PARACHUTE_DISABLED);
 }
 
 /// release - release parachute
@@ -80,6 +84,9 @@ void AP_Parachute::release()
         return;
     }
 
+    gcs().send_text(MAV_SEVERITY_INFO,"Parachute: Released");
+    AP::logger().Write_Event(DATA_PARACHUTE_RELEASED);
+
     // set release time to current system time
     if (_release_time == 0) {
         _release_time = AP_HAL::millis();
@@ -131,3 +138,15 @@ void AP_Parachute::update()
         AP_Notify::flags.parachute_release = 0;
     }
 }
+
+// singleton instance
+AP_Parachute *AP_Parachute::_singleton;
+
+namespace AP {
+
+AP_Parachute *parachute()
+{
+    return AP_Parachute::get_singleton();
+}
+
+}

libraries/AP_Parachute/AP_Parachute.h

@@ -30,6 +30,12 @@ class AP_Parachute {
         : _relay(relay)
     {
         // setup parameter defaults
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+        if (_singleton != nullptr) {
+            AP_HAL::panic("Rally must be singleton");
+        }
+#endif
+        _singleton = this;
         AP_Param::setup_object_defaults(this, var_info);
     }
 
@@ -64,7 +70,11 @@ class AP_Parachute {
 
     static const struct AP_Param::GroupInfo        var_info[];
 
+    // get singleton instance
+    static AP_Parachute *get_singleton() { return _singleton; }
+
 private:
+    static AP_Parachute *_singleton;
     // Parameters
     AP_Int8     _enabled;       // 1 if parachute release is enabled
     AP_Int8     _release_type;  // 0:Servo,1:Relay
@@ -80,3 +90,7 @@ class AP_Parachute {
     bool        _release_in_progress:1;  // true if the parachute release is in progress
     bool        _released:1;             // true if the parachute has been released
 };
+
+namespace AP {
+    AP_Parachute *parachute();
+};

libraries/SITL/SIM_Plane.cpp

@@ -369,7 +369,8 @@ void Plane::update(const struct sitl_input &input)
     calculate_forces(input, rot_accel, accel_body);
 
     update_dynamics(rot_accel);
-
+    update_external_payload(input);
+
     // update lat/lon/altitude
     update_position();
     time_advance();