AutoTest: add copter stability patch test

Tools/autotest/arducopter.py

@@ -263,6 +263,57 @@ def fly_throttle_failsafe(mavproxy, mav, side=60, timeout=180):
     mavproxy.send('switch 6\n')
     return False
 
+# fly_stability_patch - fly south, then hold loiter within 5m position and altitude and reduce 1 motor to 60% efficiency
+def fly_stability_patch(mavproxy, mav, holdtime=30, maxaltchange=5, maxdistchange=5):
+    '''hold loiter position'''
+    mavproxy.send('switch 5\n') # loiter mode
+    wait_mode(mav, 'LOITER')
+
+    # first south
+    print("turn south")
+    mavproxy.send('rc 4 1580\n')
+    if not wait_heading(mav, 180):
+        return False
+    mavproxy.send('rc 4 1500\n')
+
+    #fly west 80m
+    mavproxy.send('rc 2 1100\n')
+    if not wait_distance(mav, 80):
+        return False
+    mavproxy.send('rc 2 1500\n')
+
+    # wait for copter to slow moving
+    if not wait_groundspeed(mav, 0, 2):
+        return False
+
+    m = mav.recv_match(type='VFR_HUD', blocking=True)
+    start_altitude = m.alt
+    start = mav.location()
+    tstart = time.time()
+    tholdstart = time.time()
+    print("Holding loiter at %u meters for %u seconds" % (start_altitude, holdtime))
+
+    # cut motor 1 to 55% efficiency
+    print("Cutting motor 1 to 55% efficiency")
+    mavproxy.send('param set SIM_ENGINE_MUL 0.55\n')
+
+    while time.time() < tstart + holdtime:
+        m = mav.recv_match(type='VFR_HUD', blocking=True)
+        pos = mav.location()
+        delta = get_distance(start, pos)
+        print("Loiter Dist: %.2fm, alt:%u" % (delta, m.alt))
+        if math.fabs(m.alt - start_altitude) > maxaltchange:
+            tholdstart = time.time()    # this will cause this test to timeout and fails
+        if delta > maxdistchange:
+            tholdstart = time.time()    # this will cause this test to timeout and fails
+        if time.time() - tholdstart > holdtime:
+            print("Stability patch and Loiter OK for %u seconds" % holdtime)
+            # restore motor 1 to 100% efficiency
+            mavproxy.send('param set SIM_ENGINE_MUL 1.0\n')
+            return True
+    print("Stability Patch FAILED")
+    return False
+
 #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):
@@ -497,7 +548,7 @@ def fly_ArduCopter(viewerip=None, map=False):
         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 ##########")
@@ -543,7 +594,7 @@ def fly_ArduCopter(viewerip=None, map=False):
 
         # Loiter for 30 seconds
         print("#")
-        print("########## Test Loiter for 40 seconds ##########")
+        print("########## Test Loiter for 30 seconds ##########")
         print("#")
         if not loiter(mavproxy, mav, 30):
             print("loiter failed")
@@ -583,6 +634,26 @@ def fly_ArduCopter(viewerip=None, map=False):
             print("takeoff failed")
             failed = True
 
+        ### Stability patch ###
+        print("#")
+        print("########## Test Stability Patch ##########")
+        print("#")
+        if not fly_stability_patch(mavproxy, mav, 30):
+            print("Stability Patch failed")
+            failed = True
+
+        # RTL
+        print("# RTL #")
+        if not fly_RTL(mavproxy, mav):
+            print("RTL failed")
+            failed = True
+
+        # Takeoff
+        print("# Takeoff")
+        if not takeoff(mavproxy, mav, 10):
+            print("takeoff failed")
+            failed = True
+
         # Simple mode
         print("# Fly in SIMPLE mode")
         if not fly_simple(mavproxy, mav):