Fixed scaling issue with Circle mode

ArduCopter/Parameters.h

@@ -322,7 +322,7 @@ public:
 	command_index			(0,							k_param_command_index,					PSTR("WP_INDEX")),
 	command_must_index		(0,							k_param_command_must_index,				PSTR("WP_MUST_INDEX")),
 	waypoint_radius			(WP_RADIUS_DEFAULT,			k_param_waypoint_radius,				PSTR("WP_RADIUS")),
-	loiter_radius			(LOITER_RADIUS,		        k_param_loiter_radius,					PSTR("WP_LOITER_RAD")),
+	loiter_radius			(LOITER_RADIUS * 100,	    k_param_loiter_radius,					PSTR("WP_LOITER_RAD")),
 	waypoint_speed_max		(WAYPOINT_SPEED_MAX,		k_param_waypoint_speed_max,				PSTR("WP_SPEED_MAX")),
 
 	throttle_min			(0,							k_param_throttle_min,					PSTR("THR_MIN")),

Tools/autotest/ArduCopter.parm

@@ -24,9 +24,9 @@ RC7_TRIM        1500.000000
 RC8_MAX         2000.000000
 RC8_MIN         1000.000000
 RC8_TRIM        1500.000000
-FLTMODE1	3
+FLTMODE1	7
-FLTMODE2	1
+FLTMODE2	5
-FLTMODE3	2
+FLTMODE3	4
-FLTMODE4	6
+FLTMODE4	3
-FLTMODE5	5
+FLTMODE5	1
 FLTMODE6	0

Tools/autotest/arducopter.py

@@ -6,11 +6,12 @@ import util, pexpect, sys, time, math, shutil, os
 testdir=os.path.dirname(os.path.realpath(__file__))
 
 sys.path.insert(0, util.reltopdir('../pymavlink'))
-import mavutil
+import mavutil, mavwp
 
 HOME_LOCATION='-35.362938,149.165085,584,270'
 
 homeloc = None
+num_wp = 0
 
 # a list of pexpect objects to read while waiting for
 # messages. This keeps the output to stdout flowing
@@ -19,8 +20,8 @@ expect_list = []
 def message_hook(mav, msg):
     '''called as each mavlink msg is received'''
     global expect_list
-    if msg.get_type() in [ 'NAV_CONTROLLER_OUTPUT', 'GPS_RAW' ]:
+    #if msg.get_type() in [ 'NAV_CONTROLLER_OUTPUT', 'GPS_RAW' ]:
-        print(msg)
+    #    print(msg)
     for p in expect_list:
         try:
             p.read_nonblocking(100, timeout=0)
@@ -39,7 +40,6 @@ def expect_callback(e):
         except pexpect.TIMEOUT:
             pass
 
-
 class location(object):
     '''represent a GPS coordinate'''
     def __init__(self, lat, lng, alt=0):
@@ -71,7 +71,7 @@ def current_location(mav):
                     mav.messages['GPS_RAW'].lon,
                     mav.messages['VFR_HUD'].alt)
 
-def wait_altitude(mav, alt_min, alt_max, timeout=30):
+def wait_altitude(mav, alt_min, alt_max, timeout=60):
     '''wait for a given altitude range'''
     tstart = time.time()
     print("Waiting for altitude between %u and %u" % (alt_min, alt_max))
@@ -83,6 +83,44 @@ def wait_altitude(mav, alt_min, alt_max, timeout=30):
     print("Failed to attain altitude range")
     return False
 
+def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, timeout=60):
+    '''wait for waypoint ranges'''
+    tstart = time.time()
+    m = mav.recv_match(type='WAYPOINT_CURRENT', blocking=True)
+
+    start_wp = m.seq
+    current_wp = start_wp
+
+    print("\n***wait for waypoint ranges***\n\n\n")
+    if start_wp != wpnum_start:
+        print("Expected start waypoint %u but got %u" % (wpnum_start, start_wp))
+        return False
+
+    while time.time() < tstart + timeout:
+        m = mav.recv_match(type='WAYPOINT_CURRENT', blocking=True)
+        print("WP %u" % m.seq)
+        if m.seq == current_wp:
+            continue
+        if m.seq == current_wp+1 or (m.seq > current_wp+1 and allow_skip):
+            print("Starting new waypoint %u" % m.seq)
+            tstart = time.time()
+            current_wp = m.seq
+            if current_wp == wpnum_end:
+                print("Reached final waypoint %u" % m.seq)
+                return True
+        if m.seq > current_wp+1:
+            print("Skipped waypoint! Got wp %u expected %u" % (m.seq, current_wp+1))
+            return False
+    print("Timed out waiting for waypoint %u" % wpnum_end)
+    return False
+
+def save_wp(mavproxy, mav):
+    mavproxy.send('rc 7 2000\n')
+    mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==2000', blocking=True)
+    mavproxy.send('rc 7 1000\n')
+    mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==1000', blocking=True)
+    mavproxy.send('wp list\n')
+
 
 def arm_motors(mavproxy):
     '''arm motors'''
@@ -120,7 +158,7 @@ def takeoff(mavproxy, mav):
 
 def loiter(mavproxy, mav, maxaltchange=10, holdtime=10, timeout=60):
     '''hold loiter position'''
-    mavproxy.send('switch 5\n') # loiter mode
+    mavproxy.send('switch 2\n') # loiter mode
     mavproxy.expect('LOITER>')
     mavproxy.send('status\n')
     mavproxy.expect('>')
@@ -193,6 +231,11 @@ def fly_square(mavproxy, mav, side=50, timeout=120):
     mavproxy.expect('STABILIZE>')
     tstart = time.time()
     failed = False
+
+    print("Save WP 1")
+    save_wp(mavproxy, mav)
+
+    print("turn")
     mavproxy.send('rc 3 1430\n')
     mavproxy.send('rc 4 1610\n')
     if not wait_heading(mav, 0):
@@ -205,23 +248,37 @@ def fly_square(mavproxy, mav, side=50, timeout=120):
         failed = True
     mavproxy.send('rc 2 1500\n')
 
+    print("Save WP 2")
+    save_wp(mavproxy, mav)
+
     print("Going east %u meters" % side)
     mavproxy.send('rc 1 1610\n')
     if not wait_distance(mav, side):
         failed = True
     mavproxy.send('rc 1 1500\n')
 
+    print("Save WP 3")
+    save_wp(mavproxy, mav)
+
     print("Going south %u meters" % side)
     mavproxy.send('rc 2 1610\n')
     if not wait_distance(mav, side):
         failed = True
     mavproxy.send('rc 2 1500\n')
+    mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==1000', blocking=True)
+
+    print("Save WP 4")
+    save_wp(mavproxy, mav)
 
     print("Going west %u meters" % side)
     mavproxy.send('rc 1 1390\n')
     if not wait_distance(mav, side):
         failed = True
     mavproxy.send('rc 1 1500\n')
+
+    print("Save WP 5")
+    save_wp(mavproxy, mav)
+
     return not failed
 
 
@@ -243,6 +300,7 @@ def land(mavproxy, mav, timeout=60):
     # now let it settle gently
     mavproxy.send('rc 3 1400\n')
     tstart = time.time()
+
     if wait_altitude(mav, -5, 0):
         print("LANDING OK")
         return True
@@ -250,22 +308,60 @@ def land(mavproxy, mav, timeout=60):
         print("LANDING FAILED")
         return False
 
