HAL_PX4: removed restriction on delay in threads

threads other than the main thread should be able to sleep, but not
call the delay callback
This commit is contained in:
Andrew Tridgell 2018-07-07 11:51:19 +10:00
parent 492978c0fa
commit c317cc3fa4
1 changed files with 1 additions and 5 deletions

View File

@ -172,17 +172,13 @@ void PX4Scheduler::delay_microseconds_boost(uint16_t usec)
void PX4Scheduler::delay(uint16_t ms)
{
if (!in_main_thread()) {
::printf("ERROR: delay() from timer process\n");
return;
}
perf_begin(_perf_delay);
uint64_t start = AP_HAL::micros64();
while ((AP_HAL::micros64() - start)/1000 < ms &&
!_px4_thread_should_exit) {
delay_microseconds_semaphore(1000);
if (_min_delay_cb_ms <= ms) {
if (in_main_thread() && _min_delay_cb_ms <= ms) {
call_delay_cb();
}
}