|
|
|
@ -91,7 +91,7 @@ void AP_Parachute::update()
@@ -91,7 +91,7 @@ void AP_Parachute::update()
|
|
|
|
|
uint32_t time_diff = hal.scheduler->millis() - _release_time; |
|
|
|
|
|
|
|
|
|
// check if we should release parachute
|
|
|
|
|
if ((_release_time != 0) && !_released) { |
|
|
|
|
if ((_release_time != 0) && !_release_in_progress) { |
|
|
|
|
if (time_diff >= AP_PARACHUTE_RELEASE_DELAY_MS) { |
|
|
|
|
if (_release_type == AP_PARACHUTE_TRIGGER_TYPE_SERVO) { |
|
|
|
|
// move servo
|
|
|
|
@ -100,6 +100,7 @@ void AP_Parachute::update()
@@ -100,6 +100,7 @@ void AP_Parachute::update()
|
|
|
|
|
// set relay
|
|
|
|
|
_relay.on(_release_type); |
|
|
|
|
} |
|
|
|
|
_release_in_progress = true; |
|
|
|
|
_released = true; |
|
|
|
|
} |
|
|
|
|
}else if ((_release_time == 0) || time_diff >= AP_PARACHUTE_RELEASE_DELAY_MS + AP_PARACHUTE_RELEASE_DURATION_MS) { |
|
|
|
@ -111,7 +112,7 @@ void AP_Parachute::update()
@@ -111,7 +112,7 @@ void AP_Parachute::update()
|
|
|
|
|
_relay.off(_release_type); |
|
|
|
|
} |
|
|
|
|
// reset released flag and release_time
|
|
|
|
|
_released = false; |
|
|
|
|
_release_in_progress = false; |
|
|
|
|
_release_time = 0; |
|
|
|
|
// update AP_Notify
|
|
|
|
|
AP_Notify::flags.parachute_release = 0; |
|
|
|
|