From 67afdc7485c653e29fb50d09c05aca53a220dbfd Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 27 Apr 2012 13:32:22 +1000 Subject: [PATCH] autotest: fixes for MAVLink 1.0 in autotest --- ArduCopter/Makefile | 3 ++ ArduPlane/Makefile | 4 +++ Tools/autotest/arducopter.py | 19 +++++------ Tools/autotest/arduplane.py | 8 +++-- Tools/autotest/autotest.py | 11 +++++- Tools/autotest/common.py | 37 +++----------------- Tools/autotest/jsbsim/runsim.py | 3 +- Tools/autotest/pymavlink/mavutil.py | 45 ++++++++++++++++++++++++- Tools/autotest/pysim/sim_multicopter.py | 5 ++- Tools/autotest/pysim/util.py | 2 ++ 10 files changed, 88 insertions(+), 49 deletions(-) diff --git a/ArduCopter/Makefile b/ArduCopter/Makefile index 2beb16563a..5f5d5937c1 100644 --- a/ArduCopter/Makefile +++ b/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" diff --git a/ArduPlane/Makefile b/ArduPlane/Makefile index d8ed558b99..8205e702f7 100644 --- a/ArduPlane/Makefile +++ b/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" diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 80de97e541..b6ba3db47b 100644 --- a/Tools/autotest/arducopter.py +++ b/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 ): diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index fefd770cc8..6e4626c55a 100644 --- a/Tools/autotest/arduplane.py +++ b/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") diff --git a/Tools/autotest/autotest.py b/Tools/autotest/autotest.py index 16543603d2..7daf143b95 100755 --- a/Tools/autotest/autotest.py +++ b/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', diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 61fe2e669c..7668a3cef5 100644 --- a/Tools/autotest/common.py +++ b/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) diff --git a/Tools/autotest/jsbsim/runsim.py b/Tools/autotest/jsbsim/runsim.py index 528a811bf5..2ca40a1255 100755 --- a/Tools/autotest/jsbsim/runsim.py +++ b/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'), diff --git a/Tools/autotest/pymavlink/mavutil.py b/Tools/autotest/pymavlink/mavutil.py index d1ee4e7121..12a1d91f02 100644 --- a/Tools/autotest/pymavlink/mavutil.py +++ b/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): diff --git a/Tools/autotest/pysim/sim_multicopter.py b/Tools/autotest/pysim/sim_multicopter.py index 95e190264e..a4eb1fc342 100755 --- a/Tools/autotest/pysim/sim_multicopter.py +++ b/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''' diff --git a/Tools/autotest/pysim/util.py b/Tools/autotest/pysim/util.py index 3a058499c9..11d44eac55 100644 --- a/Tools/autotest/pysim/util.py +++ b/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)