From 0a9e679cab6bf108115af48321b337c93f792ab6 Mon Sep 17 00:00:00 2001
From: Michael Day <mday299@yahoo.com>
Date: Thu, 4 Jun 2015 22:43:05 -0600
Subject: [PATCH] AP_HAL_SITL: Temporary solution for clock drifts.
Note that this differs slightly from the clock method used in
upstream master. This keeps the Ardupilot/JSBSim clocks from
drifting away from the ACS ROS clock.
---
libraries/AP_HAL/Scheduler.h | 1 +
libraries/AP_HAL_SITL/Scheduler.cpp | 10 ++++++++-
libraries/AP_HAL_SITL/Scheduler.h | 4 ++++
libraries/SITL/SIM_Aircraft.cpp | 35 +++++++++++++++++++++++++----
4 files changed, 45 insertions(+), 5 deletions(-)
diff --git a/libraries/AP_HAL/Scheduler.h b/libraries/AP_HAL/Scheduler.h
index 6c84db550a..e6db874372 100644
--- a/libraries/AP_HAL/Scheduler.h
+++ b/libraries/AP_HAL/Scheduler.h
@@ -13,6 +13,7 @@ public:
Scheduler() {}
virtual void init() = 0;
virtual void delay(uint16_t ms) = 0;
+ virtual uint64_t get_start_time_micros64() { return 0L; }
/*
delay for the given number of microseconds. This needs to be as
diff --git a/libraries/AP_HAL_SITL/Scheduler.cpp b/libraries/AP_HAL_SITL/Scheduler.cpp
index 3a87a49c95..7a4255fa12 100644
--- a/libraries/AP_HAL_SITL/Scheduler.cpp
+++ b/libraries/AP_HAL_SITL/Scheduler.cpp
@@ -11,7 +11,6 @@ using namespace HALSITL;
extern const AP_HAL::HAL& hal;
-
AP_HAL::Proc Scheduler::_failsafe = nullptr;
volatile bool Scheduler::_timer_suspended = false;
volatile bool Scheduler::_timer_event_missed = false;
@@ -24,10 +23,19 @@ AP_HAL::MemberProc Scheduler::_io_proc[SITL_SCHEDULER_MAX_TIMER_PROCS] = {nullpt
uint8_t Scheduler::_num_io_procs = 0;
bool Scheduler::_in_io_proc = false;
+struct timeval Scheduler::_sketch_start_time;
+
Scheduler::Scheduler(SITL_State *sitlState) :
_sitlState(sitlState),
_stopped_clock_usec(0)
{
+ gettimeofday(&_sketch_start_time,NULL);
+}
+
+uint64_t Scheduler::get_start_time_micros64()
+{
+ return (1.0e6 * _sketch_start_time.tv_sec +
+ _sketch_start_time.tv_usec);
}
void Scheduler::init()
diff --git a/libraries/AP_HAL_SITL/Scheduler.h b/libraries/AP_HAL_SITL/Scheduler.h
index 56805ce52c..7e46482751 100644
--- a/libraries/AP_HAL_SITL/Scheduler.h
+++ b/libraries/AP_HAL_SITL/Scheduler.h
@@ -22,6 +22,8 @@ public:
void delay_microseconds(uint16_t us);
void register_delay_callback(AP_HAL::Proc, uint16_t min_time_ms);
+ uint64_t get_start_time_micros64();
+
void register_timer_process(AP_HAL::MemberProc);
void register_io_process(AP_HAL::MemberProc);
void suspend_timer_procs();
@@ -74,5 +76,7 @@ private:
bool _initialized;
uint64_t _stopped_clock_usec;
+
+ static struct timeval _sketch_start_time;
};
#endif // CONFIG_HAL_BOARD
diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp
index d809d6a481..807aceb5ab 100644
--- a/libraries/SITL/SIM_Aircraft.cpp
+++ b/libraries/SITL/SIM_Aircraft.cpp
@@ -31,8 +31,12 @@
#include <DataFlash/DataFlash.h>
#include <AP_Param/AP_Param.h>
-namespace SITL {
+#include <AP_HAL_SITL/Scheduler.h>
+
+extern const AP_HAL::HAL& hal;
+using HALSITL::Scheduler;
+namespace SITL {
/*
parent class for all simulator types
*/
@@ -268,15 +272,38 @@ void Aircraft::sync_frame_time(void)
static_cast<double>(rate_hz),
static_cast<double>(scaled_frame_time_us));
#endif
- const uint32_t sleep_time = static_cast<uint32_t>(scaled_frame_time_us * frame_counter);
- if (sleep_time > min_sleep_time) {
- usleep(sleep_time);
+ // Upstreams' method:
+ //const uint32_t sleep_time = static_cast<uint32_t>(scaled_frame_time_us * frame_counter);
+ //if (sleep_time > min_sleep_time) {
+ // usleep(sleep_time);
+
+ //ACS method:
+ HALSITL::Scheduler* scheduler = HALSITL::Scheduler::from(hal.scheduler);
+ uint64_t first_wall_time_us = scheduler->get_start_time_micros64();
+ uint64_t sim_time = first_wall_time_us + AP_HAL::micros64();
+ uint64_t scaled_wall_time_us = first_wall_time_us +
+ (uint64_t) ((now - first_wall_time_us) * target_speedup);
+ int64_t diff = scaled_wall_time_us - sim_time;
+
+#if 0
+ printf("%f = %lf - %lf \n", float(diff) / 1000000.0f,
+ double(scaled_wall_time_us) / 1000000.0,
+ double(sim_time) / 1000000.0);
+#endif //0
+
+ //ACS mod for keeping ROS clocked lined up with Ardupilot and JSBSim
+ //clocks:
+ //slow down a bit if sim_time has gotten ahead of wall_time
+ if (diff < 0) {
+ usleep(100000);
}
+
last_wall_time_us = now;
frame_counter = 0;
}
}
+
/* add noise based on throttle level (from 0..1) */
void Aircraft::add_noise(float throttle)
{
--
GitLab