aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

autotest: fixes for MAVLink 1.0 in autotest

Browse Source
This commit is contained in:
Andrew Tridgell 2012-04-27 13:32:22 +10:00
parent 766755aa9c
commit 8134c9a883
10 changed files with 88 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduCopter/Makefile
View File

@ -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"

4
ArduPlane/Makefile
View File

@ -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"

19
Tools/autotest/arducopter.py
View File

@ -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 ):

8
Tools/autotest/arduplane.py
View File

@ -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")

11
Tools/autotest/autotest.py
View File

@ -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',

37
Tools/autotest/common.py
View File

@ -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)

3
Tools/autotest/jsbsim/runsim.py
View File

@ -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'),

45
Tools/autotest/pymavlink/mavutil.py
View File

@ -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):

5
Tools/autotest/pysim/sim_multicopter.py
View File

@ -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'''

2
Tools/autotest/pysim/util.py
View File

@ -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)