Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

HAL_Linux: fixed time function to use integer maths 

avoid floating point rounding errors after long uptimes.

thanks to Richard (RSAXVC) for finding this
This commit is contained in:
Andrew Tridgell 2024-01-31 15:06:44 +11:00
parent 2ca6ea4ffd
commit 45ca3ad4bb
1 changed files with 12 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_HAL_Linux/system.cpp
View File

@ -7,18 +7,26 @@
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <AP_HAL/system.h> #include <AP_HAL/system.h>
#include <AP_HAL_Linux/Scheduler.h> #include <AP_HAL_Linux/Scheduler.h>
#include <AP_Math/div1000.h>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
namespace AP_HAL { namespace AP_HAL {
static struct { static struct {
struct timespec start_time; uint64_t start_time_ns;
} state; } state;
static uint64_t ts_to_nsec(struct timespec &ts)
{
return ts.tv_sec*1000000000ULL + ts.tv_nsec;
}
void init() 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, ...) void WEAK panic(const char *errormsg, ...)
@ -59,24 +67,12 @@ uint64_t micros64()
struct timespec ts; struct timespec ts;
clock_gettime(CLOCK_MONOTONIC, &ts); clock_gettime(CLOCK_MONOTONIC, &ts);
return 1.0e6*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) - return uint64_div1000(ts_to_nsec(ts) - state.start_time_ns);
(state.start_time.tv_sec +
(state.start_time.tv_nsec*1.0e-9)));
} }
uint64_t millis64() uint64_t millis64()
{ {
const Linux::Scheduler* scheduler = Linux::Scheduler::from(hal.scheduler); return uint64_div1000(micros64());
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)));
} }
} // namespace AP_HAL } // namespace AP_HAL