From 712b95bf8ef744435b7fdd605923b0b586503c35 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 8 Jan 2015 13:41:38 +1100
Subject: [PATCH] AP_HAL: added micros64() method
---
libraries/AP_HAL/Scheduler.h | 6 +++++
libraries/AP_HAL_AVR_SITL/Scheduler.cpp | 36 ++++++++++++++++---------
libraries/AP_HAL_AVR_SITL/Scheduler.h | 4 ++-
libraries/AP_HAL_AVR_SITL/sitl_ins.cpp | 2 +-
libraries/AP_HAL_Empty/Scheduler.cpp | 12 +++++++--
libraries/AP_HAL_Empty/Scheduler.h | 2 ++
libraries/AP_HAL_PX4/Scheduler.cpp | 22 ++++++++++-----
libraries/AP_HAL_PX4/Scheduler.h | 2 ++
8 files changed, 64 insertions(+), 22 deletions(-)
diff --git a/libraries/AP_HAL/Scheduler.h b/libraries/AP_HAL/Scheduler.h
index 8606a31cd..7238370da 100644
--- a/libraries/AP_HAL/Scheduler.h
+++ b/libraries/AP_HAL/Scheduler.h
@@ -3,6 +3,7 @@
#define __AP_HAL_SCHEDULER_H__
#include "AP_HAL_Namespace.h"
+#include "AP_HAL_Boards.h"
#include <stdint.h>
#include <AP_Progmem.h>
@@ -14,6 +15,11 @@ 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
virtual void delay_microseconds(uint16_t us) = 0;
virtual void register_delay_callback(AP_HAL::Proc,
uint16_t min_time_ms) = 0;
diff --git a/libraries/AP_HAL_AVR_SITL/Scheduler.cpp b/libraries/AP_HAL_AVR_SITL/Scheduler.cpp
index db0d17282..5fa731f58 100644
--- a/libraries/AP_HAL_AVR_SITL/Scheduler.cpp
+++ b/libraries/AP_HAL_AVR_SITL/Scheduler.cpp
@@ -70,52 +70,64 @@ double SITLScheduler::_cyg_sec()
}
#endif
-uint32_t SITLScheduler::_micros()
+uint64_t SITLScheduler::_micros64()
{
#ifdef __CYGWIN__
- return (uint32_t)(_cyg_sec() * 1.0e6);
+ return (uint64_t)(_cyg_sec() * 1.0e6);
#else
struct timeval tp;
gettimeofday(&tp,NULL);
- return 1.0e6*((tp.tv_sec + (tp.tv_usec*1.0e-6)) -
+ uint64_t ret = 1.0e6*((tp.tv_sec + (tp.tv_usec*1.0e-6)) -
(_sketch_start_time.tv_sec +
(_sketch_start_time.tv_usec*1.0e-6)));
+ return ret;
#endif
}
+uint64_t SITLScheduler::micros64()
+{
+ return _micros64();
+}
+
uint32_t SITLScheduler::micros()
{
- return _micros();
+ return micros64() & 0xFFFFFFFF;
}
-uint32_t SITLScheduler::millis()
+uint64_t SITLScheduler::millis64()
{
#ifdef __CYGWIN__
// 1000 ms in a second
- return (uint32_t)(_cyg_sec() * 1000);
+ return (uint64_t)(_cyg_sec() * 1000);
#else
struct timeval tp;
gettimeofday(&tp,NULL);
- return 1.0e3*((tp.tv_sec + (tp.tv_usec*1.0e-6)) -
+ uint64_t ret = 1.0e3*((tp.tv_sec + (tp.tv_usec*1.0e-6)) -
(_sketch_start_time.tv_sec +
(_sketch_start_time.tv_usec*1.0e-6)));
+ return ret;
#endif
}
+uint32_t SITLScheduler::millis()
+{
+ return millis64() & 0xFFFFFFFF;
+}
+
void SITLScheduler::delay_microseconds(uint16_t usec)
{
- uint32_t start = micros();
- while (micros() - start < usec) {
- usleep(usec - (micros() - start));
+ uint64_t start = micros64();
+ while (micros64() - start < usec) {
+ usleep(usec - (micros64() - start));
}
}
void SITLScheduler::delay(uint16_t ms)
{
- uint32_t start = micros();
+ uint64_t start = micros64();
while (ms > 0) {
- while ((micros() - start) >= 1000) {
+ while ((micros64() - start) >= 1000) {
ms--;
if (ms == 0) break;
start += 1000;
diff --git a/libraries/AP_HAL_AVR_SITL/Scheduler.h b/libraries/AP_HAL_AVR_SITL/Scheduler.h
index bd6e33aba..8719e723e 100644
--- a/libraries/AP_HAL_AVR_SITL/Scheduler.h
+++ b/libraries/AP_HAL_AVR_SITL/Scheduler.h
@@ -19,6 +19,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);
@@ -43,7 +45,7 @@ public:
void sitl_end_atomic();
// callable from interrupt handler
- static uint32_t _micros();
+ static uint64_t _micros64();
static void timer_event() { _run_timer_procs(true); _run_io_procs(true); }
private:
diff --git a/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp b/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp
index 06714a84c..6792b0054 100644
--- a/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp
+++ b/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp
@@ -52,7 +52,7 @@ float SITL_State::_gyro_drift(void)
return 0;
}
double period = _sitl->drift_time * 2;
- double minutes = fmod(_scheduler->_micros() / 60.0e6, period);
+ double minutes = fmod(_scheduler->micros64() / 60.0e6, period);
if (minutes < period/2) {
return minutes * ToRad(_sitl->drift_speed);
}
diff --git a/libraries/AP_HAL_Empty/Scheduler.cpp b/libraries/AP_HAL_Empty/Scheduler.cpp
index f2a4e16f5..c12026c48 100644
--- a/libraries/AP_HAL_Empty/Scheduler.cpp
+++ b/libraries/AP_HAL_Empty/Scheduler.cpp
@@ -14,14 +14,22 @@ void EmptyScheduler::init(void* machtnichts)
void EmptyScheduler::delay(uint16_t ms)
{}
-uint32_t EmptyScheduler::millis() {
+uint64_t EmptyScheduler::millis64() {
return 10000;
}
-uint32_t EmptyScheduler::micros() {
+uint64_t EmptyScheduler::micros64() {
return 200000;
}
+uint32_t EmptyScheduler::millis() {
+ return millis64();
+}
+
+uint32_t EmptyScheduler::micros() {
+ return micros64();
+}
+
void EmptyScheduler::delay_microseconds(uint16_t us)
{}
diff --git a/libraries/AP_HAL_Empty/Scheduler.h b/libraries/AP_HAL_Empty/Scheduler.h
index 75df768dc..2afcbf460 100644
--- a/libraries/AP_HAL_Empty/Scheduler.h
+++ b/libraries/AP_HAL_Empty/Scheduler.h
@@ -11,6 +11,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_PX4/Scheduler.cpp b/libraries/AP_HAL_PX4/Scheduler.cpp
index b097ba046..856d684ef 100644
--- a/libraries/AP_HAL_PX4/Scheduler.cpp
+++ b/libraries/AP_HAL_PX4/Scheduler.cpp
@@ -76,6 +76,16 @@ void PX4Scheduler::init(void *unused)
pthread_create(&_io_thread_ctx, &thread_attr, (pthread_startroutine_t)&PX4::PX4Scheduler::_io_thread, this);
}
+uint64_t PX4Scheduler::micros64()
+{
+ return hrt_absolute_time();
+}
+
+uint64_t PX4Scheduler::millis64()
+{
+ return micros64() / 1000;
+}
+
uint32_t PX4Scheduler::micros()
{
return (uint32_t)(hrt_absolute_time() - _sketch_start_time);
@@ -83,7 +93,7 @@ uint32_t PX4Scheduler::micros()
uint32_t PX4Scheduler::millis()
{
- return hrt_absolute_time() / 1000;
+ return millis64() & 0xFFFFFFFF;
}
/**
@@ -106,9 +116,9 @@ void PX4Scheduler::delay_microseconds(uint16_t usec)
perf_end(_perf_delay);
return;
}
- uint32_t start = micros();
- uint32_t dt;
- while ((dt=(micros() - start)) < usec) {
+ uint64_t start = micros64();
+ uint64_t dt;
+ while ((dt=(micros64() - start)) < usec) {
up_udelay(usec - dt);
}
perf_end(_perf_delay);
@@ -121,9 +131,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 b88b997dd..891eeb345 100644
--- a/libraries/AP_HAL_PX4/Scheduler.h
+++ b/libraries/AP_HAL_PX4/Scheduler.h
@@ -29,6 +29,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);
void register_timer_process(AP_HAL::MemberProc);
--
GitLab