AP_Parachute: move sink rate check to new method

also remove unused critical_sink accessor

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;
 

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 {