You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
535 lines
18 KiB
535 lines
18 KiB
#!/usr/bin/env python |
|
|
|
# Fly ArduPlane in SITL |
|
from __future__ import print_function |
|
import math |
|
import os |
|
|
|
import pexpect |
|
from pymavlink import mavutil |
|
|
|
from pysim import util |
|
|
|
from common import AutoTest |
|
from common import NotAchievedException |
|
|
|
# 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 |
|
self.wait_altitude(self.homeloc.alt+150, |
|
self.homeloc.alt+180, |
|
timeout=30) |
|
|
|
# level off |
|
self.set_rc(2, 1500) |
|
|
|
self.progress("TAKEOFF COMPLETE") |
|
|
|
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) |
|
self.wait_level_flight() |
|
|
|
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) |
|
self.wait_heading(270 - (90*i), accuracy=10) |
|
self.set_rc(1, 1500) |
|
self.progress("Starting leg %u" % i) |
|
self.wait_distance(100, accuracy=20) |
|
self.progress("Circuit complete") |
|
|
|
def fly_RTL(self): |
|
"""Fly to home.""" |
|
self.progress("Flying home in RTL") |
|
self.mavproxy.send('switch 2\n') |
|
self.wait_mode('RTL') |
|
self.wait_location(self.homeloc, |
|
accuracy=120, |
|
target_altitude=self.homeloc.alt+100, |
|
height_accuracy=20, |
|
timeout=180) |
|
self.progress("RTL Complete") |
|
|
|
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: |
|
self.wait_heading(0, accuracy=10, timeout=60) |
|
self.wait_heading(180, accuracy=10, timeout=60) |
|
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") |
|
raise NotAchievedException() |
|
|
|
self.progress("Completed Loiter OK") |
|
|
|
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: |
|
self.wait_heading(0, accuracy=10, timeout=60) |
|
self.wait_heading(180, accuracy=10, timeout=60) |
|
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") |
|
raise NotAchievedException() |
|
|
|
self.progress("Completed CIRCLE OK") |
|
|
|
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 |
|
self.progress("Failed to attain level flight") |
|
raise NotAchievedException() |
|
|
|
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) |
|
self.wait_altitude(altitude-accuracy/2, altitude+accuracy/2) |
|
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) |
|
self.change_altitude(self.homeloc.alt+300) |
|
|
|
# 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) |
|
try: |
|
self.wait_roll(-150, accuracy=90) |
|
self.wait_roll(150, accuracy=90) |
|
self.wait_roll(0, accuracy=90) |
|
except Exception as e: |
|
self.set_rc(1, 1500) |
|
raise e |
|
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) |
|
self.change_altitude(self.homeloc.alt+300) |
|
# 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) |
|
self.wait_pitch(-60, accuracy=20) |
|
self.wait_pitch(0, accuracy=20) |
|
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) |
|
self.change_altitude(self.homeloc.alt+300) |
|
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) |
|
self.wait_roll(-150, accuracy=90) |
|
self.wait_roll(150, accuracy=90) |
|
self.wait_roll(0, accuracy=90) |
|
count -= 1 |
|
|
|
self.set_rc(1, 1500) |
|
self.wait_roll(0, accuracy=5) |
|
|
|
# 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) |
|
self.change_altitude(self.homeloc.alt+300) |
|
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) |
|
self.wait_roll(-150, accuracy=90) |
|
self.wait_roll(150, accuracy=90) |
|
self.wait_roll(0, accuracy=90) |
|
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) |
|
self.wait_pitch(-60, accuracy=20) |
|
self.wait_pitch(0, accuracy=20) |
|
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) |
|
try: |
|
self.wait_heading(0 + (90*i), accuracy=20, timeout=60) |
|
except Exception as e: |
|
self.set_rc(1, 1500) |
|
raise e |
|
self.set_rc(1, 1500) |
|
self.progress("Starting leg %u" % i) |
|
self.wait_distance(100, accuracy=20) |
|
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) |
|
try: |
|
self.wait_heading(360 - (90*i), accuracy=20, timeout=60) |
|
except Exception as e: |
|
self.set_rc(4, 1500) |
|
raise e |
|
self.set_rc(4, 1500) |
|
self.progress("Starting leg %u" % i) |
|
self.wait_distance(100, accuracy=20) |
|
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") |
|
raise NotAchievedException() |
|
|
|
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') |
|
self.wait_waypoint(1, 7, max_dist=60) |
|
self.wait_groundspeed(0, 0.5, timeout=60) |
|
self.mavproxy.expect("Auto disarmed") |
|
self.progress("Mission OK") |
|
|
|
def autotest(self): |
|
"""Autotest ArduPlane in SITL.""" |
|
if not self.hasInit: |
|
self.init() |
|
|
|
self.fail_list = [] |
|
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) |
|
|
|
self.run_test("Takeoff", self.takeoff) |
|
|
|
self.run_test("Fly left circuit", self.fly_left_circuit) |
|
|
|
self.run_test("Left roll", lambda: self.axial_left_roll(1)) |
|
|
|
self.run_test("Inside loop", self.inside_loop) |
|
|
|
self.run_test("Stablize test", self.test_stabilize) |
|
|
|
self.run_test("ACRO test", self.test_acro) |
|
|
|
self.run_test("FBWB test", self.test_FBWB) |
|
|
|
self.run_test("CRUISE test", lambda: self.test_FBWB(mode='CRUISE')) |
|
|
|
self.run_test("RTL test", self.fly_RTL) |
|
|
|
self.run_test("LOITER test", self.fly_LOITER) |
|
|
|
self.run_test("CIRCLE test", self.fly_CIRCLE) |
|
|
|
self.run_test("Mission test", |
|
lambda: self.fly_mission( |
|
os.path.join(testdir, "ap1.txt"), |
|
height_accuracy=10, |
|
target_altitude=self.homeloc.alt+100)) |
|
|
|
self.run_test("Log download", |
|
lambda: self.log_download( |
|
self.buildlogs_path("ArduPlane-log.bin"))) |
|
|
|
except pexpect.TIMEOUT as e: |
|
self.progress("Failed with timeout") |
|
self.fail_list.append("timeout") |
|
|
|
self.close() |
|
|
|
if len(self.fail_list): |
|
self.progress("FAILED: %s" % self.fail_list) |
|
return False |
|
return True
|
|
|