Plane: initial support for AP_Parachute library

ArduPlane/ArduPlane.cpp

@@ -77,6 +77,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] PROGMEM = {
     SCHED_TASK(compass_save,         3000,   2500),
     SCHED_TASK(update_logging1,         5,   1700),
     SCHED_TASK(update_logging2,         5,   1700),
+    SCHED_TASK(parachute_check,         5,    500),
 #if FRSKY_TELEM_ENABLED == ENABLED
     SCHED_TASK(frsky_telemetry_send,   10,    100),
 #endif

ArduPlane/GCS_Mavlink.cpp

@@ -1473,6 +1473,28 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             result = plane.compass.handle_mag_cal_command(packet);
             break;
 
+#if PARACHUTE == ENABLED
+        case MAV_CMD_DO_PARACHUTE:
+            // configure or release parachute
+            result = MAV_RESULT_ACCEPTED;
+            switch ((uint16_t)packet.param1) {
+                case PARACHUTE_DISABLE:
+                    plane.parachute.enabled(false);
+                    break;
+                case PARACHUTE_ENABLE:
+                    plane.parachute.enabled(true);
+                    break;
+                case PARACHUTE_RELEASE:
+                    // treat as a manual release which performs some additional check of altitude
+                    plane.parachute_manual_release();
+                    break;
+                default:
+                    result = MAV_RESULT_FAILED;
+                    break;
+            }
+            break;
+#endif
+
         default:
             break;
         }

ArduPlane/Parameters.cpp

@@ -991,6 +991,12 @@ const AP_Param::Info Plane::var_info[] PROGMEM = {
     // @Path: ../libraries/AP_Relay/AP_Relay.cpp
     GOBJECT(relay,                  "RELAY_", AP_Relay),
 
+#if PARACHUTE == ENABLED
+	// @Group: CHUTE_
+    // @Path: ../libraries/AP_Parachute/AP_Parachute.cpp
+    GOBJECT(parachute,		"CHUTE_", AP_Parachute),
+#endif
+
 #if RANGEFINDER_ENABLED == ENABLED
     // @Group: RNGFND
     // @Path: ../libraries/AP_RangeFinder/RangeFinder.cpp

ArduPlane/Parameters.h

@@ -143,6 +143,7 @@ public:
         k_param_land_abort_throttle_enable,
         k_param_rssi = 97,
         k_param_rpm_sensor,
+        k_param_parachute,
         
         // 100: Arming parameters
         k_param_arming = 100,

ArduPlane/Plane.h

@@ -94,6 +94,7 @@
 
 #include <AP_OpticalFlow/AP_OpticalFlow.h>     // Optical Flow library
 #include <AP_RSSI/AP_RSSI.h>                   // RSSI Library
+#include <AP_Parachute/AP_Parachute.h>
 
 // Configuration
 #include "config.h"
@@ -550,6 +551,10 @@ private:
             FUNCTOR_BIND_MEMBER(&Plane::exit_mission_callback, void)};
 
 
+#if PARACHUTE == ENABLED
+    AP_Parachute parachute {relay};
+#endif
+
     // terrain handling
 #if AP_TERRAIN_AVAILABLE
     AP_Terrain terrain {ahrs, mission, rally};
@@ -971,6 +976,11 @@ private:
     void dataflash_periodic(void);
     uint16_t throttle_min(void) const;
     
+    void do_parachute(const AP_Mission::Mission_Command& cmd);
+    void parachute_check();
+    void parachute_release();
+    void parachute_manual_release();
+
 public:
     void mavlink_delay_cb();
     void failsafe_check(void);

ArduPlane/commands_logic.cpp

@@ -173,6 +173,12 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
         break;
 #endif
 
+#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
@@ -256,6 +262,13 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd)        // Ret
     case MAV_CMD_CONDITION_CHANGE_ALT:
         return verify_change_alt();
 
+#if PARACHUTE == ENABLED
+    case MAV_CMD_DO_PARACHUTE:
+        // assume parachute was released successfully
+        return true;
+        break;
+#endif
+
     // do commands (always return true)
     case MAV_CMD_DO_CHANGE_SPEED:
     case MAV_CMD_DO_SET_HOME:
@@ -903,6 +916,27 @@ void Plane::do_take_picture()
 #endif
 }
 
+#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
+
 // log_picture - log picture taken and send feedback to GCS
 void Plane::log_picture()
 {

ArduPlane/config.h

@@ -461,6 +461,16 @@
 #define HIL_SUPPORT ENABLED
 #endif
 
+//////////////////////////////////////////////////////////////////////////////
+// Parachute release
+#ifndef PARACHUTE
+#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
+ # define PARACHUTE ENABLED
+#else
+ # define PARACHUTE DISABLED
+#endif
+#endif
+
 /*
   build a firmware version string.
   GIT_VERSION comes from Makefile builds

ArduPlane/make.inc

@@ -48,3 +48,4 @@ LIBRARIES += AP_Rally
 LIBRARIES += AP_OpticalFlow
 LIBRARIES += AP_RSSI
 LIBRARIES += AP_RPM
+LIBRARIES += AP_Parachute

ArduPlane/parachute.cpp

@@ -0,0 +1,51 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include "Plane.h"
+
+
+/* 
+   call parachute library update
+*/
+void Plane::parachute_check()
+{
+    parachute.update();
+}
+
+/*
+  parachute_release - trigger the release of the parachute
+*/
+void Plane::parachute_release()
+{
+    if (parachute.released()) {
+        return;
+    }
+    
+    // send message to gcs and dataflash
+    gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Parachute: Released"));
+
+    // release parachute
+    parachute.release();
+}
+
+/*
+  parachute_manual_release - trigger the release of the parachute,
+  after performing some checks for pilot error checks if the vehicle
+  is landed
+*/
+void Plane::parachute_manual_release()
+{
+    // exit immediately if parachute is not enabled
+    if (!parachute.enabled() || parachute.released()) {
+        return;
+    }
+
+    // do not release if vehicle is not flying
+    if (!is_flying()) {
+        // warn user of reason for failure
+        gcs_send_text_P(MAV_SEVERITY_WARNING,PSTR("Parachute: not flying"));
+        return;
+    }
+
+    // if we get this far release parachute
+    parachute_release();
+}