Browse Source

AP_Parachute: Added time check for sink rate to avoid glitches

Signed-off-by: Vinicius Knabben <viniciusknabben@hotmail.com>
master
Vinicius Knabben 6 years ago committed by Andrew Tridgell
parent
commit
040fa3e10a
  1. 13
      libraries/AP_Parachute/AP_Parachute.cpp
  2. 1
      libraries/AP_Parachute/AP_Parachute.h

13
libraries/AP_Parachute/AP_Parachute.cpp

@ -115,10 +115,19 @@ void AP_Parachute::update() @@ -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;

1
libraries/AP_Parachute/AP_Parachute.h

@ -103,6 +103,7 @@ private: @@ -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 {

Loading…
Cancel
Save