From 040fa3e10a91ed6824a30cf706ab73be9cd7395e Mon Sep 17 00:00:00 2001 From: Vinicius Knabben Date: Fri, 22 Feb 2019 11:03:39 -0300 Subject: [PATCH] AP_Parachute: Added time check for sink rate to avoid glitches Signed-off-by: Vinicius Knabben --- libraries/AP_Parachute/AP_Parachute.cpp | 13 +++++++++++-- libraries/AP_Parachute/AP_Parachute.h | 1 + 2 files changed, 12 insertions(+), 2 deletions(-) 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 {