From 9d807802cc7c2c555b26caccfa5833fbfde4a25b Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 1 Apr 2014 20:57:03 +0900 Subject: [PATCH] Parachute: initial draft library --- libraries/AP_Parachute/AP_Parachute.cpp | 99 +++++++++++++++++++++++++ libraries/AP_Parachute/AP_Parachute.h | 71 ++++++++++++++++++ 2 files changed, 170 insertions(+) create mode 100644 libraries/AP_Parachute/AP_Parachute.cpp create mode 100644 libraries/AP_Parachute/AP_Parachute.h diff --git a/libraries/AP_Parachute/AP_Parachute.cpp b/libraries/AP_Parachute/AP_Parachute.cpp new file mode 100644 index 0000000000..51a25ab5ff --- /dev/null +++ b/libraries/AP_Parachute/AP_Parachute.cpp @@ -0,0 +1,99 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include +#include +#include +#include +#include + +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; + } +} diff --git a/libraries/AP_Parachute/AP_Parachute.h b/libraries/AP_Parachute/AP_Parachute.h new file mode 100644 index 0000000000..aa1732310c --- /dev/null +++ b/libraries/AP_Parachute/AP_Parachute.h @@ -0,0 +1,71 @@ +// -*- 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 +#include +#include + +#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 */