autotest: fixes for MAVLink 1.0 in autotest

ArduCopter/Makefile

@@ -21,6 +21,9 @@ heli:
 apm2:
 	make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2"
 
+apm2-mavlink10:
+	make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2 -DMAVLINK10=1"
+
 apm2hexa:
 	make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2 -DFRAME_CONFIG=HEXA_FRAME"
 

ArduPlane/Makefile

@@ -24,6 +24,10 @@ apm2:
 apm2beta:
 	make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2 -DAPM2_BETA_HARDWARE"
 
+
+apm2-mavlink10:
+	make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2 -DMAVLINK10=1"
+
 mavlink10:
 	make -f Makefile EXTRAFLAGS="-DMAVLINK10=1"
 

Tools/autotest/arducopter.py

@@ -7,9 +7,9 @@ import mavutil, mavwp, random
 # get location of scripts
 testdir=os.path.dirname(os.path.realpath(__file__))
 
-FRAME='octa'
-TARGET='sitl-octa'
-HOME=location(-35.362938,149.165085,584,270)
+FRAME='+'
+TARGET='sitl'
+HOME=mavutil.location(-35.362938,149.165085,584,270)
 
 homeloc = None
 num_wp = 0
@@ -68,13 +68,13 @@ def loiter(mavproxy, mav, holdtime=60, maxaltchange=20):
     wait_mode(mav, 'LOITER')
     m = mav.recv_match(type='VFR_HUD', blocking=True)
     start_altitude = m.alt
-    start = current_location(mav)
+    start = mav.location()
     tstart = time.time()
     tholdstart = time.time()
     print("Holding loiter at %u meters for %u seconds" % (start_altitude, holdtime))
     while time.time() < tstart + holdtime:
         m = mav.recv_match(type='VFR_HUD', blocking=True)
-        pos = current_location(mav)
+        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:
@@ -174,7 +174,7 @@ def fly_RTL(mavproxy, mav, side=60):
     tstart = time.time()
     while time.time() < tstart + 200:
         m = mav.recv_match(type='VFR_HUD', blocking=True)
-        pos = current_location(mav)
+        pos = mav.location()
         #delta = get_distance(start, pos)
         print("Alt: %u" % m.alt)
         if(m.alt <= 1):
@@ -199,7 +199,7 @@ def fly_failsafe(mavproxy, mav, side=60):
     tstart = time.time()
     while time.time() < tstart + 250:
         m = mav.recv_match(type='VFR_HUD', blocking=True)
-        pos = current_location(mav)
+        pos = mav.location()
         home_distance = get_distance(HOME, pos)
         print("Alt: %u  HomeDistance: %.0f" % (m.alt, home_distance))
         if m.alt <= 1 and home_distance < 10:
@@ -404,9 +404,8 @@ def fly_ArduCopter(viewerip=None):
     e = 'None'
     try:
         mav.wait_heartbeat()
-        mav.recv_match(type='GPS_RAW', blocking=True)
         setup_rc(mavproxy)
-        homeloc = current_location(mav)
+        homeloc = mav.location()
 
         print("# Calibrate level")
         if not calibrate_level(mavproxy, mav):
@@ -568,7 +567,7 @@ def fly_ArduCopter(viewerip=None):
 #!	        old_lon = 0
 #!
 #!	        while(1):
-#!	            pos = current_location(mav)
+#!	            pos = mav.location()
 #!	            tmp = (pos.lat *10e7) - (old_lat *10e7)
 #!	            print("tmp %d " % tmp)
 #!	            if(tmp > 0 ):

Tools/autotest/arduplane.py

@@ -287,11 +287,13 @@ def fly_ArduPlane(viewerip=None):
     failed = False
     e = 'None'
     try:
+        print("Waiting for a heartbeat with mavlink protocol %s" % mav.WIRE_PROTOCOL_VERSION)
         mav.wait_heartbeat()
+        print("Setting up RC parameters")
         setup_rc(mavproxy)
-        mav.recv_match(type='GPS_RAW', condition='MAV.flightmode!="INITIALISING" and GPS_RAW.fix_type==2 and GPS_RAW.lat != 0 and GPS_RAW.alt != 0 and VFR_HUD.alt > 10',
-                       blocking=True)
-        homeloc = current_location(mav)
+        print("Waiting for GPS fix")
+        mav.wait_gps_fix()
+        homeloc = mav.location()
         print("Home location: %s" % homeloc)
         if not takeoff(mavproxy, mav):
             print("Failed takeoff")

Tools/autotest/autotest.py

@@ -7,7 +7,7 @@ import optparse, fnmatch, time, glob, traceback
 
 sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), 'pysim'))
 sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), 'pymavlink'))
-import util, arducopter, arduplane
+import util
 
 os.environ['PYTHONUNBUFFERED'] = '1'
 
@@ -111,9 +111,18 @@ parser.add_option("--skip", type='string', default='', help='list of steps to sk
 parser.add_option("--list", action='store_true', default=False, help='list the available steps')
 parser.add_option("--viewerip", default=None, help='IP address to send MAVLink and fg packets to')
 parser.add_option("--experimental", default=False, action='store_true', help='enable experimental tests')
+parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0")
 
 opts, args = parser.parse_args()
 
+if opts.mav10 or os.getenv('MAVLINK10'):
+    os.environ['MAVLINK10'] = '1'
+    import mavlinkv10 as mavlink
+else:
+    import mavlink as mavlink
+
+import  arducopter, arduplane
+
 steps = [
     'prerequesites',
     'build1280.ArduPlane',

Tools/autotest/common.py

@@ -23,8 +23,6 @@ def idle_hook(mav):
 
 def message_hook(mav, msg):
     '''called as each mavlink msg is received'''
-#    if msg.get_type() in [ 'NAV_CONTROLLER_OUTPUT', 'GPS_RAW' ]:
-#        print(msg)
     idle_hook(mav)
 
 def expect_callback(e):
@@ -35,17 +33,6 @@ def expect_callback(e):
             continue
         util.pexpect_drain(p)
 
-class location(object):
-    '''represent a GPS coordinate'''
-    def __init__(self, lat, lng, alt=0, heading=0):
-        self.lat = lat
-        self.lng = lng
-        self.alt = alt
-        self.heading = heading
-
-    def __str__(self):
-        return "lat=%.6f,lon=%.6f,alt=%.1f" % (self.lat, self.lng, self.alt)
-
 def get_distance(loc1, loc2):
     '''get ground distance between two locations'''
     dlat 		= loc2.lat - loc1.lat
@@ -61,15 +48,6 @@ def get_bearing(loc1, loc2):
         bearing += 360.00
     return bearing;
 
-def current_location(mav):
-    '''return current location'''
-    # ensure we have a position
-    mav.recv_match(type='VFR_HUD', blocking=True)
-    mav.recv_match(type='GPS_RAW', blocking=True)
-    return location(mav.messages['GPS_RAW'].lat,
-                    mav.messages['GPS_RAW'].lon,
-                    mav.messages['VFR_HUD'].alt)
-
 def wait_altitude(mav, alt_min, alt_max, timeout=30):
     climb_rate = 0
     previous_alt = 0
@@ -148,10 +126,9 @@ def wait_heading(mav, heading, accuracy=5, timeout=30):
 def wait_distance(mav, distance, accuracy=5, timeout=30):
     '''wait for flight of a given distance'''
     tstart = time.time()
-    start = current_location(mav)
+    start = mav.location()
     while time.time() < tstart + timeout:
-        m = mav.recv_match(type='GPS_RAW', blocking=True)
-        pos = current_location(mav)
+        pos = mav.location()
         delta = get_distance(start, pos)
         print("Distance %.2f meters" % delta)
         if math.fabs(delta - distance) <= accuracy:
@@ -172,8 +149,7 @@ def wait_location(mav, loc, accuracy=5, timeout=30, target_altitude=None, height
     print("Waiting for location %.4f,%.4f at altitude %.1f height_accuracy=%.1f" % (
         loc.lat, loc.lng, target_altitude, height_accuracy))
     while time.time() < tstart + timeout:
-        m = mav.recv_match(type='GPS_RAW', blocking=True)
-        pos = current_location(mav)
+        pos = mav.location()
         delta = get_distance(loc, pos)
         print("Distance %.2f meters alt %.1f" % (delta, pos.alt))
         if delta <= accuracy:
@@ -188,9 +164,7 @@ def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, max_dist=2, time
     '''wait for waypoint ranges'''
     tstart = time.time()
     # this message arrives after we set the current WP
-    m = mav.recv_match(type='WAYPOINT_CURRENT', blocking=True)
-
-    start_wp = m.seq
+    start_wp = mav.waypoint_current()
     current_wp = start_wp
 
     print("\ntest: wait for waypoint ranges start=%u end=%u\n\n" % (wpnum_start, wpnum_end))
@@ -199,8 +173,7 @@ def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, max_dist=2, time
     #    return False
 
     while time.time() < tstart + timeout:
-        m = mav.recv_match(type='WAYPOINT_CURRENT', blocking=True)
-        seq = m.seq
+        seq = mav.waypoint_current()
         m = mav.recv_match(type='NAV_CONTROLLER_OUTPUT', blocking=True)
         wp_dist = m.wp_dist
         m = mav.recv_match(type='VFR_HUD', blocking=True)

Tools/autotest/jsbsim/runsim.py

@@ -5,6 +5,7 @@ import sys, os, pexpect, fdpexpect, socket
 import math, time, select, struct, signal, errno
 
 sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..', 'pysim'))
+sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..', 'pymavlink'))
 
 import util, fgFDM, atexit
 
@@ -260,7 +261,7 @@ def main_loop():
             update_wind(wind)
             last_wind_update = tnow
 
-        if tnow - last_report > 0.5:
+        if tnow - last_report > 3:
             print("FPS %u asl=%.1f agl=%.1f roll=%.1f pitch=%.1f a=(%.2f %.2f %.2f)" % (
                 frame_count / (time.time() - last_report),
                 fdm.get('altitude', units='meters'),

Tools/autotest/pymavlink/mavutil.py

@@ -10,7 +10,7 @@ import socket, math, struct, time, os, fnmatch, array, sys, errno
 from math import *
 from mavextra import *
 
-if os.getenv('MAVLINK10'):
+if os.getenv('MAVLINK10') or 'MAVLINK10' in os.environ:
     import mavlinkv10 as mavlink
 else:
     import mavlink
@@ -34,6 +34,17 @@ def evaluate_condition(condition, vars):
 
 mavfile_global = None
 
+class location(object):
+    '''represent a GPS coordinate'''
+    def __init__(self, lat, lng, alt=0, heading=0):
+        self.lat = lat
+        self.lng = lng
+        self.alt = alt
+        self.heading = heading
+
+    def __str__(self):
+        return "lat=%.6f,lon=%.6f,alt=%.1f" % (self.lat, self.lng, self.alt)
+
 class mavfile(object):
     '''a generic mavlink port'''
     def __init__(self, fd, address, source_system=255, notimestamps=False):
@@ -223,6 +234,14 @@ class mavfile(object):
         else:
             self.mav.waypoint_set_current_send(self.target_system, self.target_component, seq)
 
+    def waypoint_current(self):
+        '''return current waypoint'''
+        if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
+            m = self.recv_match(type='MISSION_CURRENT', blocking=True)
+        else:
+            m = self.recv_match(type='WAYPOINT_CURRENT', blocking=True)
+        return m.seq
+
     def waypoint_count_send(self, seq):
         '''wrapper for waypoint_count_send'''
         if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
@@ -277,6 +296,30 @@ class mavfile(object):
             MAV_ACTION_CALIBRATE_ACC = 19
             self.mav.action_send(self.target_system, self.target_component, MAV_ACTION_CALIBRATE_ACC)
 
+    def wait_gps_fix(self):
+        self.recv_match(type='VFR_HUD', blocking=True)
+        if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
+            self.recv_match(type='GPS_RAW_INT', blocking=True,
+                            condition='GPS_RAW_INT.fix_type==3 and GPS_RAW_INT.lat != 0 and GPS_RAW_INT.alt != 0')
+        else:
+            self.recv_match(type='GPS_RAW', blocking=True,
+                            condition='GPS_RAW.fix_type==2 and GPS_RAW.lat != 0 and GPS_RAW.alt != 0')
+
+    def location(self):
+        '''return current location'''
+        self.wait_gps_fix()
+        if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
+            return location(self.messages['GPS_RAW_INT'].lat*1.0e-7,
+                            self.messages['GPS_RAW_INT'].lon*1.0e-7,
+                            self.messages['VFR_HUD'].alt,
+                            self.messages['VFR_HUD'].heading)
+        else:
+            return location(self.messages['GPS_RAW'].lat,
+                            self.messages['GPS_RAW'].lon,
+                            self.messages['VFR_HUD'].alt,
+                            self.messages['VFR_HUD'].heading)
+
+
 class mavserial(mavfile):
     '''a serial mavlink port'''
     def __init__(self, device, baud=115200, autoreconnect=False, source_system=255):

Tools/autotest/pysim/sim_multicopter.py

@@ -3,7 +3,10 @@
 from multicopter import MultiCopter
 import util, time, os, sys, math
 import socket, struct
-import select, fgFDM, errno
+import select, errno
+
+sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..', 'pymavlink'))
+import fgFDM
 
 def sim_send(m, a):
     '''send flight information to mavproxy and flightgear'''

Tools/autotest/pysim/util.py

@@ -59,6 +59,8 @@ def deltree(path):
 
 def build_SIL(atype, target='sitl'):
     '''build desktop SIL'''
+    if os.getenv('MAVLINK10'):
+        target += '-mavlink10'
     run_cmd("make clean %s" % target,
             dir=reltopdir(atype),
             checkfail=True)