diff --git a/libraries/AP_HAL/Scheduler.h b/libraries/AP_HAL/Scheduler.h
index 6c84db550a28ce4f33bf9f26710881d88c8b9df9..e6db874372306322d4c0763327b19d492d3b7d4e 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 3a87a49c95136c3efaebb8efa76fbad8e2a226b4..7a4255fa121da5e5b6d19729eed29512c5a4d650 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 56805ce52cf8e6f41cca8194db98b7047de022ed..7e46482751da9fd34bd2279ba54d2d634f32e021 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 d809d6a4811ee540250608037ac06074dd7842db..807aceb5abfa850cad7b102d2dbb6fd456cc937f 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)
 {