AutoTest: add support for GPS glitching

Tools/autotest/common.py

@@ -223,3 +223,9 @@ def mission_count(filename):
     wploader.load(filename)
     num_wp = wploader.count()
     return num_wp
+
+def sim_location(mav):
+    '''return current simulator location'''
+    from pymavlink import mavutil
+    m = mav.recv_match(type='SIMSTATE', blocking=True)
+    return mavutil.location(m.lat*1.0e-7, m.lng*1.0e-7, 0, math.degrees(m.yaw))

libraries/AP_HAL_AVR_SITL/sitl_gps.cpp

@@ -528,6 +528,7 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude,
 {
 	struct gps_data d;
 	char c;
+    Vector3f glitch_offsets = _sitl->gps_glitch;
 
 	// 5Hz, to match the real config in APM
 	if (hal.scheduler->millis() - gps_state.last_update < 200) {
@@ -541,9 +542,9 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude,
 
 	gps_state.last_update = hal.scheduler->millis();
 
-	d.latitude = latitude;
-	d.longitude = longitude;
-	d.altitude = altitude;
+	d.latitude = latitude + glitch_offsets.x;
+	d.longitude = longitude + glitch_offsets.y;
+	d.altitude = altitude + glitch_offsets.z;
 	d.speedN = speedN;
 	d.speedE = speedE;
 	d.speedD = speedD;

libraries/SITL/SITL.cpp

@@ -42,6 +42,7 @@ const AP_Param::GroupInfo SITL::var_info[] PROGMEM = {
     AP_GROUPINFO("GPS_NUMSATS",   14, SITL,  gps_numsats,   10),
     AP_GROUPINFO("MAG_ERROR",     15, SITL,  mag_error,  0),
     AP_GROUPINFO("SERVO_RATE",    16, SITL,  servo_rate,  0),
+    AP_GROUPINFO("GPS_GLITCH",    17, SITL,  gps_glitch,  0),
     AP_GROUPEND
 };
 

libraries/SITL/SITL.h

@@ -62,6 +62,7 @@ public:
     AP_Int8  gps_type;    // see enum GPSType
     AP_Float gps_byteloss;// byte loss as a percent
     AP_Int8  gps_numsats; // number of visible satellites
+    AP_Vector3f  gps_glitch;  // glitch offsets in lat, lon and altitude
 
     // wind control
     AP_Float wind_speed;