HAL_SITL: use a synthetic clock when possible

this decouples wall clock time from simulation time if the FDM
supports it

libraries/AP_HAL_AVR_SITL/AP_HAL_AVR_SITL_Main.h

@@ -10,7 +10,6 @@
         hal.scheduler->system_initialized(); \
         for(;;) { \
 		loop(); \
-		AVR_SITL::SITL_State::loop_hook(); \
 	} \
         return 0;\
     }\

libraries/AP_HAL_AVR_SITL/SITL_State.cpp

@@ -20,6 +20,9 @@
 #include <sys/select.h>
 
 #include <AP_Param.h>
+#include <pthread.h>
+
+typedef void *(*pthread_startroutine_t)(void *);
 
 #ifdef __CYGWIN__
 #include <stdio.h>
@@ -49,37 +52,11 @@ extern const AP_HAL::HAL& hal;
 
 using namespace AVR_SITL;
 
-enum SITL_State::vehicle_type SITL_State::_vehicle;
-uint16_t SITL_State::_framerate;
-uint16_t SITL_State::_base_port = 5760;
-uint16_t SITL_State::_rcout_port = 5502;
-uint16_t SITL_State::_simin_port = 5501;
-struct sockaddr_in SITL_State::_rcout_addr;
-pid_t SITL_State::_parent_pid;
-uint32_t SITL_State::_update_count;
-bool SITL_State::_motors_on;
-uint16_t SITL_State::sonar_pin_value;
-uint16_t SITL_State::airspeed_pin_value;
-uint16_t SITL_State::voltage_pin_value;
-uint16_t SITL_State::current_pin_value;
-float SITL_State::_current;
-
-AP_Baro *SITL_State::_barometer;
-AP_InertialSensor *SITL_State::_ins;
-SITLScheduler *SITL_State::_scheduler;
-Compass *SITL_State::_compass;
-OpticalFlow *SITL_State::_optical_flow;
-AP_Terrain *SITL_State::_terrain;
-
-int SITL_State::_sitl_fd;
-SITL *SITL_State::_sitl;
-uint16_t SITL_State::pwm_output[11];
-uint16_t SITL_State::last_pwm_output[11];
-uint16_t SITL_State::pwm_input[8];
-bool SITL_State::new_rc_input;
+// this allows loop_hook to be called
+SITL_State *g_state;
 
 // catch floating point exceptions
