diff --git a/libraries/AP_Parachute/AP_Parachute.cpp b/libraries/AP_Parachute/AP_Parachute.cpp index b9d250a98a..684ee77e5d 100644 --- a/libraries/AP_Parachute/AP_Parachute.cpp +++ b/libraries/AP_Parachute/AP_Parachute.cpp @@ -115,10 +115,19 @@ void AP_Parachute::update() if (_enabled <= 0) { return; } - // check if the plane is sinking too fast and release parachute + // 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) { - release(); + 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; diff --git a/libraries/AP_Parachute/AP_Parachute.h b/libraries/AP_Parachute/AP_Parachute.h index fde7171f43..4280fb6df8 100644 --- a/libraries/AP_Parachute/AP_Parachute.h +++ b/libraries/AP_Parachute/AP_Parachute.h @@ -103,6 +103,7 @@ private: 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 }; namespace AP {