AutoTest: add copter gps glitch test

Tools/autotest/arducopter.py

@@ -392,6 +392,89 @@ def fly_fence_test(mavproxy, mav, timeout=180):
     print("Fence test failed to reach home - timed out after %u seconds" % timeout)
     return False
 
+# fly_gps_glitch_test - fly south east and deal with gps glitch
+def fly_gps_glitch_test(mavproxy, mav, timeout=30, max_distance=100):
+    '''hold loiter position'''
+    mavproxy.send('switch 5\n') # loiter mode
+    wait_mode(mav, 'LOITER')
+
+    # turn on simulator display of gps and actual position
+    mavproxy.send('map set showgpspos 1\n')
+    mavproxy.send('map set showsimpos 1\n')
+
+    # set-up gps glitch array
+    glitch_lat = [0.0002996,0.0006958,0.0009431,0.0009991,0.0009444,0.0007716,0.0006221]
+    glitch_lon = [0.0000717,0.0000912,0.0002761,0.0002626,0.0002807,0.0002049,0.0001304]
+    glitch_num = len(glitch_lat)
+    print("GPS Glitches:")
+    for i in range(1,glitch_num):
+        print("glitch %d %.7f %.7f" % (i,glitch_lat[i],glitch_lon[i]))
+
+    # turn south east
+    print("turn south east")
+    mavproxy.send('rc 4 1580\n')
+    if not wait_heading(mav, 150):
+        return False
+    mavproxy.send('rc 4 1500\n')
+
+    # fly forward (south east) at least 60m
+    mavproxy.send('rc 2 1100\n')
+    if not wait_distance(mav, 60):
+        return False
+    mavproxy.send('rc 2 1500\n')
+
+    # wait for copter to slow down
+    if not wait_groundspeed(mav, 0, 2):
+        return False
+
+    # record time and position
+    tstart = time.time()
+    start_pos = sim_location(mav)
+
+    # initialise current glitch
+    glitch_current = 0;
+    print("Apply first glitch")
+    mavproxy.send('param set SIM_GPS_GLITCH_X %.7f\n' % glitch_lat[glitch_current])
+    mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current])
+
+    # record position for 30 seconds
+    while time.time() < tstart + timeout:
+
+        time_in_sec = int(time.time() - tstart);
+        #print("time: %d" % time_in_sec)
+        if time_in_sec > glitch_current and glitch_current != -1:
+            glitch_current = time_in_sec
+            # turn off glitching if we've reached the end of the glitch list
+            if glitch_current >= glitch_num:
+                glitch_current = -1
+                mavproxy.send('param set SIM_GPS_GLITCH_X 0\n')
+                mavproxy.send('param set SIM_GPS_GLITCH_Y 0\n')
+            else:
+                #move onto the next glitch
+                mavproxy.send('param set SIM_GPS_GLITCH_X %.7f\n' % glitch_lat[glitch_current])
+                mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current])
+
+        m = mav.recv_match(type='VFR_HUD', blocking=True)
+        curr_pos = sim_location(mav)
+        moved_distance = get_distance(curr_pos, start_pos)
+        print("Alt: %u  Moved: %.0f" % (m.alt, moved_distance))
+        if moved_distance > max_distance:
+            print("Moved over %u meters, Failed!" % max_distance)
+            # disable gps glitch
+            mavproxy.send('param set SIM_GPS_GLITCH_X 0\n')
+            mavproxy.send('param set SIM_GPS_GLITCH_Y 0\n')
+            return False
+
+    # disable gps glitch
+    if glitch_current != -1:
+        glitch_current = -1
+        mavproxy.send('param set SIM_GPS_GLITCH_X 0\n')
+        mavproxy.send('param set SIM_GPS_GLITCH_Y 0\n')
+
+    # if we've gotten this far then we've succeeded
+    print("GPS glitch test passed!  stayed within %u meters for %u seconds" % (max_distance, timeout))
+    return True
+
 #fly_simple - assumes the simple bearing is initialised to be directly north
 #   flies a box with 100m west, 15 seconds north, 50 seconds east, 15 seconds south
 def fly_simple(mavproxy, mav, side=100, timeout=120):
@@ -645,6 +728,24 @@ def fly_ArduCopter(viewerip=None, map=False):
             print("takeoff failed")
             failed = True
 
+        # Fly GPS Glitch test 1
+        print("# GPS Glitch Test 1")
+        if not fly_gps_glitch_test(mavproxy, mav):
+            print("failed GPS glitch test")
+            failed = True
+
+        # RTL after GPS Glitch test
+        print("# RTL #")
+        if not fly_RTL(mavproxy, mav):
+            print("RTL failed")
+            failed = True
+
+        # take-off ahead of next test
+        print("# Takeoff")
+        if not takeoff(mavproxy, mav, 10):
+            print("takeoff failed")
+            failed = True
+
         # Fly a square in Stabilize mode
         print("#")
         print("########## Fly A square and save WPs with CH7 switch ##########")