Parachute: initial draft library

libraries/AP_Parachute/AP_Parachute.cpp

+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include <AP_Parachute.h>
+#include <AP_Relay.h>
+#include <AP_Math.h>
+#include <RC_Channel.h>
+#include <AP_HAL.h>
+
+extern const AP_HAL::HAL& hal;
+
+const AP_Param::GroupInfo AP_Parachute::var_info[] PROGMEM = {
+
+    // @Param: ENABLED
+    // @DisplayName: Parachute release enabled or disabled
+    // @Description: Parachute release enabled or disabled
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Standard
+    AP_GROUPINFO("ENABLED", 0, AP_Parachute, _enabled, 0),
+
+    // @Param: TYPE
+    // @DisplayName: Parachute release mechanism type (relay or servo)
+    // @Description: Parachute release mechanism type (relay or servo)
+    // @Values: 0:Relay 0,1:Relay 1,2:Relay 2,3:Relay 3,10:Servo
+    // @User: Standard
+    AP_GROUPINFO("TYPE", 1, AP_Parachute, _release_type, AP_PARACHUTE_TRIGGER_TYPE_RELAY_0),
+
+    // @Param: SERVO_ON
+    // @DisplayName: Parachute Servo ON PWM value
+    // @Description: Parachute Servo PWM value when parachute is released
+    // @Range: 1000 2000
+    // @Units: pwm
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("SERVO_ON", 2, AP_Parachute, _servo_on_pwm, AP_PARACHUTE_SERVO_ON_PWM_DEFAULT),
+
+    // @Param: SERVO_OFF
+    // @DisplayName: Servo OFF PWM value
+    // @Description: Parachute Servo PWM value when parachute is not released
+    // @Range: 1000 2000
+    // @Units: pwm
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("SERVO_OFF", 3, AP_Parachute, _servo_off_pwm, AP_PARACHUTE_SERVO_OFF_PWM_DEFAULT),
+
+    // @Param: ALT_MIN
+    // @DisplayName: Parachute min altitude in cm above home
+    // @Description: Parachute min altitude above home.  When vehicle is below this alt parachute cannot be released
+    // @Range: 0 10000
+    // @Units: Centimeters
+    // @Increment: 100
+    // @User: Standard
+    AP_GROUPINFO("ALT_MIN", 4, AP_Parachute, _alt_min_cm, AP_PARACHUTE_ALT_MIN_DEFAULT),
+
+    AP_GROUPEND
+};
+
+
+/// release - release parachute
+void AP_Parachute::release()
+{
+    // exit immediately if not enabled
+    if (_enabled <= 0) {
+        return;
+    }
+
+    // trigger the servo or relay
+    if (_release_type == AP_PARACHUTE_TRIGGER_TYPE_SERVO) {
+	    RC_Channel_aux::set_radio(RC_Channel_aux::k_parachute_release, _servo_on_pwm);
+    }else if (_release_type <= AP_PARACHUTE_TRIGGER_TYPE_RELAY_3) {
+        _relay.on(_release_type);
+    }
+
+	// leave a message that it should be active for this many loops (assumes 50hz loops)
+    _release_time = hal.scheduler->millis();
+}
+
+/// update - shuts off the trigger should be called at about 10hz
+void AP_Parachute::update()
+{
+    // exit immediately if not enabled or parachute not released
+    if (_enabled <= 0 || _release_time == 0) {
+        return;
+    }
+
+    // check if release mechanism has been triggered more than 1 second ago
+    if (hal.scheduler->millis() - _release_time > AP_PARACHUTE_RELEASE_DURATION_MS) {
+        // trigger the servo or relay
+        if (_release_type == AP_PARACHUTE_TRIGGER_TYPE_SERVO) {
+            // move servo back to off position
+            RC_Channel_aux::set_radio(RC_Channel_aux::k_parachute_release, _servo_off_pwm);
+        }else if (_release_type <= AP_PARACHUTE_TRIGGER_TYPE_RELAY_3) {
+            // set relay back to zero volts
+            _relay.on(_release_type);
+        }
+
+        // reset release_time
+        _release_time = 0;
+    }
+}

libraries/AP_Parachute/AP_Parachute.h

+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/// @file	AP_Parachute.h
+/// @brief	Parachute release library
+
+#ifndef AP_PARACHUTE_H
+#define AP_PARACHUTE_H
+
+#include <AP_Param.h>
+#include <AP_Common.h>
+#include <AP_Relay.h>
+
+#define AP_PARACHUTE_TRIGGER_TYPE_RELAY_0       0
+#define AP_PARACHUTE_TRIGGER_TYPE_RELAY_1       1
+#define AP_PARACHUTE_TRIGGER_TYPE_RELAY_2       2
+#define AP_PARACHUTE_TRIGGER_TYPE_RELAY_3       3
+#define AP_PARACHUTE_TRIGGER_TYPE_SERVO         10
+
+#define AP_PARACHUTE_RELEASE_DURATION_MS       1000    // when parachute is released, servo or relay stay at their released position/value for 1000ms (1second)
+
+#define AP_PARACHUTE_SERVO_ON_PWM_DEFAULT      1300    // default PWM value to move servo to when shutter is activated
+#define AP_PARACHUTE_SERVO_OFF_PWM_DEFAULT     1100    // default PWM value to move servo to when shutter is deactivated
+
+#define AP_PARACHUTE_ALT_MIN_DEFAULT           1000    // default min altitude the vehicle should have before parachute is released
+
+/// @class	AP_Parachute
+/// @brief	Class managing the release of a parachute
+class AP_Parachute {
+
+public:
+
+    /// Constructor
+    AP_Parachute(AP_Relay& relay) :
+        _relay(relay),
+        _release_time(0)
+    {
+        // setup parameter defaults
+        AP_Param::setup_object_defaults(this, var_info);
+    }
+
+    /// enabled - enable or disable parachute release
+    void enabled(bool on_off) { _enabled = on_off; }
+
+    /// enabled - returns true if parachute release is enabled
+    bool enabled() const { return _enabled; }
+
+    /// release - release parachute
+    void release();
+
+    /// update - shuts off the trigger should be called at about 10hz
+    void update();
+
+    /// alt_min_cm - returns the min altitude above home the vehicle should have before parachute is released
+    int16_t alt_min_cm() const { return _alt_min_cm; }
+
+    static const struct AP_Param::GroupInfo        var_info[];
+
+private:
+    // Parameters
+    AP_Int8     _enabled;       // 1 if parachute release is enabled
+    AP_Int8     _release_type;  // 0:Servo,1:Relay
+    AP_Int16    _servo_on_pwm;  // PWM value to move servo to when shutter is activated
+    AP_Int16    _servo_off_pwm; // PWM value to move servo to when shutter is deactivated
+    AP_Int16    _alt_min_cm;    // min altitude the vehicle should have before parachute is released
+
+    // internal variables
+    AP_Relay&   _relay;         // pointer to relay object from the base class Relay. The subclasses could be AP_Relay_APM1 or AP_Relay_APM2
+    uint32_t    _release_time;  // count of number of cycles servo or relay has been at on position
+};
+
+#endif /* AP_PARACHUTE_H */