diff --git a/libraries/AP_HAL/Scheduler.h b/libraries/AP_HAL/Scheduler.h index d3233cce6e..f951d9a010 100644 --- a/libraries/AP_HAL/Scheduler.h +++ b/libraries/AP_HAL/Scheduler.h @@ -15,11 +15,8 @@ public: virtual void delay(uint16_t ms) = 0; virtual uint32_t millis() = 0; virtual uint32_t micros() = 0; -#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150 - // offer non-wrapping 64 bit versions on faster CPUs virtual uint64_t millis64() = 0; virtual uint64_t micros64() = 0; -#endif /* delay for the given number of microseconds. This needs to be as diff --git a/libraries/AP_HAL_AVR/Scheduler.cpp b/libraries/AP_HAL_AVR/Scheduler.cpp index 32811f02ca..91520d8d14 100644 --- a/libraries/AP_HAL_AVR/Scheduler.cpp +++ b/libraries/AP_HAL_AVR/Scheduler.cpp @@ -66,6 +66,24 @@ uint32_t AVRScheduler::millis() { return _timer.millis(); } +/* + 64 bit version of millis(). This wraps at 32 bits on AVR + */ +uint64_t AVRScheduler::millis64() { + return millis(); +} + +/* + 64 bit version of micros(). This wraps when 32 bit millis() wraps + */ +uint64_t AVRScheduler::micros64() { + // this is slow, but solves the problem with logging uint64_t timestamps + uint64_t ret = millis(); + ret *= 1000ULL; + ret += micros() % 1000UL; + return ret; +} + void AVRScheduler::delay_microseconds(uint16_t us) { _timer.delay_microseconds(us); } diff --git a/libraries/AP_HAL_AVR/Scheduler.h b/libraries/AP_HAL_AVR/Scheduler.h index 467669ff86..239df7be78 100644 --- a/libraries/AP_HAL_AVR/Scheduler.h +++ b/libraries/AP_HAL_AVR/Scheduler.h @@ -28,6 +28,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_FLYMAPLE/Scheduler.cpp b/libraries/AP_HAL_FLYMAPLE/Scheduler.cpp index 7de92f0079..abd4fbdf21 100644 --- a/libraries/AP_HAL_FLYMAPLE/Scheduler.cpp +++ b/libraries/AP_HAL_FLYMAPLE/Scheduler.cpp @@ -103,6 +103,18 @@ uint32_t FLYMAPLEScheduler::micros() { return libmaple_micros(); } +uint64_t FLYMAPLEScheduler::millis64() { + return millis(); +} + +uint64_t FLYMAPLEScheduler::micros64() { + // this is slow, but solves the problem with logging uint64_t timestamps + uint64_t ret = millis(); + ret *= 1000ULL; + ret += micros() % 1000; + return ret; +} + void FLYMAPLEScheduler::delay_microseconds(uint16_t us) { delay_us(us); diff --git a/libraries/AP_HAL_FLYMAPLE/Scheduler.h b/libraries/AP_HAL_FLYMAPLE/Scheduler.h index b7f7b3914f..d9d7b0597c 100644 --- a/libraries/AP_HAL_FLYMAPLE/Scheduler.h +++ b/libraries/AP_HAL_FLYMAPLE/Scheduler.h @@ -30,6 +30,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);