Skip to content
Snippets Groups Projects
Commit 0a9e679c authored by Michael Day's avatar Michael Day Committed by Michael Day
Browse files

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.
parent fe1b41c4
No related branches found
No related tags found
No related merge requests found
...@@ -13,6 +13,7 @@ public: ...@@ -13,6 +13,7 @@ public:
Scheduler() {} Scheduler() {}
virtual void init() = 0; virtual void init() = 0;
virtual void delay(uint16_t ms) = 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 delay for the given number of microseconds. This needs to be as
......
...@@ -11,7 +11,6 @@ using namespace HALSITL; ...@@ -11,7 +11,6 @@ using namespace HALSITL;
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
AP_HAL::Proc Scheduler::_failsafe = nullptr; AP_HAL::Proc Scheduler::_failsafe = nullptr;
volatile bool Scheduler::_timer_suspended = false; volatile bool Scheduler::_timer_suspended = false;
volatile bool Scheduler::_timer_event_missed = false; volatile bool Scheduler::_timer_event_missed = false;
...@@ -24,10 +23,19 @@ AP_HAL::MemberProc Scheduler::_io_proc[SITL_SCHEDULER_MAX_TIMER_PROCS] = {nullpt ...@@ -24,10 +23,19 @@ AP_HAL::MemberProc Scheduler::_io_proc[SITL_SCHEDULER_MAX_TIMER_PROCS] = {nullpt
uint8_t Scheduler::_num_io_procs = 0; uint8_t Scheduler::_num_io_procs = 0;
bool Scheduler::_in_io_proc = false; bool Scheduler::_in_io_proc = false;
struct timeval Scheduler::_sketch_start_time;
Scheduler::Scheduler(SITL_State *sitlState) : Scheduler::Scheduler(SITL_State *sitlState) :
_sitlState(sitlState), _sitlState(sitlState),
_stopped_clock_usec(0) _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() void Scheduler::init()
......
...@@ -22,6 +22,8 @@ public: ...@@ -22,6 +22,8 @@ public:
void delay_microseconds(uint16_t us); void delay_microseconds(uint16_t us);
void register_delay_callback(AP_HAL::Proc, uint16_t min_time_ms); 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_timer_process(AP_HAL::MemberProc);
void register_io_process(AP_HAL::MemberProc); void register_io_process(AP_HAL::MemberProc);
void suspend_timer_procs(); void suspend_timer_procs();
...@@ -74,5 +76,7 @@ private: ...@@ -74,5 +76,7 @@ private:
bool _initialized; bool _initialized;
uint64_t _stopped_clock_usec; uint64_t _stopped_clock_usec;
static struct timeval _sketch_start_time;
}; };
#endif // CONFIG_HAL_BOARD #endif // CONFIG_HAL_BOARD
...@@ -31,8 +31,12 @@ ...@@ -31,8 +31,12 @@
#include <DataFlash/DataFlash.h> #include <DataFlash/DataFlash.h>
#include <AP_Param/AP_Param.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 parent class for all simulator types
*/ */
...@@ -268,15 +272,38 @@ void Aircraft::sync_frame_time(void) ...@@ -268,15 +272,38 @@ void Aircraft::sync_frame_time(void)
static_cast<double>(rate_hz), static_cast<double>(rate_hz),
static_cast<double>(scaled_frame_time_us)); static_cast<double>(scaled_frame_time_us));
#endif #endif
const uint32_t sleep_time = static_cast<uint32_t>(scaled_frame_time_us * frame_counter); // Upstreams' method:
if (sleep_time > min_sleep_time) { //const uint32_t sleep_time = static_cast<uint32_t>(scaled_frame_time_us * frame_counter);
usleep(sleep_time); //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; last_wall_time_us = now;
frame_counter = 0; frame_counter = 0;
} }
} }
/* add noise based on throttle level (from 0..1) */ /* add noise based on throttle level (from 0..1) */
void Aircraft::add_noise(float throttle) void Aircraft::add_noise(float throttle)
{ {
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment