Adds vehicle_goto test.

tests/sitl/__main__.py

@@ -100,20 +100,36 @@ def lets_run_a_test(name):
     sys.stdout.flush()
     sys.stderr.flush()
 
-    timeout = 300
+    # APPVEYOR = SLOW
+    timeout = 15*60 if sys.platform == 'win32' else 5*60
     try:
-        p = Popen([sys.executable, '-m', 'MAVProxy.mavproxy', '--logfile=' + tempfile.mkstemp()[1], '--master=tcp:127.0.0.1:5760'], cwd=testpath, env=newenv, stdin=PIPE, stdout=PIPE, stderr=PIPE)
+        p = Popen([sys.executable, '-m', 'MAVProxy.mavproxy', '--logfile=' + tempfile.mkstemp()[1], '--master=tcp:127.0.0.1:5760'], cwd=testpath, env=newenv, stdin=PIPE, stdout=PIPE)#, stderr=PIPE)
         bg.append(p)
 
+        while p.poll() == None:
+            line = p.stdout.readline()
+            sys.stdout.write(line)
+            sys.stdout.flush()
+            if 'parameters' in line:
+                break
+
         # TODO this sleep is only for us to waiting until
         # all parameters to be received; would prefer to 
         # move this to testlib.py and happen asap
-        time.sleep(20)
+        time.sleep(3)
         p.stdin.write('module load droneapi.module.api\n')
         p.stdin.write('param set ARMING_CHECK 0\n')
         p.stdin.write('api start testlib.py\n')
+        p.stdin.flush()
+
+        while True:
+            nextline = p.stdout.readline()
+            if nextline == '' and p.poll() != None:
+                break
+            sys.stdout.write(nextline)
+            sys.stdout.flush()
 
-        wait_timeout(p, timeout)
+        # wait_timeout(p, timeout)
     except RuntimeError:
         kill(p.pid)
         p.returncode = 143

tests/sitl/test_goto.py

@@ -0,0 +1,81 @@
+"""
+simple_goto.py: GUIDED mode "simple goto" example (Copter Only)
+
+The example demonstrates how to arm and takeoff in Copter and how to navigate to 
+points using Vehicle.commands.goto.
+
+Full documentation is provided at http://python.dronekit.io/examples/simple_goto.html
+"""
+
+import time
+from droneapi.lib import VehicleMode, Location
+from pymavlink import mavutil
+from testlib import assert_equals
+
+def test_goto(local_connect):
+    api = local_connect()
+    vehicle = api.get_vehicles()[0]
+
+    def arm_and_takeoff(aTargetAltitude):
+        """
+        Arms vehicle and fly to aTargetAltitude.
+        """
+
+        print "Basic pre-arm checks"
+        # Don't let the user try to fly autopilot is booting
+        if vehicle.mode.name == "INITIALISING":
+            print "Waiting for vehicle to initialise"
+            time.sleep(1)
+        while vehicle.gps_0.fix_type < 2:
+            print "Waiting for GPS...:", vehicle.gps_0.fix_type
+            time.sleep(1)
+
+        print "Arming motors"
+        # Copter should arm in GUIDED mode
+        vehicle.mode    = VehicleMode("GUIDED")
+        vehicle.armed   = True
+        vehicle.flush()
+
+        while not vehicle.armed and not api.exit and vehicle.mode.name != 'LAND':
+            print " Waiting for arming..."
+            time.sleep(1)
+
+        # Failure will result in arming but immediately landing
+        assert_equals(vehicle.mode.name, 'GUIDED')
+
+        print "Taking off!"
+        vehicle.commands.takeoff(aTargetAltitude) # Take off to target altitude
+        vehicle.flush()
+
+        # Wait until the vehicle reaches a safe height before processing the goto (otherwise the command 
+        #  after Vehicle.commands.takeoff will execute immediately).
+        while not api.exit:
+            print " Altitude: ", vehicle.location.alt
+            if vehicle.location.alt>=aTargetAltitude*0.95: #Just below target, in case of undershoot.
+                print "Reached target altitude"
+                break;
+
+            assert_equals(vehicle.mode.name, 'GUIDED')
+            time.sleep(1)
+
+    arm_and_takeoff(10)
+
+    print "Going to first point..."
+    point1 = Location(-35.361354, 149.165218, 20, is_relative=True)
+    vehicle.commands.goto(point1)
+    vehicle.flush()
+
+    # sleep so we can see the change in map
+    time.sleep(3)
+
+    print "Going to second point..."
+    point2 = Location(-35.363244, 149.168801, 20, is_relative=True)
+    vehicle.commands.goto(point2)
+    vehicle.flush()
+
+    # sleep so we can see the change in map
+    time.sleep(3)
+
+    print "Returning to Launch"
+    vehicle.mode    = VehicleMode("RTL")
+    vehicle.flush()