From f7726b7d442cc98e8b890aab38e1095c44f42b4e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 31 Jan 2024 15:06:44 +1100 Subject: [PATCH 1/3] HAL_Linux: fixed time function to use integer maths avoid floating point rounding errors after long uptimes. thanks to Richard (RSAXVC) for finding this --- libraries/AP_HAL_Linux/system.cpp | 28 ++++++++++++---------------- 1 file changed, 12 insertions(+), 16 deletions(-) diff --git a/libraries/AP_HAL_Linux/system.cpp b/libraries/AP_HAL_Linux/system.cpp index 0e0bb8e4f73ab..2bc1641ecf66e 100644 --- a/libraries/AP_HAL_Linux/system.cpp +++ b/libraries/AP_HAL_Linux/system.cpp @@ -7,18 +7,26 @@ #include #include #include +#include extern const AP_HAL::HAL& hal; namespace AP_HAL { static struct { - struct timespec start_time; + uint64_t start_time_ns; } state; +static uint64_t ts_to_nsec(struct timespec &ts) +{ + return ts.tv_sec*1000000000ULL + ts.tv_nsec; +} + void init() { - clock_gettime(CLOCK_MONOTONIC, &state.start_time); + struct timespec ts {}; + clock_gettime(CLOCK_MONOTONIC, &ts); + state.start_time_ns = ts_to_nsec(ts); } void WEAK panic(const char *errormsg, ...) @@ -59,24 +67,12 @@ uint64_t micros64() struct timespec ts; clock_gettime(CLOCK_MONOTONIC, &ts); - return 1.0e6*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) - - (state.start_time.tv_sec + - (state.start_time.tv_nsec*1.0e-9))); + return uint64_div1000(ts_to_nsec(ts) - state.start_time_ns); } uint64_t millis64() { - const Linux::Scheduler* scheduler = Linux::Scheduler::from(hal.scheduler); - uint64_t stopped_usec = scheduler->stopped_clock_usec(); - if (stopped_usec) { - return stopped_usec / 1000; - } - - struct timespec ts; - clock_gettime(CLOCK_MONOTONIC, &ts); - return 1.0e3*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) - - (state.start_time.tv_sec + - (state.start_time.tv_nsec*1.0e-9))); + return uint64_div1000(micros64()); } } // namespace AP_HAL From 7bcbe517d1bb296d53b304cd36cfdc803c53738b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 5 Feb 2024 14:30:09 +1100 Subject: [PATCH 2/3] HAL_SITL: use integer maths for clocks same fix as for HAL_Linux --- libraries/AP_HAL_SITL/system.cpp | 20 ++++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/libraries/AP_HAL_SITL/system.cpp b/libraries/AP_HAL_SITL/system.cpp index 1e4a59be2019e..81c5b4941b0c0 100644 --- a/libraries/AP_HAL_SITL/system.cpp +++ b/libraries/AP_HAL_SITL/system.cpp @@ -18,12 +18,19 @@ using HALSITL::Scheduler; namespace AP_HAL { static struct { - struct timeval start_time; + uint64_t start_time_ns; } state; +static uint64_t ts_to_nsec(struct timespec &ts) +{ + return ts.tv_sec*1000000000ULL + ts.tv_nsec; +} + void init() { - gettimeofday(&state.start_time, nullptr); + struct timespec ts {}; + clock_gettime(CLOCK_MONOTONIC, &ts); + state.start_time_ns = ts_to_nsec(ts); } #if defined(__CYGWIN__) || defined(__CYGWIN64__) || defined(CYGWIN_BUILD) @@ -170,12 +177,9 @@ uint64_t micros64() return stopped_usec; } - struct timeval tp; - gettimeofday(&tp, nullptr); - uint64_t ret = 1.0e6 * ((tp.tv_sec + (tp.tv_usec * 1.0e-6)) - - (state.start_time.tv_sec + - (state.start_time.tv_usec * 1.0e-6))); - return ret; + struct timespec ts; + clock_gettime(CLOCK_MONOTONIC, &ts); + return uint64_div1000(ts_to_nsec(ts) - state.start_time_ns); } uint64_t millis64() From 805b24c3182747c5ed535a65ba296941bc43446d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 5 Feb 2024 14:38:40 +1100 Subject: [PATCH 3/3] HAL_Linux: fixed float calc of time for onboard flow --- libraries/AP_HAL_Linux/OpticalFlow_Onboard.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_HAL_Linux/OpticalFlow_Onboard.cpp b/libraries/AP_HAL_Linux/OpticalFlow_Onboard.cpp index 981afc77bbaa8..9f597ace53413 100644 --- a/libraries/AP_HAL_Linux/OpticalFlow_Onboard.cpp +++ b/libraries/AP_HAL_Linux/OpticalFlow_Onboard.cpp @@ -220,7 +220,7 @@ void OpticalFlow_Onboard::push_gyro(float gyro_x, float gyro_y, float dt) _integrated_gyro.x += (gyro_x - _gyro_bias.x) * dt; _integrated_gyro.y += (gyro_y - _gyro_bias.y) * dt; sample.gyro = _integrated_gyro; - sample.time_us = 1.0e6 * (ts.tv_sec + (ts.tv_nsec*1.0e-9)); + sample.time_us = ts.tv_sec*1000000ULL + ts.tv_nsec/1000ULL; _gyro_ring_buffer->push(sample); }