-void SITL_State::_sig_fpe(int signum)
+static void _sig_fpe(int signum)
 {
 	fprintf(stderr, "ERROR: Floating point exception - aborting\n");
     abort();
@@ -106,7 +83,12 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
     setvbuf(stdout, (char *)0, _IONBF, 0);
     setvbuf(stderr, (char *)0, _IONBF, 0);
 
-	while ((opt = getopt(argc, argv, "swhr:H:CI:P:")) != -1) {
+    _synthetic_clock_mode = false;
+    _base_port = 5760;
+    _rcout_port = 5502;
+    _simin_port = 5501;
+
+	while ((opt = getopt(argc, argv, "swhr:H:CI:P:S")) != -1) {
 		switch (opt) {
 		case 'w':
 			AP_Param::erase_all();
@@ -131,6 +113,9 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
 		case 'P':
             _set_param_default(optarg);
 			break;
+		case 'S':
+            _synthetic_clock_mode = true;
+			break;
 		default:
 			_usage();
 			exit(1);
@@ -205,7 +190,6 @@ void SITL_State::_sitl_setup(void)
 	_rcout_addr.sin_port = htons(_rcout_port);
 	inet_pton(AF_INET, "127.0.0.1", &_rcout_addr.sin_addr);
 
-	_setup_timer();
 #ifndef HIL_MODE
 	_setup_fdm();
 #endif
@@ -228,6 +212,28 @@ void SITL_State::_sitl_setup(void)
         _update_gps(0, 0, 0, 0, 0, 0, false);
 #endif
     }
+
+    if (_synthetic_clock_mode) {
+        // start with non-zero clock
+        hal.scheduler->stop_clock(100);
+    }
+
+    // setup a pipe used to trigger loop to stop sleeping
+    pipe(_fdm_pipe);
+    AVR_SITL::SITLUARTDriver::_set_nonblocking(_fdm_pipe[0]);
+    AVR_SITL::SITLUARTDriver::_set_nonblocking(_fdm_pipe[1]);
+
+    g_state = this;
+
+    /*
+      setup thread that receives input from the FDM
+     */
+    pthread_attr_t thread_attr;
+    pthread_attr_init(&thread_attr);
+
+    pthread_create(&_fdm_thread_ctx, &thread_attr, 
+                   (pthread_startroutine_t)&AVR_SITL::SITL_State::_fdm_thread, this);
+
 }
 
 
@@ -270,96 +276,89 @@ void SITL_State::_setup_fdm(void)
 
 
 /*
-  timer called at 1kHz
+  thread for FDM input
  */
-void SITL_State::_timer_handler(int signum)
+void SITL_State::_fdm_thread(void)
 {
-	static uint32_t last_update_count;
-    static uint32_t last_pwm_input;
+	uint32_t last_update_count = 0;
+    uint32_t last_pwm_input = 0;
 
-	static bool in_timer;
+    while (true) {
+        fd_set fds;
+        struct timeval tv;
 
-	if (in_timer || _scheduler->interrupts_are_blocked() || _sitl == NULL){
-		return;
-    }
+        if (next_stop_clock != 0) {
+            hal.scheduler->stop_clock(next_stop_clock);
+            next_stop_clock = 0;
+        }
 
-    _scheduler->sitl_begin_atomic();
-	in_timer = true;
+        tv.tv_sec = 1;
+        tv.tv_usec = 0;
 
-#ifndef __CYGWIN__
-	/* make sure we die if our parent dies */
-	if (kill(_parent_pid, 0) != 0) {
-		exit(1);
-	}
-#else
-    
-	static uint16_t count = 0;
-	static uint32_t last_report;
-        
-    count++;
-	if (hal.scheduler->millis() - last_report > 1000) {
-		fprintf(stdout, "TH %u cps\n", count);
-	//	print_trace();
-		count = 0;
-		last_report = hal.scheduler->millis();
-	}
-#endif
+        FD_ZERO(&fds);
+        FD_SET(_sitl_fd, &fds);
+        if (select(_sitl_fd+1, &fds, NULL, NULL, &tv) != 1) {
+            // internal error
+            _simulator_output(true);
+            continue;
+        }
 
-    // simulate RC input at 50Hz
-    if (hal.scheduler->millis() - last_pwm_input >= 20 && _sitl->rc_fail == 0) {
-        last_pwm_input = hal.scheduler->millis();
-        new_rc_input = true;
-    }
+        /* check for packet from flight sim */
+        _fdm_input();
 
-#ifndef HIL_MODE
-	/* check for packet from flight sim */
-	_fdm_input();
+        /* make sure we die if our parent dies */
+        if (kill(_parent_pid, 0) != 0) {
+            exit(1);
+        }
 
-	// send RC output to flight sim
-	_simulator_output();
-#endif
+        if (_scheduler->interrupts_are_blocked() || _sitl == NULL) {
+            continue;
+        }
 
-	if (_update_count == 0 && _sitl != NULL) {
-#ifndef HIL_MODE
-		_update_gps(0, 0, 0, 0, 0, 0, false);
-		_update_barometer(0);
-#endif
-		_scheduler->timer_event();
-        _scheduler->sitl_end_atomic();
-		in_timer = false;
-		return;
-	}
+        // simulate RC input at 50Hz
+        if (hal.scheduler->millis() - last_pwm_input >= 20 && _sitl->rc_fail == 0) {
+            last_pwm_input = hal.scheduler->millis();
+            new_rc_input = true;
+        }
 
-	if (_update_count == last_update_count) {
-		_scheduler->timer_event();
-        _scheduler->sitl_end_atomic();
-		in_timer = false;
-		return;
-	}
-	last_update_count = _update_count;
+        _scheduler->sitl_begin_atomic();
 
-    if (_sitl != NULL) {
-#ifndef HIL_MODE
-        _update_gps(_sitl->state.latitude, _sitl->state.longitude,
-                    _sitl->state.altitude,
-                    _sitl->state.speedN, _sitl->state.speedE, _sitl->state.speedD,
-                    !_sitl->gps_disable);
-        _update_ins(_sitl->state.rollDeg, _sitl->state.pitchDeg, _sitl->state.yawDeg,
-                    _sitl->state.rollRate, _sitl->state.pitchRate, _sitl->state.yawRate,
-                    _sitl->state.xAccel, _sitl->state.yAccel, _sitl->state.zAccel,
-                    _sitl->state.airspeed, _sitl->state.altitude);
-        _update_barometer(_sitl->state.altitude);
-        _update_compass(_sitl->state.rollDeg, _sitl->state.pitchDeg, _sitl->state.yawDeg);
-		_update_flow();
-#endif
-    }
+        if (_update_count == 0 && _sitl != NULL) {
+            _update_gps(0, 0, 0, 0, 0, 0, false);
+            _update_barometer(0);
+            _scheduler->timer_event();
+            _scheduler->sitl_end_atomic();
+            continue;
+        }
 
-	// trigger all APM timers. We do this last as it can re-enable
-	// interrupts, which can lead to recursion
-	_scheduler->timer_event();
+        if (_update_count == last_update_count) {
+            _scheduler->timer_event();
+            _scheduler->sitl_end_atomic();
+            continue;
+        }
+        last_update_count = _update_count;
+
+        if (_sitl != NULL) {
+            _update_gps(_sitl->state.latitude, _sitl->state.longitude,
+                        _sitl->state.altitude,
+                        _sitl->state.speedN, _sitl->state.speedE, _sitl->state.speedD,
+                        !_sitl->gps_disable);
+            _update_ins(_sitl->state.rollDeg, _sitl->state.pitchDeg, _sitl->state.yawDeg,
+                        _sitl->state.rollRate, _sitl->state.pitchRate, _sitl->state.yawRate,
+                        _sitl->state.xAccel, _sitl->state.yAccel, _sitl->state.zAccel,
+                        _sitl->state.airspeed, _sitl->state.altitude);
+            _update_barometer(_sitl->state.altitude);
+            _update_compass(_sitl->state.rollDeg, _sitl->state.pitchDeg, _sitl->state.yawDeg);
+            _update_flow();
+        }
 
-    _scheduler->sitl_end_atomic();
-	in_timer = false;
+        // trigger all APM timers. 
+        _scheduler->timer_event();
+        _scheduler->sitl_end_atomic();
+
+        char b = 0;
+        write(_fdm_pipe[1], &b, 1);
+    }
 }
 
 #ifndef HIL_MODE
@@ -373,12 +372,25 @@ void SITL_State::_fdm_input(void)
 		uint16_t pwm[8];
 	};
 	union {
+        struct {
+            uint64_t timestamp;
+            struct sitl_fdm fg_pkt;
+        } fg_pkt_timestamped;
 		struct sitl_fdm fg_pkt;
 		struct pwm_packet pwm_pkt;
 	} d;
+    bool got_fg_input = false;
+    next_stop_clock = 0;
 
 	size = recv(_sitl_fd, &d, sizeof(d), MSG_DONTWAIT);
 	switch (size) {
+    case 148:
+        /* a fg packate with a timestamp */
+        next_stop_clock = d.fg_pkt_timestamped.timestamp;
+        memmove(&d.fg_pkt, &d.fg_pkt_timestamped.fg_pkt, sizeof(d.fg_pkt));
+        _synthetic_clock_mode = true;
+        // fall through
+
 	case 140:
 		static uint32_t last_report;
 		static uint32_t count;
@@ -388,6 +400,8 @@ void SITL_State::_fdm_input(void)
 			return;
 		}
 
+        got_fg_input = true;
+
 		if (d.fg_pkt.latitude == 0 ||
 		    d.fg_pkt.longitude == 0 ||
 		    d.fg_pkt.altitude <= 0) {
@@ -428,6 +442,10 @@ void SITL_State::_fdm_input(void)
 	}
 	}
 