+def circle(mavproxy, mav, maxaltchange=10, holdtime=90, timeout=35):
+    '''fly circle'''
+    print("FLY CIRCLE")
+    mavproxy.send('switch 1\n') # CIRCLE mode
+    mavproxy.expect('CIRCLE>')
+    mavproxy.send('status\n')
+    mavproxy.expect('>')
+    m = mav.recv_match(type='VFR_HUD', blocking=True)
+    start_altitude = m.alt
+    tstart = time.time()
+    tholdstart = time.time()
+    print("Circle at %u meters for %u seconds" % (start_altitude, holdtime))
+    while time.time() < tstart + timeout:
+        m = mav.recv_match(type='VFR_HUD', blocking=True)
+        print("heading %u" % m.heading)
 
-def fly_mission(mavproxy, mav, filename, height_accuracy=-1, target_altitude=None):
+    print("CIRCLE OK for %u seconds" % holdtime)
+    return True
+
+
+def fly_mission(mavproxy, mav, height_accuracy=-1, target_altitude=None):
     '''fly a mission from a file'''
+    print("Fly a mission")
     global homeloc
+    global num_wp
+    mavproxy.send('switch 4\n') # auto mode
+    mavproxy.expect('AUTO>')
+
+    wait_altitude(mav, 30, 40)
+    if wait_waypoint(mav, 1, num_wp):
+        print("MISSION COMPLETE")
+        return True
+    else:
+        return False
+
+    #if not wait_distance(mav, 30, timeout=120):
+    #    return False
+    #if not wait_location(mav, homeloc, timeout=600, target_altitude=target_altitude, height_accuracy=height_accuracy):
+    #    return False
+
+def load_mission(mavproxy, mav, filename):
+    '''load a mission from a file'''
+    global num_wp
     mavproxy.send('wp load %s\n' % filename)
     mavproxy.expect('flight plan received')
     mavproxy.send('wp list\n')
     mavproxy.expect('Requesting [0-9]+ waypoints')
-    mavproxy.send('switch 1\n') # auto mode
-    mavproxy.expect('AUTO>')
-    if not wait_distance(mav, 30, timeout=120):
-        return False
-    if not wait_location(mav, homeloc, timeout=600, target_altitude=target_altitude, height_accuracy=height_accuracy):
-        return False
-    return True
 
+    wploader = mavwp.MAVWPLoader()
+    wploader.load(filename)
+    num_wp = wploader.count()
+    print("loaded mission")
+    for i in range(num_wp):
+        print (dir(wploader.wp(i)))
 
 def setup_rc(mavproxy):
     '''setup RC override control'''
@@ -339,6 +435,7 @@ def fly_ArduCopter(viewerip=None):
         raise
     mav.message_hooks.append(message_hook)
 
+
     failed = False
     try:
         mav.wait_heartbeat()
@@ -347,19 +444,35 @@ def fly_ArduCopter(viewerip=None):
         homeloc = current_location(mav)
         if not arm_motors(mavproxy):
             failed = True
+
         if not takeoff(mavproxy, mav):
             failed = True
+
         if not fly_square(mavproxy, mav):
             failed = True
+
         if not loiter(mavproxy, mav):
             failed = True
-        if not land(mavproxy, mav):
+
+        #Fly a circle for 60 seconds
+        if not circle(mavproxy, mav):
             failed = True
+
+        # fly the stores mission
+        if not fly_mission(mavproxy, mav,height_accuracy = 0.5, target_altitude=10):
+            failed = True
+
         #fly_mission(mavproxy, mav, os.path.join(testdir, "mission_ttt.txt"), height_accuracy=0.2)
-        if not fly_mission(mavproxy, mav, os.path.join(testdir, "mission1.txt"), height_accuracy = 0.5, target_altitude=10):
+
+        if not load_mission(mavproxy, mav, os.path.join(testdir, "mission_ttt.txt")):
             failed = True
+
+        if not fly_mission(mavproxy, mav,height_accuracy = 0.5, target_altitude=10):
+            failed = True
+
         if not land(mavproxy, mav):
             failed = True
+
         if not disarm_motors(mavproxy):
             failed = True
     except pexpect.TIMEOUT, e: