diff --git a/libraries/AP_HAL_PX4/Scheduler.cpp b/libraries/AP_HAL_PX4/Scheduler.cpp index b01039e23b..45ea3f7523 100644 --- a/libraries/AP_HAL_PX4/Scheduler.cpp +++ b/libraries/AP_HAL_PX4/Scheduler.cpp @@ -38,8 +38,6 @@ PX4Scheduler::PX4Scheduler() : void PX4Scheduler::init(void *unused) { - _sketch_start_time = hrt_absolute_time(); - _main_task_pid = getpid(); // setup the timer thread - this will call tasks at 1kHz @@ -78,7 +76,7 @@ void PX4Scheduler::init(void *unused) uint64_t PX4Scheduler::micros64() { - return hrt_absolute_time() - _sketch_start_time; + return hrt_absolute_time(); } uint64_t PX4Scheduler::millis64() @@ -116,7 +114,7 @@ void PX4Scheduler::delay_microseconds(uint16_t usec) perf_end(_perf_delay); return; } - uint64_t start = micros(); + uint64_t start = micros64(); uint64_t dt; while ((dt=(micros64() - start)) < usec) { up_udelay(usec - dt); @@ -131,9 +129,9 @@ void PX4Scheduler::delay(uint16_t ms) return; } perf_begin(_perf_delay); - uint64_t start = hrt_absolute_time(); + uint64_t start = micros64(); - while ((hrt_absolute_time() - start)/1000 < ms && + while ((micros64() - start)/1000 < ms && !_px4_thread_should_exit) { delay_microseconds_semaphore(1000); if (_min_delay_cb_ms <= ms) { diff --git a/libraries/AP_HAL_PX4/Scheduler.h b/libraries/AP_HAL_PX4/Scheduler.h index 891eeb3456..0e40573846 100644 --- a/libraries/AP_HAL_PX4/Scheduler.h +++ b/libraries/AP_HAL_PX4/Scheduler.h @@ -53,7 +53,6 @@ private: uint16_t _min_delay_cb_ms; AP_HAL::Proc _failsafe; volatile bool _timer_pending; - uint64_t _sketch_start_time; volatile bool _timer_suspended; diff --git a/libraries/AP_HAL_PX4/UARTDriver.cpp b/libraries/AP_HAL_PX4/UARTDriver.cpp index fd0219381c..6985992ded 100644 --- a/libraries/AP_HAL_PX4/UARTDriver.cpp +++ b/libraries/AP_HAL_PX4/UARTDriver.cpp @@ -391,7 +391,7 @@ int PX4UARTDriver::_write_fd(const uint8_t *buf, uint16_t n) _last_write_time != 0 && _total_written != 0 && _os_write_buffer_size == _total_written && - hrt_elapsed_time(&_last_write_time) > 500*1000UL) { + (hal.scheduler->micros64() - _last_write_time) > 500*1000UL) { // it doesn't look like hw flow control is working ::printf("disabling flow control on %s _total_written=%u\n", _devpath, (unsigned)_total_written); @@ -407,17 +407,17 @@ int PX4UARTDriver::_write_fd(const uint8_t *buf, uint16_t n) if (ret > 0) { BUF_ADVANCEHEAD(_writebuf, ret); - _last_write_time = hrt_absolute_time(); + _last_write_time = hal.scheduler->micros64(); _total_written += ret; return ret; } - if (hrt_absolute_time() - _last_write_time > 2000 && + if (hal.scheduler->micros64() - _last_write_time > 2000 && _flow_control == FLOW_CONTROL_DISABLE) { #if 0 // this trick is disabled for now, as it sometimes blocks on // re-opening the ttyACM0 port, which would cause a crash - if (hrt_absolute_time() - _last_write_time > 2000000) { + if (hal.scheduler->micros64() - _last_write_time > 2000000) { // we haven't done a successful write for 2 seconds - try // reopening the port _initialised = false; @@ -430,11 +430,11 @@ int PX4UARTDriver::_write_fd(const uint8_t *buf, uint16_t n) return n; } - _last_write_time = hrt_absolute_time(); + _last_write_time = hal.scheduler->micros64(); _initialised = true; } #else - _last_write_time = hrt_absolute_time(); + _last_write_time = hal.scheduler->micros64(); #endif // we haven't done a successful write for 2ms, which means the // port is running at less than 500 bytes/sec. Start