diff --git a/libraries/AP_HAL_Linux/Scheduler.cpp b/libraries/AP_HAL_Linux/Scheduler.cpp index 9644c7f0a5..7e51e64b1d 100644 --- a/libraries/AP_HAL_Linux/Scheduler.cpp +++ b/libraries/AP_HAL_Linux/Scheduler.cpp @@ -100,9 +100,9 @@ void LinuxScheduler::delay(uint16_t ms) stopped_clock_usec += 1000UL*ms; return; } - uint32_t start = millis(); + uint64_t start = millis64(); - while ((millis() - start) < ms) { + while ((millis64() - start) < ms) { // this yields the CPU to other apps _microsleep(1000); if (_min_delay_cb_ms <= ms) { @@ -113,7 +113,7 @@ void LinuxScheduler::delay(uint16_t ms) } } -uint32_t LinuxScheduler::millis() +uint64_t LinuxScheduler::millis64() { if (stopped_clock_usec) { return stopped_clock_usec/1000; @@ -125,7 +125,7 @@ uint32_t LinuxScheduler::millis() (_sketch_start_time.tv_nsec*1.0e-9))); } -uint32_t LinuxScheduler::micros() +uint64_t LinuxScheduler::micros64() { if (stopped_clock_usec) { return stopped_clock_usec; @@ -137,6 +137,16 @@ uint32_t LinuxScheduler::micros() (_sketch_start_time.tv_nsec*1.0e-9))); } +uint32_t LinuxScheduler::millis() +{ + return millis64() & 0xFFFFFFFF; +} + +uint32_t LinuxScheduler::micros() +{ + return micros64() & 0xFFFFFFFF; +} + void LinuxScheduler::delay_microseconds(uint16_t us) { _microsleep(us); @@ -239,12 +249,12 @@ void *LinuxScheduler::_timer_thread(void) this aims to run at an average of 1kHz, so that it can be used to drive 1kHz processes without drift */ - uint32_t next_run_usec = micros() + 1000; + uint64_t next_run_usec = micros64() + 1000; while (true) { - uint32_t dt = next_run_usec - micros(); + uint64_t dt = next_run_usec - micros64(); if (dt > 2000) { // we've lost sync - restart - next_run_usec = micros(); + next_run_usec = micros64(); } else { _microsleep(dt); } diff --git a/libraries/AP_HAL_Linux/Scheduler.h b/libraries/AP_HAL_Linux/Scheduler.h index f7a4e036c6..dc11ceddc3 100644 --- a/libraries/AP_HAL_Linux/Scheduler.h +++ b/libraries/AP_HAL_Linux/Scheduler.h @@ -17,6 +17,8 @@ public: void delay(uint16_t ms); uint32_t millis(); uint32_t micros(); + uint64_t millis64(); + uint64_t micros64(); void delay_microseconds(uint16_t us); void register_delay_callback(AP_HAL::Proc, uint16_t min_time_ms); diff --git a/libraries/AP_HAL_Linux/Semaphores.cpp b/libraries/AP_HAL_Linux/Semaphores.cpp index 5b6a918ab7..09717bd4a4 100644 --- a/libraries/AP_HAL_Linux/Semaphores.cpp +++ b/libraries/AP_HAL_Linux/Semaphores.cpp @@ -21,13 +21,13 @@ bool LinuxSemaphore::take(uint32_t timeout_ms) if (take_nonblocking()) { return true; } - uint32_t start = hal.scheduler->micros(); + uint64_t start = hal.scheduler->micros64(); do { hal.scheduler->delay_microseconds(200); if (take_nonblocking()) { return true; } - } while ((hal.scheduler->micros() - start) < timeout_ms*1000); + } while ((hal.scheduler->micros64() - start) < timeout_ms*1000); return false; }