+    if (got_fg_input) {
+        // send RC output to flight sim
+        _simulator_output(_synthetic_clock_mode);
+    }
 }
 #endif
 
@@ -460,7 +478,7 @@ void SITL_State::_apply_servo_filter(float deltat)
 /*
   send RC outputs to simulator
  */
-void SITL_State::_simulator_output(void)
+void SITL_State::_simulator_output(bool synthetic_clock_mode)
 {
 	static uint32_t last_update_usec;
 	struct {
@@ -495,7 +513,7 @@ void SITL_State::_simulator_output(void)
 
 	// output at chosen framerate
     uint32_t now = hal.scheduler->micros();
-	if (last_update_usec != 0 && now - last_update_usec < 1000000/_framerate) {
+	if (!synthetic_clock_mode && last_update_usec != 0 && now - last_update_usec < 1000000/_framerate) {
 		return;
 	}
     float deltat = (now - last_update_usec) * 1.0e-6f;
@@ -577,28 +595,6 @@ void SITL_State::_simulator_output(void)
 	sendto(_sitl_fd, (void*)&control, sizeof(control), MSG_DONTWAIT, (const sockaddr *)&_rcout_addr, sizeof(_rcout_addr));
 }
 
-
-/*
-  setup a timer used to prod the ISRs
- */
-void SITL_State::_setup_timer(void)
-{
-	struct itimerval it;
-	struct sigaction act;
-
-	act.sa_handler = _timer_handler;
-        act.sa_flags = SA_RESTART|SA_NODEFER;
-        sigemptyset(&act.sa_mask);
-        sigaddset(&act.sa_mask, SIGALRM);
-        sigaction(SIGALRM, &act, NULL);
-
-	it.it_interval.tv_sec = 0;
-	it.it_interval.tv_usec = 1000; // 1KHz
-	it.it_value = it.it_interval;
-
-	setitimer(ITIMER_REAL, &it, NULL);
-}
-
 // generate a random float between -1 and 1
 float SITL_State::_rand_float(void)
 {
@@ -661,11 +657,20 @@ void SITL_State::loop_hook(void)
         FD_SET(fd, &fds);
         max_fd = max(fd, max_fd);
     }
+
+    FD_SET(_fdm_pipe[0], &fds);
+    max_fd = max(_fdm_pipe[0], max_fd);
+
     tv.tv_sec = 0;
     tv.tv_usec = 100;
     fflush(stdout);
     fflush(stderr);
     select(max_fd+1, &fds, NULL, NULL, &tv);
+    
+    if (FD_ISSET(_fdm_pipe[0], &fds)) {
+        char b;
+        read(_fdm_pipe[0], &b, 1);
+    }
 }
 
 

libraries/AP_HAL_AVR_SITL/SITL_State.h

@@ -39,18 +39,18 @@ public:
     int gps_pipe(void);
     int gps2_pipe(void);
     ssize_t gps_read(int fd, void *buf, size_t count);
-    static uint16_t pwm_output[11];
-    static uint16_t last_pwm_output[11];
-    static uint16_t pwm_input[8];
-    static bool new_rc_input;
-    static void loop_hook(void);
+    uint16_t pwm_output[11];
+    uint16_t last_pwm_output[11];
+    uint16_t pwm_input[8];
+    bool new_rc_input;
+    void loop_hook(void);
     uint16_t base_port(void) const { return _base_port; }
 
     // simulated airspeed, sonar and battery monitor
-    static uint16_t sonar_pin_value;    // pin 0
-    static uint16_t airspeed_pin_value; // pin 1
-    static uint16_t voltage_pin_value;  // pin 13
-    static uint16_t current_pin_value;  // pin 12
+    uint16_t sonar_pin_value;    // pin 0
+    uint16_t airspeed_pin_value; // pin 1
+    uint16_t voltage_pin_value;  // pin 13
+    uint16_t current_pin_value;  // pin 12
 
 private:
     void _parse_command_line(int argc, char * const argv[]);
@@ -61,12 +61,10 @@ private:
     void _setup_timer(void);
     void _setup_adc(void);
 
-    // these methods are static as they are called
-    // from the timer
-    static float height_agl(void);
-    static void _update_barometer(float height);
-    static void _update_compass(float rollDeg, float pitchDeg, float yawDeg);
-    static void _update_flow(void);
+    float height_agl(void);
+    void _update_barometer(float height);
+    void _update_compass(float rollDeg, float pitchDeg, float yawDeg);
+    void _update_flow(void);
 
     struct gps_data {
 	    double latitude;
@@ -79,64 +77,67 @@ private:
     };
 
     #define MAX_GPS_DELAY 100
-    static gps_data _gps_data[MAX_GPS_DELAY];
-
-    static bool _gps_has_basestation_position;
-    static gps_data _gps_basestation_data;
-    static void _gps_write(const uint8_t *p, uint16_t size);
-    static void _gps_send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size);
-    static void _update_gps_ubx(const struct gps_data *d);
-    static void _update_gps_mtk(const struct gps_data *d);
-    static void _update_gps_mtk16(const struct gps_data *d);
-    static void _update_gps_mtk19(const struct gps_data *d);
-    static uint16_t _gps_nmea_checksum(const char *s);
-    static void _gps_nmea_printf(const char *fmt, ...);
-    static void _update_gps_nmea(const struct gps_data *d);
-    static void _sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_t len, uint8_t *payload);
-    static void _update_gps_sbp(const struct gps_data *d, bool sim_rtk);
-
-    static void _update_gps(double latitude, double longitude, float altitude,
+    gps_data _gps_data[MAX_GPS_DELAY];
+
+    bool _gps_has_basestation_position;
+    gps_data _gps_basestation_data;
+    void _gps_write(const uint8_t *p, uint16_t size);
+    void _gps_send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size);
+    void _update_gps_ubx(const struct gps_data *d);
+    void _update_gps_mtk(const struct gps_data *d);
+    void _update_gps_mtk16(const struct gps_data *d);
+    void _update_gps_mtk19(const struct gps_data *d);
+    uint16_t _gps_nmea_checksum(const char *s);
+    void _gps_nmea_printf(const char *fmt, ...);
+    void _update_gps_nmea(const struct gps_data *d);
+    void _sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_t len, uint8_t *payload);
+    void _update_gps_sbp(const struct gps_data *d, bool sim_rtk);
+
+    void _update_gps(double latitude, double longitude, float altitude,
 			    double speedN, double speedE, double speedD, bool have_lock);
 
-    static void _update_ins(float roll, 	float pitch, 	float yaw,		// Relative to earth
+    void _update_ins(float roll, 	float pitch, 	float yaw,		// Relative to earth
 			    double rollRate, 	double pitchRate,double yawRate,	// Local to plane
 			    double xAccel, 	double yAccel, 	double zAccel,		// Local to plane
 			    float airspeed,	float altitude);
-    static void _fdm_input(void);
-    static void _simulator_output(void);
-    static void _apply_servo_filter(float deltat);
-    static uint16_t _airspeed_sensor(float airspeed);
-    static uint16_t _ground_sonar();
-    static float _gyro_drift(void);
-    static float _rand_float(void);
-    static Vector3f _rand_vec3f(void);
-
-    // signal handlers
-    static void _sig_fpe(int signum);
-    static void _timer_handler(int signum);
+    void _fdm_input(void);
+    void _simulator_output(bool synthetic_clock_mode);
+    void _apply_servo_filter(float deltat);
+    uint16_t _airspeed_sensor(float airspeed);
+    uint16_t _ground_sonar();
+    float _gyro_drift(void);
+    float _rand_float(void);
+    Vector3f _rand_vec3f(void);
+
+    pthread_t _fdm_thread_ctx;
+    void _fdm_thread(void);
+    int _fdm_pipe[2];
+    uint64_t next_stop_clock;
 
     // internal state
-    static enum vehicle_type _vehicle;
-    static uint16_t _framerate;
-    static uint16_t _base_port;
+    enum vehicle_type _vehicle;
+    uint16_t _framerate;
+    uint16_t _base_port;
     float _initial_height;
-    static struct sockaddr_in _rcout_addr;
-    static pid_t _parent_pid;
-    static uint32_t _update_count;
-    static bool _motors_on;
-
-    static AP_Baro *_barometer;
-    static AP_InertialSensor *_ins;
-    static SITLScheduler *_scheduler;
-    static Compass *_compass;
-    static OpticalFlow *_optical_flow;
-    static AP_Terrain *_terrain;
-
-    static int _sitl_fd;
-    static SITL *_sitl;
-    static uint16_t _rcout_port;
-    static uint16_t _simin_port;
-    static float _current;
+    struct sockaddr_in _rcout_addr;
+    pid_t _parent_pid;
+    uint32_t _update_count;
+    bool _motors_on;
+
+    AP_Baro *_barometer;
+    AP_InertialSensor *_ins;
+    SITLScheduler *_scheduler;
+    Compass *_compass;
+    OpticalFlow *_optical_flow;
+    AP_Terrain *_terrain;
+
+    int _sitl_fd;
+    SITL *_sitl;
+    uint16_t _rcout_port;
+    uint16_t _simin_port;
+    float _current;
+
+    bool _synthetic_clock_mode;
 };
 
 #endif // CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL

libraries/AP_HAL_AVR_SITL/Scheduler.cpp

@@ -8,6 +8,8 @@
 #include <sys/time.h>
 #include <unistd.h>
 #include <fenv.h>
+#include <signal.h>
+#include <pthread.h>
 
 #ifdef __CYGWIN__
 #include <stdio.h> 
@@ -44,9 +46,16 @@ double SITLScheduler::_cyg_freq = 0;
 long SITLScheduler::_cyg_start = 0;
 #endif
 
+static void sigcont_handler(int)
+{
+}
+
 SITLScheduler::SITLScheduler(SITL_State *sitlState) :
-    _sitlState(sitlState)
-{}
+    _sitlState(sitlState),
+    stopped_clock_usec(0)
+{
+    signal(SIGCONT, sigcont_handler);
+}
 
 void SITLScheduler::init(void *unused) 
 {
@@ -88,6 +97,9 @@ uint64_t SITLScheduler::_micros64()
 
 uint64_t SITLScheduler::micros64() 
 {
+    if (stopped_clock_usec) {
+        return stopped_clock_usec;
+    }
     return _micros64();
 }
 
@@ -98,6 +110,9 @@ uint32_t SITLScheduler::micros()
 
 uint64_t SITLScheduler::millis64() 
 {
+    if (stopped_clock_usec) {
+        return stopped_clock_usec/1000;
+    }
 #ifdef __CYGWIN__
 	// 1000 ms in a second
 	return (uint64_t)(_cyg_sec() * 1000);
@@ -116,24 +131,29 @@ uint32_t SITLScheduler::millis()
     return millis64() & 0xFFFFFFFF;
 }
 
+extern AVR_SITL::SITL_State *g_state;
+
+
 void SITLScheduler::delay_microseconds(uint16_t usec) 
 {
 	uint64_t start = micros64();
-	while (micros64() - start < usec) {
-		usleep(usec - (micros64() - start));
+    uint64_t dtime;
+	while ((dtime=(micros64() - start) < usec)) {
+        if (stopped_clock_usec) {
+            wait_time_usec = start + usec;
+            pause();
+            wait_time_usec = 0;
+        } else {
+            usleep(usec - dtime);
+        }
 	}
 }
 
 void SITLScheduler::delay(uint16_t ms)
 {
-	uint64_t start = micros64();
-    
     while (ms > 0) {
-        while ((micros64() - start) >= 1000) {
-            ms--;
-            if (ms == 0) break;
-            start += 1000;
-        }
+        delay_microseconds(1000);
+        ms--;
         if (_min_delay_cb_ms <= ms) {
             if (_delay_cb) {
                 _delay_cb();
@@ -294,4 +314,24 @@ void SITLScheduler::panic(const prog_char_t *errormsg) {
     for(;;);
 }
 
+/*
+  set simulation timestamp
+ */
+void SITLScheduler::stop_clock(uint64_t time_usec)
+{
+    stopped_clock_usec = time_usec;
+    /*
+      we want to ensure the main thread 
+     */
+    while (wait_time_usec == 0) {
+        pthread_yield();
+    }
+    kill(0, SIGCONT);
+    while (wait_time_usec != 0 && wait_time_usec <= time_usec) {
+        kill(0, SIGCONT);
+        pthread_yield();
+    }
+    _run_io_procs(false);
+}
+
 #endif

libraries/AP_HAL_AVR_SITL/Scheduler.h

@@ -73,8 +73,11 @@ private:
     static double _cyg_sec();
 #endif
 
-    bool _initialized;
+    void stop_clock(uint64_t time_usec);
 
+    bool _initialized;
+    volatile uint64_t stopped_clock_usec;
+    volatile uint64_t wait_time_usec;
 };
 #endif
 #endif // __AP_HAL_SITL_SCHEDULER_H__

libraries/AP_HAL_AVR_SITL/sitl_gps.cpp

@@ -32,9 +32,6 @@ extern const AP_HAL::HAL& hal;
 
 static uint8_t next_gps_index;
 static uint8_t gps_delay;
-SITL_State::gps_data SITL_State::_gps_data[MAX_GPS_DELAY];
-bool SITL_State::_gps_has_basestation_position = false;
-SITL_State::gps_data SITL_State::_gps_basestation_data;
 
 // state of GPS emulation
 static struct gps_state {