AP_Parachute: Added time check for sink rate to avoid glitches

Signed-off-by: Vinicius Knabben <viniciusknabben@hotmail.com>
ArduPilot · Apr 29, 2019 · 245345d · 245345d
1 parent 583e82b
commit 245345d
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 2 deletions.
diff --git 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
@@ -103,6 +103,7 @@ class AP_Parachute {
     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 {