ardupilot/Tools/autotest/arduplane.py

595 lines
20 KiB
Python
Raw Normal View History

#!/usr/bin/env python
# Fly ArduPlane in SITL
2016-11-08 07:06:05 -04:00
from __future__ import print_function
import math
import os
import pexpect
2013-06-18 03:27:21 -03:00
from pymavlink import mavutil
from pysim import util
from common import AutoTest
# get location of scripts
testdir = os.path.dirname(os.path.realpath(__file__))
HOME = mavutil.location(-35.362938, 149.165085, 585, 354)
WIND = "0,180,0.2" # speed,direction,variance
class AutoTestPlane(AutoTest):
def __init__(self,
binary,
valgrind=False,
gdb=False,
speedup=10,
frame=None,
params=None,
gdbserver=False,
**kwargs):
super(AutoTestPlane, self).__init__(**kwargs)
self.binary = binary
self.valgrind = valgrind
self.gdb = gdb
self.frame = frame
self.params = params
self.gdbserver = gdbserver
self.home = "%f,%f,%u,%u" % (HOME.lat,
HOME.lng,
HOME.alt,
HOME.heading)
self.homeloc = None
self.speedup = speedup
self.speedup_default = 10
self.sitl = None
self.hasInit = False
self.log_name = "ArduPlane"
def init(self):
if self.frame is None:
self.frame = 'plane-elevrev'
defaults_file = os.path.join(testdir,
'default_params/plane-jsbsim.parm')
self.sitl = util.start_SITL(self.binary,
wipe=True,
model=self.frame,
home=self.home,
speedup=self.speedup,
defaults_file=defaults_file,
valgrind=self.valgrind,
gdb=self.gdb,
gdbserver=self.gdbserver)
self.mavproxy = util.start_MAVProxy_SITL(
'ArduPlane', options=self.mavproxy_options())
self.mavproxy.expect('Telemetry log: (\S+)\r\n')
logfile = self.mavproxy.match.group(1)
self.progress("LOGFILE %s" % logfile)
buildlog = self.buildlogs_path("ArduPlane-test.tlog")
self.progress("buildlog=%s" % buildlog)
if os.path.exists(buildlog):
os.unlink(buildlog)
try:
os.link(logfile, buildlog)
except Exception:
pass
self.mavproxy.expect('Received [0-9]+ parameters')
util.expect_setup_callback(self.mavproxy, self.expect_callback)
self.expect_list_clear()
self.expect_list_extend([self.sitl, self.mavproxy])
self.progress("Started simulator")
# get a mavlink connection going
connection_string = '127.0.0.1:19550'
try:
self.mav = mavutil.mavlink_connection(connection_string,
robust_parsing=True)
except Exception as msg:
self.progress("Failed to start mavlink connection on %s: %s"
% (connection_string, msg))
raise
self.mav.message_hooks.append(self.message_hook)
self.mav.idle_hooks.append(self.idle_hook)
self.hasInit = True
self.progress("Ready to start testing!")
def takeoff(self):
"""Takeoff get to 30m altitude."""
self.wait_ready_to_arm()
self.arm_vehicle()
self.mavproxy.send('switch 4\n')
self.wait_mode('FBWA')
# some rudder to counteract the prop torque
self.set_rc(4, 1700)
# some up elevator to keep the tail down
self.set_rc(2, 1200)
# get it moving a bit first
self.set_rc(3, 1300)
self.mav.recv_match(condition='VFR_HUD.groundspeed>6', blocking=True)
# a bit faster again, straighten rudder
self.set_rc(3, 1600)
self.set_rc(4, 1500)
self.mav.recv_match(condition='VFR_HUD.groundspeed>12', blocking=True)
# hit the gas harder now, and give it some more elevator
self.set_rc(2, 1100)
self.set_rc(3, 2000)
# gain a bit of altitude
if not self.wait_altitude(self.homeloc.alt+150,
self.homeloc.alt+180,
timeout=30):
return False
# level off
self.set_rc(2, 1500)
self.progress("TAKEOFF COMPLETE")
return True
def fly_left_circuit(self):
"""Fly a left circuit, 200m on a side."""
self.mavproxy.send('switch 4\n')
self.wait_mode('FBWA')
self.set_rc(3, 2000)
if not self.wait_level_flight():
return False
self.progress("Flying left circuit")
# do 4 turns
for i in range(0, 4):
# hard left
self.progress("Starting turn %u" % i)
self.set_rc(1, 1000)
if not self.wait_heading(270 - (90*i), accuracy=10):
return False
self.set_rc(1, 1500)
self.progress("Starting leg %u" % i)
if not self.wait_distance(100, accuracy=20):
return False
self.progress("Circuit complete")
return True
def fly_RTL(self):
"""Fly to home."""
self.progress("Flying home in RTL")
self.mavproxy.send('switch 2\n')
self.wait_mode('RTL')
if not self.wait_location(self.homeloc,
accuracy=120,
target_altitude=self.homeloc.alt+100,
height_accuracy=20,
timeout=180):
return False
self.progress("RTL Complete")
return True
def fly_LOITER(self, num_circles=4):
"""Loiter where we are."""
self.progress("Testing LOITER for %u turns" % num_circles)
self.mavproxy.send('loiter\n')
self.wait_mode('LOITER')
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
initial_alt = m.alt
self.progress("Initial altitude %u\n" % initial_alt)
while num_circles > 0:
if not self.wait_heading(0, accuracy=10, timeout=60):
return False
if not self.wait_heading(180, accuracy=10, timeout=60):
return False
num_circles -= 1
self.progress("Loiter %u circles left" % num_circles)
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
final_alt = m.alt
self.progress("Final altitude %u initial %u\n" %
(final_alt, initial_alt))
self.mavproxy.send('mode FBWA\n')
self.wait_mode('FBWA')
if abs(final_alt - initial_alt) > 20:
self.progress("Failed to maintain altitude")
return False
self.progress("Completed Loiter OK")
return True
def fly_CIRCLE(self, num_circles=1):
"""Circle where we are."""
self.progress("Testing CIRCLE for %u turns" % num_circles)
self.mavproxy.send('mode CIRCLE\n')
self.wait_mode('CIRCLE')
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
initial_alt = m.alt
self.progress("Initial altitude %u\n" % initial_alt)
while num_circles > 0:
if not self.wait_heading(0, accuracy=10, timeout=60):
return False
if not self.wait_heading(180, accuracy=10, timeout=60):
return False
num_circles -= 1
self.progress("CIRCLE %u circles left" % num_circles)
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
final_alt = m.alt
self.progress("Final altitude %u initial %u\n" %
(final_alt, initial_alt))
self.mavproxy.send('mode FBWA\n')
self.wait_mode('FBWA')
if abs(final_alt - initial_alt) > 20:
self.progress("Failed to maintain altitude")
return False
self.progress("Completed CIRCLE OK")
return True
def wait_level_flight(self, accuracy=5, timeout=30):
"""Wait for level flight."""
tstart = self.get_sim_time()
self.progress("Waiting for level flight")
self.set_rc(1, 1500)
self.set_rc(2, 1500)
self.set_rc(4, 1500)
while self.get_sim_time() < tstart + timeout:
m = self.mav.recv_match(type='ATTITUDE', blocking=True)
roll = math.degrees(m.roll)
pitch = math.degrees(m.pitch)
self.progress("Roll=%.1f Pitch=%.1f" % (roll, pitch))
if math.fabs(roll) <= accuracy and math.fabs(pitch) <= accuracy:
self.progress("Attained level flight")
return True
self.progress("Failed to attain level flight")
return False
def change_altitude(self, altitude, accuracy=30):
"""Get to a given altitude."""
self.mavproxy.send('mode FBWA\n')
self.wait_mode('FBWA')
alt_error = self.mav.messages['VFR_HUD'].alt - altitude
if alt_error > 0:
self.set_rc(2, 2000)
else:
self.set_rc(2, 1000)
if not self.wait_altitude(altitude-accuracy/2, altitude+accuracy/2):
return False
self.set_rc(2, 1500)
self.progress("Reached target altitude at %u" %
self.mav.messages['VFR_HUD'].alt)
return self.wait_level_flight()
def axial_left_roll(self, count=1):
"""Fly a left axial roll."""
# full throttle!
self.set_rc(3, 2000)
if not self.change_altitude(self.homeloc.alt+300):
return False
# fly the roll in manual
self.mavproxy.send('switch 6\n')
self.wait_mode('MANUAL')
while count > 0:
self.progress("Starting roll")
self.set_rc(1, 1000)
if not self.wait_roll(-150, accuracy=90):
self.set_rc(1, 1500)
return False
if not self.wait_roll(150, accuracy=90):
self.set_rc(1, 1500)
return False
if not self.wait_roll(0, accuracy=90):
self.set_rc(1, 1500)
return False
count -= 1
# back to FBWA
self.set_rc(1, 1500)
self.mavproxy.send('switch 4\n')
self.wait_mode('FBWA')
self.set_rc(3, 1700)
return self.wait_level_flight()
def inside_loop(self, count=1):
"""Fly a inside loop."""
# full throttle!
self.set_rc(3, 2000)
if not self.change_altitude(self.homeloc.alt+300):
return False
# fly the loop in manual
self.mavproxy.send('switch 6\n')
self.wait_mode('MANUAL')
while count > 0:
self.progress("Starting loop")
self.set_rc(2, 1000)
if not self.wait_pitch(-60, accuracy=20):
return False
if not self.wait_pitch(0, accuracy=20):
return False
count -= 1
# back to FBWA
self.set_rc(2, 1500)
self.mavproxy.send('switch 4\n')
self.wait_mode('FBWA')
self.set_rc(3, 1700)
return self.wait_level_flight()
def test_stabilize(self, count=1):
"""Fly stabilize mode."""
# full throttle!
self.set_rc(3, 2000)
self.set_rc(2, 1300)
if not self.change_altitude(self.homeloc.alt+300):
return False
self.set_rc(2, 1500)
self.mavproxy.send("mode STABILIZE\n")
self.wait_mode('STABILIZE')
count = 1
while count > 0:
self.progress("Starting roll")
self.set_rc(1, 2000)
if not self.wait_roll(-150, accuracy=90):
return False
if not self.wait_roll(150, accuracy=90):
return False
if not self.wait_roll(0, accuracy=90):
return False
count -= 1
self.set_rc(1, 1500)
if not self.wait_roll(0, accuracy=5):
return False
# back to FBWA
self.mavproxy.send('mode FBWA\n')
self.wait_mode('FBWA')
self.set_rc(3, 1700)
return self.wait_level_flight()
def test_acro(self, count=1):
"""Fly ACRO mode."""
# full throttle!
self.set_rc(3, 2000)
self.set_rc(2, 1300)
if not self.change_altitude(self.homeloc.alt+300):
return False
self.set_rc(2, 1500)
self.mavproxy.send("mode ACRO\n")
self.wait_mode('ACRO')
count = 1
while count > 0:
self.progress("Starting roll")
self.set_rc(1, 1000)
if not self.wait_roll(-150, accuracy=90):
return False
if not self.wait_roll(150, accuracy=90):
return False
if not self.wait_roll(0, accuracy=90):
return False
count -= 1
self.set_rc(1, 1500)
# back to FBWA
self.mavproxy.send('mode FBWA\n')
self.wait_mode('FBWA')
self.wait_level_flight()
self.mavproxy.send("mode ACRO\n")
self.wait_mode('ACRO')
count = 2
while count > 0:
self.progress("Starting loop")
self.set_rc(2, 1000)
if not self.wait_pitch(-60, accuracy=20):
return False
if not self.wait_pitch(0, accuracy=20):
return False
count -= 1
self.set_rc(2, 1500)
# back to FBWA
self.mavproxy.send('mode FBWA\n')
self.wait_mode('FBWA')
self.set_rc(3, 1700)
return self.wait_level_flight()
def test_FBWB(self, count=1, mode='FBWB'):
"""Fly FBWB or CRUISE mode."""
self.mavproxy.send("mode %s\n" % mode)
self.wait_mode(mode)
self.set_rc(3, 1700)
self.set_rc(2, 1500)
# lock in the altitude by asking for an altitude change then releasing
self.set_rc(2, 1000)
self.wait_distance(50, accuracy=20)
self.set_rc(2, 1500)
self.wait_distance(50, accuracy=20)
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
initial_alt = m.alt
self.progress("Initial altitude %u\n" % initial_alt)
self.progress("Flying right circuit")
# do 4 turns
for i in range(0, 4):
# hard left
self.progress("Starting turn %u" % i)
self.set_rc(1, 1800)
if not self.wait_heading(0 + (90*i), accuracy=20, timeout=60):
self.set_rc(1, 1500)
return False
self.set_rc(1, 1500)
self.progress("Starting leg %u" % i)
if not self.wait_distance(100, accuracy=20):
return False
self.progress("Circuit complete")
self.progress("Flying rudder left circuit")
# do 4 turns
for i in range(0, 4):
# hard left
self.progress("Starting turn %u" % i)
self.set_rc(4, 1900)
if not self.wait_heading(360 - (90*i), accuracy=20, timeout=60):
self.set_rc(4, 1500)
return False
self.set_rc(4, 1500)
self.progress("Starting leg %u" % i)
if not self.wait_distance(100, accuracy=20):
return False
self.progress("Circuit complete")
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
final_alt = m.alt
self.progress("Final altitude %u initial %u\n" %
(final_alt, initial_alt))
# back to FBWA
self.mavproxy.send('mode FBWA\n')
self.wait_mode('FBWA')
if abs(final_alt - initial_alt) > 20:
self.progress("Failed to maintain altitude")
return False
return self.wait_level_flight()
def fly_mission(self, filename, height_accuracy=-1, target_altitude=None):
"""Fly a mission from a file."""
self.progress("Flying mission %s" % filename)
self.mavproxy.send('wp load %s\n' % filename)
self.mavproxy.expect('Flight plan received')
self.mavproxy.send('wp list\n')
self.mavproxy.expect('Requesting [0-9]+ waypoints')
self.mavproxy.send('switch 1\n') # auto mode
self.wait_mode('AUTO')
if not self.wait_waypoint(1, 7, max_dist=60):
return False
if not self.wait_groundspeed(0, 0.5, timeout=60):
return False
self.mavproxy.expect("Auto disarmed")
self.progress("Mission OK")
return True
def autotest(self):
"""Autotest ArduPlane in SITL."""
if not self.hasInit:
self.init()
failed = False
fail_list = []
e = 'None'
try:
self.progress("Waiting for a heartbeat with mavlink protocol %s"
% self.mav.WIRE_PROTOCOL_VERSION)
self.mav.wait_heartbeat()
self.progress("Setting up RC parameters")
self.set_rc_default()
self.set_rc(3, 1000)
self.set_rc(8, 1800)
self.progress("Waiting for GPS fix")
self.mav.recv_match(condition='VFR_HUD.alt>10', blocking=True)
self.mav.wait_gps_fix()
while self.mav.location().alt < 10:
self.mav.wait_gps_fix()
self.homeloc = self.mav.location()
self.progress("Home location: %s" % self.homeloc)
if not self.takeoff():
self.progress("Failed takeoff")
failed = True
fail_list.append("takeoff")
if not self.fly_left_circuit():
self.progress("Failed left circuit")
failed = True
fail_list.append("left_circuit")
if not self.axial_left_roll(1):
self.progress("Failed left roll")
failed = True
fail_list.append("left_roll")
if not self.inside_loop():
self.progress("Failed inside loop")
failed = True
fail_list.append("inside_loop")
if not self.test_stabilize():
self.progress("Failed stabilize test")
failed = True
fail_list.append("stabilize")
if not self.test_acro():
self.progress("Failed ACRO test")
failed = True
fail_list.append("acro")
if not self.test_FBWB():
self.progress("Failed FBWB test")
failed = True
fail_list.append("fbwb")
if not self.test_FBWB(mode='CRUISE'):
self.progress("Failed CRUISE test")
failed = True
fail_list.append("cruise")
if not self.fly_RTL():
self.progress("Failed RTL")
failed = True
fail_list.append("RTL")
if not self.fly_LOITER():
self.progress("Failed LOITER")
failed = True
fail_list.append("LOITER")
if not self.fly_CIRCLE():
self.progress("Failed CIRCLE")
failed = True
fail_list.append("LOITER")
if not self.fly_mission(os.path.join(testdir, "ap1.txt"),
height_accuracy=10,
target_altitude=self.homeloc.alt+100):
self.progress("Failed mission")
failed = True
fail_list.append("mission")
if not self.log_download(self.buildlogs_path("ArduPlane-log.bin")):
self.progress("Failed log download")
failed = True
fail_list.append("log_download")
except pexpect.TIMEOUT as e:
self.progress("Failed with timeout")
failed = True
fail_list.append("timeout")
self.close()
if failed:
self.progress("FAILED: %s" % e)
self.progress("Fail list: %s" % fail_list)
return False
return True