mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-22 07:44:03 -04:00
AP_Logging: update EKF while waiting for log open
this prevents the EKF getting unhappy when we arm
This commit is contained in:
parent
c2c013964d
commit
ece3cf5241
@ -719,18 +719,22 @@ void AP_Logger_File::PrepForArming_start_logging()
|
||||
}
|
||||
|
||||
uint32_t start_ms = AP_HAL::millis();
|
||||
const uint32_t open_limit_ms = 250;
|
||||
const uint32_t open_limit_ms = 1000;
|
||||
|
||||
/*
|
||||
log open happens in the io_timer thread. We allow for a maximum
|
||||
of 250ms to complete the open
|
||||
of 1s to complete the open
|
||||
*/
|
||||
start_new_log_pending = true;
|
||||
EXPECT_DELAY_MS(250);
|
||||
EXPECT_DELAY_MS(1000);
|
||||
while (AP_HAL::millis() - start_ms < open_limit_ms) {
|
||||
if (logging_started()) {
|
||||
break;
|
||||
}
|
||||
#if !APM_BUILD_TYPE(APM_BUILD_Replay) && !defined(HAL_BUILD_AP_PERIPH)
|
||||
// keep the EKF ticking over
|
||||
AP::ahrs().update();
|
||||
#endif
|
||||
hal.scheduler->delay(1);
|
||||
}
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user