#!/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