diff --git a/libraries/AP_HAL_Linux/Scheduler.cpp b/libraries/AP_HAL_Linux/Scheduler.cpp index fb954ff3ee..e2f96e04b9 100644 --- a/libraries/AP_HAL_Linux/Scheduler.cpp +++ b/libraries/AP_HAL_Linux/Scheduler.cpp @@ -9,7 +9,6 @@ #include "Util.h" #include "SPIUARTDriver.h" #include "RPIOUARTDriver.h" -#include #include #include #include @@ -69,7 +68,7 @@ void Scheduler::_create_realtime_thread(pthread_t *ctx, int rtprio, if (r != 0) { hal.console->printf("Error creating thread '%s': %s\n", name, strerror(r)); - panic("Failed to create thread"); + AP_HAL::panic("Failed to create thread"); } pthread_attr_destroy(&attr); @@ -82,8 +81,6 @@ void Scheduler::init(void* machtnichts) { mlockall(MCL_CURRENT|MCL_FUTURE); - clock_gettime(CLOCK_MONOTONIC, &_sketch_start_time); - struct sched_param param = { .sched_priority = APM_LINUX_MAIN_PRIORITY }; sched_setscheduler(0, SCHED_FIFO, ¶m); @@ -140,12 +137,12 @@ void Scheduler::_microsleep(uint32_t usec) void Scheduler::delay(uint16_t ms) { - if (stopped_clock_usec) { + if (_stopped_clock_usec) { return; } - uint64_t start = millis64(); + uint64_t start = AP_HAL::millis64(); - while ((millis64() - start) < ms) { + while ((AP_HAL::millis64() - start) < ms) { // this yields the CPU to other apps _microsleep(1000); if (_min_delay_cb_ms <= ms) { @@ -158,41 +155,27 @@ void Scheduler::delay(uint16_t ms) uint64_t Scheduler::millis64() { - if (stopped_clock_usec) { - return stopped_clock_usec/1000; - } - struct timespec ts; - clock_gettime(CLOCK_MONOTONIC, &ts); - return 1.0e3*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) - - (_sketch_start_time.tv_sec + - (_sketch_start_time.tv_nsec*1.0e-9))); + return AP_HAL::millis64(); } uint64_t Scheduler::micros64() { - if (stopped_clock_usec) { - return stopped_clock_usec; - } - struct timespec ts; - clock_gettime(CLOCK_MONOTONIC, &ts); - return 1.0e6*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) - - (_sketch_start_time.tv_sec + - (_sketch_start_time.tv_nsec*1.0e-9))); + return AP_HAL::micros64(); } uint32_t Scheduler::millis() { - return millis64() & 0xFFFFFFFF; + return AP_HAL::millis(); } uint32_t Scheduler::micros() { - return micros64() & 0xFFFFFFFF; + return AP_HAL::micros(); } void Scheduler::delay_microseconds(uint16_t us) { - if (stopped_clock_usec) { + if (_stopped_clock_usec) { return; } _microsleep(us); @@ -300,12 +283,12 @@ void *Scheduler::_timer_thread(void* arg) this aims to run at an average of 1kHz, so that it can be used to drive 1kHz processes without drift */ - uint64_t next_run_usec = sched->micros64() + 1000; + uint64_t next_run_usec = AP_HAL::micros64() + 1000; while (true) { - uint64_t dt = next_run_usec - sched->micros64(); + uint64_t dt = next_run_usec - AP_HAL::micros64(); if (dt > 2000) { // we've lost sync - restart - next_run_usec = sched->micros64(); + next_run_usec = AP_HAL::micros64(); } else { sched->_microsleep(dt); } @@ -439,7 +422,7 @@ bool Scheduler::system_initializing() { void Scheduler::system_initialized() { if (_initialized) { - panic("PANIC: scheduler::system_initialized called more than once"); + AP_HAL::panic("PANIC: scheduler::system_initialized called more than once"); } _initialized = true; } @@ -451,8 +434,8 @@ void Scheduler::reboot(bool hold_in_bootloader) void Scheduler::stop_clock(uint64_t time_usec) { - if (time_usec >= stopped_clock_usec) { - stopped_clock_usec = time_usec; + if (time_usec >= _stopped_clock_usec) { + _stopped_clock_usec = time_usec; _run_io(); } } diff --git a/libraries/AP_HAL_Linux/Scheduler.h b/libraries/AP_HAL_Linux/Scheduler.h index 5c3dffd530..16f9f791c0 100644 --- a/libraries/AP_HAL_Linux/Scheduler.h +++ b/libraries/AP_HAL_Linux/Scheduler.h @@ -18,6 +18,11 @@ typedef void *(*pthread_startroutine_t)(void *); public: Scheduler(); + + static Scheduler *from(AP_HAL::Scheduler *scheduler) { + return static_cast(scheduler); + } + void init(void* machtnichts); void delay(uint16_t ms); uint32_t millis(); @@ -48,8 +53,9 @@ public: void stop_clock(uint64_t time_usec); + uint64_t stopped_clock_usec() const { return _stopped_clock_usec; } + private: - struct timespec _sketch_start_time; void _timer_handler(int signum); void _microsleep(uint32_t usec); @@ -88,7 +94,7 @@ private: void _create_realtime_thread(pthread_t *ctx, int rtprio, const char *name, pthread_startroutine_t start_routine); - uint64_t stopped_clock_usec; + uint64_t _stopped_clock_usec; Semaphore _timer_semaphore; Semaphore _io_semaphore; diff --git a/libraries/AP_HAL_Linux/system.cpp b/libraries/AP_HAL_Linux/system.cpp index 65467f3997..ff0c119248 100644 --- a/libraries/AP_HAL_Linux/system.cpp +++ b/libraries/AP_HAL_Linux/system.cpp @@ -1,9 +1,77 @@ +#include +#include +#include +#include + +#include #include +#include + +extern const AP_HAL::HAL& hal; namespace AP_HAL { +static struct { + struct timespec start_time; +} state; + void init() { + clock_gettime(CLOCK_MONOTONIC, &state.start_time); +} + +void panic(const char *errormsg, ...) +{ + va_list ap; + + va_start(ap, errormsg); + vdprintf(1, errormsg, ap); + va_end(ap); + write(1, "\n", 1); + + hal.rcin->deinit(); + hal.scheduler->delay_microseconds(10000); + exit(1); +} + +uint32_t micros() +{ + return micros64() & 0xFFFFFFFF; +} + +uint32_t millis() +{ + return millis64() & 0xFFFFFFFF; +} + +uint64_t micros64() +{ + const Linux::Scheduler* scheduler = Linux::Scheduler::from(hal.scheduler); + uint64_t stopped_usec = scheduler->stopped_clock_usec(); + if (stopped_usec) { + return stopped_usec; + } + + 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))); +} + +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))); } } // namespace AP_HAL