diff --git a/libraries/AP_Parachute/AP_Parachute.cpp b/libraries/AP_Parachute/AP_Parachute.cpp index 9a8cc1a9e9..b5502742df 100644 --- a/libraries/AP_Parachute/AP_Parachute.cpp +++ b/libraries/AP_Parachute/AP_Parachute.cpp @@ -55,6 +55,16 @@ const AP_Param::GroupInfo AP_Parachute::var_info[] PROGMEM = { AP_GROUPEND }; +/// enabled - enable or disable parachute release +void AP_Parachute::enabled(bool on_off) +{ + _enabled = on_off; + + // if disabled clear release_time + if (_enabled <= 0) { + _release_time = 0; + } +} /// release - release parachute void AP_Parachute::release() @@ -64,14 +74,7 @@ void AP_Parachute::release() 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) + // set release time to current system time _release_time = hal.scheduler->millis(); // update AP_Notify @@ -81,14 +84,27 @@ void AP_Parachute::release() /// update - shuts off the trigger should be called at about 10hz void AP_Parachute::update() { - // exit immediately if not enabled or parachute not released + // exit immediately if not enabled or parachute not to be 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 + // calc time since release + uint32_t time_diff = hal.scheduler->millis() - _release_time; + + // check if we should release parachute + if (!_released) { + if (time_diff >= AP_PARACHUTE_RELEASE_DELAY_MS) { + if (_release_type == AP_PARACHUTE_TRIGGER_TYPE_SERVO) { + // move 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) { + // set relay + _relay.on(_release_type); + } + _released = true; + } + }else if (time_diff >= AP_PARACHUTE_RELEASE_DELAY_MS + AP_PARACHUTE_RELEASE_DURATION_MS) { 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); @@ -96,10 +112,9 @@ void AP_Parachute::update() // set relay back to zero volts _relay.off(_release_type); } - - // reset release_time + // reset released flag and release_time + _released = false; _release_time = 0; - // update AP_Notify AP_Notify::flags.parachute_release = 0; } diff --git a/libraries/AP_Parachute/AP_Parachute.h b/libraries/AP_Parachute/AP_Parachute.h index 333322d06a..40d2fd29fa 100644 --- a/libraries/AP_Parachute/AP_Parachute.h +++ b/libraries/AP_Parachute/AP_Parachute.h @@ -16,6 +16,7 @@ #define AP_PARACHUTE_TRIGGER_TYPE_RELAY_3 3 #define AP_PARACHUTE_TRIGGER_TYPE_SERVO 10 +#define AP_PARACHUTE_RELEASE_DELAY_MS 500 // delay in milliseconds between call to release() and when servo or relay actually moves. Allows for warning to user #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 @@ -39,7 +40,7 @@ public: } /// enabled - enable or disable parachute release - void enabled(bool on_off) { _enabled = on_off; } + void enabled(bool on_off); /// enabled - returns true if parachute release is enabled bool enabled() const { return _enabled; } @@ -66,7 +67,8 @@ private: // 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 + uint32_t _release_time; // system time that parachute is ordered to be released (actual release will happen 0.5 seconds later) + bool _released; // true if the parachute has been released }; #endif /* AP_PARACHUTE_H */