diff --git a/libraries/AP_Parachute/AP_Parachute.cpp b/libraries/AP_Parachute/AP_Parachute.cpp index 37e5d36dda..50f6339c7e 100644 --- a/libraries/AP_Parachute/AP_Parachute.cpp +++ b/libraries/AP_Parachute/AP_Parachute.cpp @@ -118,19 +118,7 @@ void AP_Parachute::update() if (_enabled <= 0) { return; } - // check if the plane is sinking too fast for more than a second and release parachute - uint32_t time = AP_HAL::millis(); - if((_critical_sink > 0) && (_sink_rate > _critical_sink) && !_release_initiated && _is_flying) { - if(_sink_time == 0) { - _sink_time = AP_HAL::millis(); - } - if((time - _sink_time) >= 1000) { - release(); - } - } else { - _sink_time = 0; - } - + // calc time since release uint32_t time_diff = AP_HAL::millis() - _release_time; uint32_t delay_ms = _delay_ms<=0 ? 0: (uint32_t)_delay_ms; @@ -164,6 +152,41 @@ void AP_Parachute::update() } } +// set_sink_rate - set vehicle sink rate +void AP_Parachute::set_sink_rate(float sink_rate) +{ + // reset sink time if critical sink rate check is disabled or vehicle is not flying + if ((_critical_sink <= 0) || !_is_flying) { + _sink_time_ms = 0; + return; + } + + // reset sink_time if vehicle is not sinking too fast + if (sink_rate <= _critical_sink) { + _sink_time_ms = 0; + return; + } + + // start time when sinking too fast + if (_sink_time_ms == 0) { + _sink_time_ms = AP_HAL::millis(); + } +} + +// trigger parachute release if sink_rate is below critical_sink_rate for 1sec +void AP_Parachute::check_sink_rate() +{ + // return immediately if parachute is being released or vehicle is not flying + if (_release_initiated || !_is_flying) { + return; + } + + // if vehicle is sinking too fast for more than a second release parachute + if ((_sink_time_ms > 0) && ((AP_HAL::millis() - _sink_time_ms) > 1000)) { + release(); + } +} + // singleton instance AP_Parachute *AP_Parachute::_singleton; diff --git a/libraries/AP_Parachute/AP_Parachute.h b/libraries/AP_Parachute/AP_Parachute.h index 9a401914cc..0669343442 100644 --- a/libraries/AP_Parachute/AP_Parachute.h +++ b/libraries/AP_Parachute/AP_Parachute.h @@ -72,9 +72,6 @@ public: /// update - shuts off the trigger should be called at about 10hz void update(); - - /// critical_sink - returns the configured maximum sink rate to trigger emergency release - float critical_sink() const { return _critical_sink; } /// alt_min - returns the min altitude above home the vehicle should have before parachute is released /// 0 = altitude check disabled @@ -84,7 +81,10 @@ public: void set_is_flying(const bool is_flying) { _is_flying = is_flying; } // set_sink_rate - set vehicle sink rate - void set_sink_rate(float sink_rate) { _sink_rate = sink_rate; } + void set_sink_rate(float sink_rate); + + // trigger parachute release if sink_rate is below critical_sink_rate for 1sec + void check_sink_rate(); static const struct AP_Param::GroupInfo var_info[]; @@ -109,8 +109,7 @@ private: bool _release_in_progress:1; // true if the parachute release is in progress bool _released:1; // true if the parachute has been released bool _is_flying:1; // true if the vehicle is flying - float _sink_rate; // vehicle sink rate in m/s - uint32_t _sink_time; // time that the vehicle exceeded critical sink rate + uint32_t _sink_time_ms; // system time that the vehicle exceeded critical sink rate }; namespace AP {