From 259dda810d8b8e7fbe3de7e28bb460c2a50ed97b Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Mon, 5 Mar 2018 16:14:34 +0100 Subject: [PATCH] Tools: use base class to share code between vehicle tests Autotest: common.py add new base class and test Autotest : Common add heartbeat handling on wait_mode timeout Autotest: common.py add reach_heading_manual and reach_distance_manual Autotest: new autotest implementation for Rover Autotest : rover add drive_square test Autotest: new autotest implementation for Copter Autotest: new autotest implementation for Plane Autotest: new autotest implementation for Sub Autotest: new autotest implementation Autotest: new autotest implementation for QuadPlane Autotest : Sub disable GCS_Failsafe on autotest to prevent timeout. The failsafe aren't currently tested on Sub --- Tools/autotest/apmrover2.py | 851 ++++++----- Tools/autotest/arducopter.py | 2596 +++++++++++++++++----------------- Tools/autotest/arduplane.py | 1024 +++++++------- Tools/autotest/ardusub.py | 327 +++-- Tools/autotest/autotest.py | 29 +- Tools/autotest/common.py | 855 ++++++----- Tools/autotest/quadplane.py | 244 ++-- 7 files changed, 3167 insertions(+), 2759 deletions(-) diff --git a/Tools/autotest/apmrover2.py b/Tools/autotest/apmrover2.py index cda0c3160c..9576747090 100644 --- a/Tools/autotest/apmrover2.py +++ b/Tools/autotest/apmrover2.py @@ -2,363 +2,574 @@ # Drive APMrover2 in SITL from __future__ import print_function -import os + import shutil + import pexpect from common import * from pysim import util from pysim import vehicleinfo -from pymavlink import mavutil, mavwp # get location of scripts testdir = os.path.dirname(os.path.realpath(__file__)) -################################################# -# CONFIG -################################################# - # HOME = mavutil.location(-35.362938, 149.165085, 584, 270) HOME = mavutil.location(40.071374969556928, -105.22978898137808, 1583.702759, 246) -homeloc = None -num_wp = 0 -speedup_default = 10 -########################################################## -# TESTS DRIVE -########################################################## -def drive_left_circuit(mavproxy, mav): - """Drive a left circuit, 50m on a side.""" - mavproxy.send('switch 6\n') - wait_mode(mav, 'MANUAL') - set_rc(mavproxy, mav, 3, 2000) +class AutotestRover(Autotest): + def __init__(self, binary, viewerip=None, use_map=False, valgrind=False, gdb=False, speedup=10, frame=None, params=None, gdbserver=False): + super(AutotestRover, self).__init__() + self.binary = binary + self.options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=10' + self.viewerip = viewerip + self.use_map = use_map + self.valgrind = valgrind + self.gdb = gdb + self.frame = frame + self.params = params + self.gdbserver = gdbserver - progress("Driving left circuit") - # do 4 turns - for i in range(0, 4): - # hard left - progress("Starting turn %u" % i) - set_rc(mavproxy, mav, 1, 1000) - if not wait_heading(mav, 270 - (90*i), accuracy=10): + 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 + + def init(self): + if self.frame is None: + self.frame = 'rover' + + if self.viewerip: + self.options += " --out=%s:14550" % self.viewerip + if self.use_map: + self.options += ' --map' + + self.sitl = util.start_SITL(self.binary, wipe=True, model=self.frame, home=self.home, + speedup=self.speedup_default) + self.mavproxy = util.start_MAVProxy_SITL('APMrover2') + + progress("WAITING FOR PARAMETERS") + self.mavproxy.expect('Received [0-9]+ parameters') + + # setup test parameters + vinfo = vehicleinfo.VehicleInfo() + if self.params is None: + self.params = vinfo.options["APMrover2"]["frames"][self.frame]["default_params_filename"] + if not isinstance(self.params, list): + self.params = [self.params] + for x in self.params: + self.mavproxy.send("param load %s\n" % os.path.join(testdir, x)) + self.mavproxy.expect('Loaded [0-9]+ parameters') + self.set_parameter('LOG_REPLAY', 1) + self.set_parameter('LOG_DISARMED', 1) + progress("RELOADING SITL WITH NEW PARAMETERS") + + # restart with new parms + util.pexpect_close(self.mavproxy) + util.pexpect_close(self.sitl) + + self.sitl = util.start_SITL(self.binary, model=self.frame, home=self.home, speedup=self.speedup, + valgrind=self.valgrind, gdb=self.gdb, gdbserver=self.gdbserver) + self.mavproxy = util.start_MAVProxy_SITL('APMrover2', options=self.options) + self.mavproxy.expect('Telemetry log: (\S+)') + logfile = self.mavproxy.match.group(1) + progress("LOGFILE %s" % logfile) + + buildlog = util.reltopdir("../buildlogs/APMrover2-test.tlog") + 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, expect_callback) + + expect_list_clear() + expect_list_extend([self.sitl, self.mavproxy]) + + progress("Started simulator") + + # get a mavlink connection going + try: + self.mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True) + except Exception as msg: + progress("Failed to start mavlink connection on 127.0.0.1:19550" % msg) + raise + self.mav.message_hooks.append(message_hook) + self.mav.idle_hooks.append(idle_hook) + self.hasInit = True + progress("Ready to start testing!") + + def close(self): + if self.use_map: + self.mavproxy.send("module unload map\n") + self.mavproxy.expect("Unloaded module map") + + self.mav.close() + util.pexpect_close(self.mavproxy) + util.pexpect_close(self.sitl) + + valgrind_log = util.valgrind_log_filepath(binary=self.binary, model=self.frame) + if os.path.exists(valgrind_log): + os.chmod(valgrind_log, 0o644) + shutil.copy(valgrind_log, util.reltopdir("../buildlogs/APMrover2-valgrind.log")) + + # def reset_and_arm(self): + # """Reset RC, set to MANUAL and arm.""" + # self.mav.wait_heartbeat() + # # ensure all sticks in the middle + # self.set_rc_default() + # self.mavproxy.send('switch 1\n') + # self.mav.wait_heartbeat() + # self.disarm_vehicle() + # self.mav.wait_heartbeat() + # self.arm_vehicle() + # + # # TEST ARM RADIO + # def test_arm_motors_radio(self): + # """Test Arming motors with radio.""" + # progress("Test arming motors with radio") + # self.mavproxy.send('switch 6\n') # stabilize/manual mode + # self.wait_mode('MANUAL') + # self.mavproxy.send('rc 3 1500\n') # throttle at zero + # self.mavproxy.send('rc 1 2000\n') # steer full right + # self.mavproxy.expect('APM: Throttle armed') + # self.mavproxy.send('rc 1 1500\n') + # + # self.mav.motors_armed_wait() + # progress("MOTORS ARMED OK") + # return True + # + # # TEST DISARM RADIO + # def test_disarm_motors_radio(self): + # """Test Disarm motors with radio.""" + # progress("Test disarming motors with radio") + # self.mavproxy.send('switch 6\n') # stabilize/manual mode + # self.wait_mode('MANUAL') + # self.mavproxy.send('rc 3 1500\n') # throttle at zero + # self.mavproxy.send('rc 1 1000\n') # steer full right + # tstart = self.get_sim_time() + # self.mav.wait_heartbeat() + # timeout = 15 + # while self.get_sim_time() < tstart + timeout: + # self.mav.wait_heartbeat() + # if not self.mav.motors_armed(): + # disarm_delay = self.get_sim_time() - tstart + # progress("MOTORS DISARMED OK WITH RADIO") + # self.mavproxy.send('rc 1 1500\n') # steer full right + # self.mavproxy.send('rc 4 1500\n') # yaw full right + # progress("Disarm in %ss" % disarm_delay) + # return True + # progress("FAILED TO DISARM WITH RADIO") + # return False + # + # # TEST AUTO DISARM + # def test_autodisarm_motors(self): + # """Test Autodisarm motors.""" + # progress("Test Autodisarming motors") + # self.mavproxy.send('switch 6\n') # stabilize/manual mode + # # NOT IMPLEMENTED ON ROVER + # progress("MOTORS AUTODISARMED OK") + # return True + # + # # TEST RC OVERRIDE + # # TEST RC OVERRIDE TIMEOUT + # def test_rtl(self, home, distance_min=5, timeout=250): + # """Return, land.""" + # super(AutotestRover, self).test_rtl(home, distance_min, timeout) + # + # def test_mission(self, filename): + # """Test a mission from a file.""" + # progress("Test mission %s" % filename) + # num_wp = self.load_mission_from_file(filename) + # self.mavproxy.send('wp set 1\n') + # self.mav.wait_heartbeat() + # self.mavproxy.send('switch 4\n') # auto mode + # self.wait_mode('AUTO') + # ret = self.wait_waypoint(0, num_wp-1, max_dist=5, timeout=500) + # + # if ret: + # self.mavproxy.expect("Mission Complete") + # self.mav.wait_heartbeat() + # self.wait_mode('HOLD') + # progress("test: MISSION COMPLETE: passed=%s" % ret) + # return ret + + ########################################################## + # TESTS DRIVE + ########################################################## + # Drive a square in manual mode + def drive_square(self, side=50): + """Drive a square, Driving N then E .""" + progress("TEST SQUARE") + success = True + + # use LEARNING Mode + self.mavproxy.send('switch 5\n') + self.wait_mode('MANUAL') + + # first aim north + progress("\nTurn right towards north") + if not self.reach_heading_manual(10): + success = False + + # save bottom left corner of box as waypoint + progress("Save WP 1 & 2") + self.save_wp() + + # pitch forward to fly north + progress("\nGoing north %u meters" % side) + if not self.reach_distance_manual(side): + success = False + + # save top left corner of square as waypoint + progress("Save WP 3") + self.save_wp() + + # roll right to fly east + progress("\nGoing east %u meters" % side) + if not self.reach_heading_manual(100): + success = False + if not self.reach_distance_manual(side): + success = False + + # save top right corner of square as waypoint + progress("Save WP 4") + self.save_wp() + + # pitch back to fly south + progress("\nGoing south %u meters" % side) + if not self.reach_heading_manual(190): + success = False + if not self.reach_distance_manual(side): + success = False + + # save bottom right corner of square as waypoint + progress("Save WP 5") + self.save_wp() + + # roll left to fly west + progress("\nGoing west %u meters" % side) + if not self.reach_heading_manual(280): + success = False + if not self.reach_distance_manual(side): + success = False + + # save bottom left corner of square (should be near home) as waypoint + progress("Save WP 6") + self.save_wp() + + return success + + def drive_left_circuit(self): + """Drive a left circuit, 50m on a side.""" + self.mavproxy.send('switch 6\n') + self.wait_mode('MANUAL') + self.set_rc(3, 2000) + + progress("Driving left circuit") + # do 4 turns + for i in range(0, 4): + # hard left + 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) + progress("Starting leg %u" % i) + if not self.wait_distance(50, accuracy=7): + return False + self.set_rc(3, 1500) + progress("Circuit complete") + return True + + # def test_throttle_failsafe(self, home, distance_min=10, side=60, timeout=300): + # """Fly east, Failsafe, return, land.""" + # + # self.mavproxy.send('switch 6\n') # manual mode + # self.wait_mode('MANUAL') + # self.mavproxy.send("param set FS_ACTION 1\n") + # + # # first aim east + # progress("turn east") + # if not self.reach_heading_manual(135): + # return False + # + # # fly east 60 meters + # progress("# Going forward %u meters" % side) + # if not self.reach_distance_manual(side): + # return False + # + # # pull throttle low + # progress("# Enter Failsafe") + # self.mavproxy.send('rc 3 900\n') + # + # tstart = self.get_sim_time() + # success = False + # while self.get_sim_time() < tstart + timeout and not success: + # m = self.mav.recv_match(type='VFR_HUD', blocking=True) + # pos = self.mav.location() + # home_distance = self.get_distance(home, pos) + # progress("Alt: %u HomeDistance: %.0f" % (m.alt, home_distance)) + # # check if we've reached home + # if home_distance <= distance_min: + # progress("RTL Complete") + # success = True + # + # # reduce throttle + # self.mavproxy.send('rc 3 1500\n') + # self.mavproxy.expect('APM: Failsafe ended') + # self.mavproxy.send('switch 2\n') # manual mode + # self.mav.wait_heartbeat() + # self.wait_mode('MANUAL') + # + # if success: + # progress("Reached failsafe home OK") + # return True + # else: + # progress("Failed to reach Home on failsafe RTL - timed out after %u seconds" % timeout) + # return False + + ################################################# + # AUTOTEST ALL + ################################################# + def drive_mission(self, filename): + """Drive a mission from a file.""" + progress("Driving 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 4\n') # auto mode + self.set_rc(3, 1500) + self.wait_mode('AUTO') + if not self.wait_waypoint(1, 4, max_dist=5): return False - set_rc(mavproxy, mav, 1, 1500) - progress("Starting leg %u" % i) - if not wait_distance(mav, 50, accuracy=7): + self.wait_mode('HOLD') + progress("Mission OK") + return True + + def do_get_banner(self): + self.mavproxy.send("long DO_SEND_BANNER 1\n") + start = time.time() + while True: + m = self.mav.recv_match(type='STATUSTEXT', blocking=True, timeout=1) + if m is not None and "ArduRover" in m.text: + progress("banner received: %s" % m.text) + return True + if time.time() - start > 10: + break + + progress("banner not received") + + return False + + def drive_brake_get_stopping_distance(self, speed): + # measure our stopping distance: + old_cruise_speed = self.get_parameter('CRUISE_SPEED') + old_accel_max = self.get_parameter('ATC_ACCEL_MAX') + + # controller tends not to meet cruise speed (max of ~14 when 15 + # set), thus *1.2 + self.set_parameter('CRUISE_SPEED', speed*1.2) + # at time of writing, the vehicle is only capable of 10m/s/s accel + self.set_parameter('ATC_ACCEL_MAX', 15) + self.mavproxy.send("mode STEERING\n") + self.wait_mode('STEERING') + self.set_rc(3, 2000) + self.wait_groundspeed(15, 100) + initial = self.mav.location() + initial_time = time.time() + while time.time() - initial_time < 2: + # wait for a position update from the autopilot + start = self.mav.location() + if start != initial: + break + self.set_rc(3, 1500) + self.wait_groundspeed(0, 0.2) # why do we not stop?! + initial = self.mav.location() + initial_time = time.time() + while time.time() - initial_time < 2: + # wait for a position update from the autopilot + stop = self.mav.location() + if stop != initial: + break + delta = self.get_distance(start, stop) + + self.set_parameter('CRUISE_SPEED', old_cruise_speed) + self.set_parameter('ATC_ACCEL_MAX', old_accel_max) + + return delta + + def drive_brake(self): + old_using_brake = self.get_parameter('ATC_BRAKE') + old_cruise_speed = self.get_parameter('CRUISE_SPEED') + + self.set_parameter('CRUISE_SPEED', 15) + self.set_parameter('ATC_BRAKE', 0) + + distance_without_brakes = self.drive_brake_get_stopping_distance(15) + + # brakes on: + self.set_parameter('ATC_BRAKE', 1) + distance_with_brakes = self.drive_brake_get_stopping_distance(15) + # revert state: + self.set_parameter('ATC_BRAKE', old_using_brake) + self.set_parameter('CRUISE_SPEED', old_cruise_speed) + + delta = distance_without_brakes - distance_with_brakes + if delta < distance_without_brakes * 0.05: # 5% isn't asking for much + progress("Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)" % (distance_with_brakes, distance_without_brakes, delta)) return False - set_rc(mavproxy, mav, 3, 1500) - progress("Circuit complete") - return True + else: + progress("Brakes work (with=%0.2fm without=%0.2fm delta=%0.2fm)" % (distance_with_brakes, distance_without_brakes, delta)) + return True -def drive_RTL(mavproxy, mav): - """Drive to home.""" - progress("Driving home in RTL") - mavproxy.send('switch 3\n') - if not wait_location(mav, homeloc, accuracy=22, timeout=90): - return False - progress("RTL Complete") - return True + def drive_rtl_mission(self): + mission_filepath = os.path.join(testdir, "ArduRover-Missions", "rtl.txt") + self.mavproxy.send('wp load %s\n' % mission_filepath) + self.mavproxy.expect('Flight plan received') + self.mavproxy.send('switch 4\n') # auto mode + self.set_rc(3, 1500) + self.wait_mode('AUTO') + self.mavproxy.expect('Executing RTL') + m = self.mav.recv_match(type='NAV_CONTROLLER_OUTPUT', + blocking=True, + timeout=0.1) + if m is None: + progress("Did not receive NAV_CONTROLLER_OUTPUT message") + return False -################################################# -# AUTOTEST ALL -################################################# -def drive_mission(mavproxy, mav, filename): - """Drive a mission from a file.""" - global homeloc - progress("Driving mission %s" % filename) - mavproxy.send('wp load %s\n' % filename) - mavproxy.expect('Flight plan received') - mavproxy.send('wp list\n') - mavproxy.expect('Requesting [0-9]+ waypoints') - mavproxy.send('switch 4\n') # auto mode - set_rc(mavproxy, mav, 3, 1500) - wait_mode(mav, 'AUTO') - if not wait_waypoint(mav, 1, 4, max_dist=5): - return False - wait_mode(mav, 'HOLD') - progress("Mission OK") - return True + wp_dist_min = 5 + if m.wp_dist < wp_dist_min: + progress("Did not start at least 5 metres from destination") + return False + progress("NAV_CONTROLLER_OUTPUT.wp_dist looks good (%u >= %u)" % + (m.wp_dist, wp_dist_min,)) -def do_get_banner(mavproxy, mav): - mavproxy.send("long DO_SEND_BANNER 1\n") - start = time.time() - while True: - m = mav.recv_match(type='STATUSTEXT', blocking=True, timeout=1) - if m is not None and "ArduRover" in m.text: - progress("banner received: %s" % (m.text)) - return True - if time.time() - start > 10: - break + self.wait_mode('HOLD') - progress("banner not received") + pos = self.mav.location() + home_distance = self.get_distance(HOME, pos) + home_distance_max = 5 + if home_distance > home_distance_max: + progress("Did not get home (%u metres distant > %u)" % + (home_distance, home_distance_max)) + return False + self.mavproxy.send('switch 6\n') + self.wait_mode('MANUAL') + progress("RTL Mission OK") + return True - return False + def autotest(self): + """Autotest APMrover2 in SITL.""" + if not self.hasInit: + self.init() + progress("Started simulator") + failed = False + e = 'None' + try: + progress("Waiting for a heartbeat with mavlink protocol %s" % self.mav.WIRE_PROTOCOL_VERSION) + self.mav.wait_heartbeat() + progress("Setting up RC parameters") + self.set_rc_default() + self.set_rc(8, 1800) + progress("Waiting for GPS fix") + self.mav.wait_gps_fix() + self.homeloc = self.mav.location() + progress("Home location: %s" % self.homeloc) + self.mavproxy.send('switch 6\n') # Manual mode + self.wait_mode('MANUAL') + progress("Waiting reading for arm") + self.wait_ready_to_arm() + if not self.arm_vehicle(): + progress("Failed to ARM") + failed = True -def drive_brake_get_stopping_distance(mavproxy, mav, speed): - # measure our stopping distance: - old_cruise_speed = get_parameter(mavproxy, 'CRUISE_SPEED') - old_accel_max = get_parameter(mavproxy, 'ATC_ACCEL_MAX') + progress("#") + progress("########## Drive an RTL mission ##########") + progress("#") + # Drive a square in learning mode + # self.reset_and_arm() + if not self.drive_rtl_mission(): + progress("Failed RTL mission") + failed = True - # controller tends not to meet cruise speed (max of ~14 when 15 - # set), thus *1.2 - set_parameter(mavproxy, 'CRUISE_SPEED', speed*1.2) - # at time of writing, the vehicle is only capable of 10m/s/s accel - set_parameter(mavproxy, 'ATC_ACCEL_MAX', 15) - mavproxy.send("mode STEERING\n") - wait_mode(mav, 'STEERING') - set_rc(mavproxy, mav, 3, 2000) - wait_groundspeed(mav, 15, 100) - initial = mav.location() - initial_time = time.time() - while time.time() - initial_time < 2: - # wait for a position update from the autopilot - start = mav.location() - if start != initial: - break - set_rc(mavproxy, mav, 3, 1500) - wait_groundspeed(mav, 0, 0.2) # why do we not stop?! - initial = mav.location() - initial_time = time.time() - while time.time() - initial_time < 2: - # wait for a position update from the autopilot - stop = mav.location() - if stop != initial: - break - delta = get_distance(start, stop) + progress("#") + progress("########## Drive a square and save WPs with CH7 switch ##########") + progress("#") + # Drive a square in learning mode + # self.reset_and_arm() + if not self.drive_square(): + progress("Failed drive square") + failed = True - set_parameter(mavproxy, 'CRUISE_SPEED', old_cruise_speed) - set_parameter(mavproxy, 'ATC_ACCEL_MAX', old_accel_max) + if not self.drive_mission(os.path.join(testdir, "rover1.txt")): + progress("Failed mission") + failed = True - return delta + if not self.drive_brake(): + progress("Failed brake") + failed = True + if not self.disarm_vehicle(): + progress("Failed to DISARM") + failed = True -def drive_brake(mavproxy, mav): - old_using_brake = get_parameter(mavproxy, 'ATC_BRAKE') - old_cruise_speed = get_parameter(mavproxy, 'CRUISE_SPEED') + # do not move this to be the first test. MAVProxy's dedupe + # function may bite you. + progress("Getting banner") + if not self.do_get_banner(): + progress("FAILED: get banner") + failed = True - set_parameter(mavproxy, 'CRUISE_SPEED', 15) - set_parameter(mavproxy, 'ATC_BRAKE', 0) + progress("Getting autopilot capabilities") + if not self.do_get_autopilot_capabilities(): + progress("FAILED: get capabilities") + failed = True - distance_without_brakes = drive_brake_get_stopping_distance(mavproxy, mav, 15) + progress("Setting mode via MAV_COMMAND_DO_SET_MODE") + if not self.do_set_mode_via_command_long(): + failed = True - # brakes on: - set_parameter(mavproxy, 'ATC_BRAKE', 1) - distance_with_brakes = drive_brake_get_stopping_distance(mavproxy, mav, 15) - # revert state: - set_parameter(mavproxy, 'ATC_BRAKE', old_using_brake) - set_parameter(mavproxy, 'CRUISE_SPEED', old_cruise_speed) + # Throttle Failsafe + progress("#") + progress("########## Test Failsafe ##########") + progress("#") + # self.reset_and_arm() + # if not self.test_throttle_failsafe(HOME, distance_min=4): + # progress("Throttle failsafe failed") + # sucess = False - delta = distance_without_brakes - distance_with_brakes - if delta < distance_without_brakes * 0.05: # 5% isn't asking for much - progress("Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)" % (distance_with_brakes, distance_without_brakes, delta)) - return False - else: - progress("Brakes work (with=%0.2fm without=%0.2fm delta=%0.2fm)" % (distance_with_brakes, distance_without_brakes, delta)) + if not self.log_download(util.reltopdir("../buildlogs/APMrover2-log.bin")): + progress("Failed log download") + failed = True + # if not drive_left_circuit(self): + # progress("Failed left circuit") + # failed = True + # if not drive_RTL(self): + # progress("Failed RTL") + # failed = True - return True - - -vinfo = vehicleinfo.VehicleInfo() - - -def drive_rtl_mission(mavproxy, mav): - mission_filepath = os.path.join(testdir, "ArduRover-Missions", "rtl.txt") - mavproxy.send('wp load %s\n' % mission_filepath) - mavproxy.expect('Flight plan received') - mavproxy.send('switch 4\n') # auto mode - set_rc(mavproxy, mav, 3, 1500) - wait_mode(mav, 'AUTO') - mavproxy.expect('Executing RTL') - - m = mav.recv_match(type='NAV_CONTROLLER_OUTPUT', - blocking=True, - timeout=0.1) - if m is None: - progress("Did not receive NAV_CONTROLLER_OUTPUT message") - return False - - wp_dist_min = 5 - if m.wp_dist < wp_dist_min: - progress("Did not start at least 5 metres from destination") - return False - - progress("NAV_CONTROLLER_OUTPUT.wp_dist looks good (%u >= %u)" % - (m.wp_dist, wp_dist_min,)) - - wait_mode(mav, 'HOLD') - - pos = mav.location() - home_distance = get_distance(HOME, pos) - home_distance_max = 5 - if home_distance > home_distance_max: - progress("Did not get home (%u metres distant > %u)" % - (home_distance,home_distance_max)) - return False - mavproxy.send('switch 6\n') - wait_mode(mav, 'MANUAL') - progress("RTL Mission OK") - return True - -def drive_APMrover2(binary, viewerip=None, use_map=False, valgrind=False, gdb=False, frame=None, params=None, gdbserver=False, speedup=10): - """Drive APMrover2 in SITL. - - you can pass viewerip as an IP address to optionally send fg and - mavproxy packets too for local viewing of the mission in real time - """ - global homeloc - - if frame is None: - frame = 'rover' - - options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=10' - if viewerip: - options += " --out=%s:14550" % viewerip - if use_map: - options += ' --map' - - home = "%f,%f,%u,%u" % (HOME.lat, HOME.lng, HOME.alt, HOME.heading) - sitl = util.start_SITL(binary, wipe=True, model=frame, home=home, speedup=speedup) - mavproxy = util.start_MAVProxy_SITL('APMrover2') - - progress("WAITING FOR PARAMETERS") - mavproxy.expect('Received [0-9]+ parameters') - - # setup test parameters - if params is None: - params = vinfo.options["APMrover2"]["frames"][frame]["default_params_filename"] - if not isinstance(params, list): - params = [params] - for x in params: - mavproxy.send("param load %s\n" % os.path.join(testdir, x)) - mavproxy.expect('Loaded [0-9]+ parameters') - set_parameter(mavproxy, 'LOG_REPLAY', 1) - set_parameter(mavproxy, 'LOG_DISARMED', 1) - - # restart with new parms - util.pexpect_close(mavproxy) - util.pexpect_close(sitl) - - sitl = util.start_SITL(binary, model='rover', home=home, speedup=speedup, valgrind=valgrind, gdb=gdb, gdbserver=gdbserver) - mavproxy = util.start_MAVProxy_SITL('APMrover2', options=options) - mavproxy.expect('Telemetry log: (\S+)\r\n') - logfile = mavproxy.match.group(1) - progress("LOGFILE %s" % logfile) - - buildlog = util.reltopdir("../buildlogs/APMrover2-test.tlog") - progress("buildlog=%s" % buildlog) - if os.path.exists(buildlog): - os.unlink(buildlog) - try: - os.link(logfile, buildlog) - except Exception: - pass - - mavproxy.expect('Received [0-9]+ parameters') - - util.expect_setup_callback(mavproxy, expect_callback) - - expect_list_clear() - expect_list_extend([sitl, mavproxy]) - - progress("Started simulator") - - # get a mavlink connection going - try: - mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True) - except Exception as msg: - progress("Failed to start mavlink connection on 127.0.0.1:19550" % msg) - raise - mav.message_hooks.append(message_hook) - mav.idle_hooks.append(idle_hook) - - failed = False - e = 'None' - try: - progress("Waiting for a heartbeat with mavlink protocol %s" % mav.WIRE_PROTOCOL_VERSION) - mav.wait_heartbeat() - progress("Setting up RC parameters") - set_rc_default(mavproxy) - set_rc(mavproxy, mav, 8, 1800) - progress("Waiting for GPS fix") - mav.wait_gps_fix() - homeloc = mav.location() - progress("Home location: %s" % homeloc) - mavproxy.send('switch 6\n') # Manual mode - wait_mode(mav, 'MANUAL') - progress("Waiting reading for arm") - wait_ready_to_arm(mav) - if not arm_vehicle(mavproxy, mav): - progress("Failed to ARM") + except pexpect.TIMEOUT as e: + progress("Failed with timeout") failed = True - progress("#") - progress("########## Drive an RTL mission ##########") - progress("#") - # Drive a square in learning mode - if not drive_rtl_mission(mavproxy, mav): - progress("Failed RTL mission") - failed = True + self.close() - progress("#") - progress("########## Drive a square and save WPs with CH7 switch ##########") - progress("#") - # Drive a square in learning mode - if not drive_mission(mavproxy, mav, os.path.join(testdir, "rover1.txt")): - progress("Failed mission") - failed = True - - if not drive_brake(mavproxy, mav): - progress("Failed brake") - failed = True - - if not disarm_vehicle(mavproxy, mav): - progress("Failed to DISARM") - failed = True - - # do not move this to be the first test. MAVProxy's dedupe - # function may bite you. - progress("Getting banner") - if not do_get_banner(mavproxy, mav): - progress("FAILED: get banner") - failed = True - - progress("Getting autopilot capabilities") - if not do_get_autopilot_capabilities(mavproxy, mav): - progress("FAILED: get capabilities") - failed = True - - progress("Setting mode via MAV_COMMAND_DO_SET_MODE") - if not do_set_mode_via_command_long(mavproxy, mav): - failed = True - - if not log_download(mavproxy, mav, util.reltopdir("../buildlogs/APMrover2-log.bin")): - progress("Failed log download") - failed = True -# if not drive_left_circuit(mavproxy, mav): -# progress("Failed left circuit") -# failed = True -# if not drive_RTL(mavproxy, mav): -# progress("Failed RTL") -# failed = True - - except pexpect.TIMEOUT as e: - progress("Failed with timeout") - failed = True - - mav.close() - util.pexpect_close(mavproxy) - util.pexpect_close(sitl) - - valgrind_log = util.valgrind_log_filepath(binary=binary, model='rover') - if os.path.exists(valgrind_log): - os.chmod(valgrind_log, 0o644) - shutil.copy(valgrind_log, util.reltopdir("../buildlogs/APMrover2-valgrind.log")) - - if failed: - progress("FAILED: %s" % e) - return False - return True + if failed: + progress("FAILED: %s" % e) + return False + return True diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 706de17245..057f99c869 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -14,20 +14,11 @@ from common import * from pysim import util from pysim import vehicleinfo -vinfo = vehicleinfo.VehicleInfo() - # get location of scripts testdir = os.path.dirname(os.path.realpath(__file__)) - -################################################# -# CONFIG -################################################# HOME = mavutil.location(-35.362938, 149.165085, 584, 270) AVCHOME = mavutil.location(40.072842, -105.230575, 1586, 0) -homeloc = None -num_wp = 0 - # Flight mode switch positions are set-up in arducopter.param to be # switch 1 = Circle # switch 2 = Land @@ -37,1437 +28,1382 @@ num_wp = 0 # switch 6 = Stabilize -################################################# -# UTILITIES -################################################# -def takeoff(mavproxy, mav, alt_min=30, takeoff_throttle=1700): - """Takeoff get to 30m altitude.""" - mavproxy.send('switch 6\n') # stabilize mode - wait_mode(mav, 'STABILIZE') - set_rc(mavproxy, mav, 3, takeoff_throttle) - m = mav.recv_match(type='VFR_HUD', blocking=True) - if (m.alt < alt_min): - wait_altitude(mav, alt_min, (alt_min + 5)) - hover(mavproxy, mav) - progress("TAKEOFF COMPLETE") - return True +class AutotestCopter(Autotest): + def __init__(self, binary, viewerip=None, use_map=False, valgrind=False, gdb=False, speedup=10, frame=None, params=None, gdbserver=False): + super(AutotestCopter, self).__init__() + self.binary = binary + self.options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=5' + self.viewerip = viewerip + self.use_map = use_map + 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 -def land(mavproxy, mav, timeout=60): - """Land the quad.""" - progress("STARTING LANDING") - mavproxy.send('switch 2\n') # land mode - wait_mode(mav, 'LAND') - progress("Entered Landing Mode") - ret = wait_altitude(mav, -5, 1) - progress("LANDING: ok= %s" % ret) - return ret + self.log_name = "ArduCopter" + self.logfile = None + self.buildlog = None + self.copy_tlog = False + self.sitl = None + self.hasInit = False -def hover(mavproxy, mav, hover_throttle=1500): - set_rc(mavproxy, mav, 3, hover_throttle) - return True + def init(self): + if self.frame is None: + self.frame = '+' + if self.frame == 'heli': + self.log_name = "HeliCopter" + self.home = "%f,%f,%u,%u" % (AVCHOME.lat, AVCHOME.lng, AVCHOME.alt, AVCHOME.heading) + else: + self.options += " --quadcopter" -# loiter - fly south west, then hold loiter within 5m position and altitude -def loiter(mavproxy, mav, holdtime=10, maxaltchange=5, maxdistchange=5): - """Hold loiter position.""" - mavproxy.send('switch 5\n') # loiter mode - wait_mode(mav, 'LOITER') + if self.viewerip: + self.options += " --out=%s:14550" % self.viewerip + if self.use_map: + self.options += ' --map' - # first aim south east - progress("turn south east") - set_rc(mavproxy, mav, 4, 1580) - if not wait_heading(mav, 170): - return False - set_rc(mavproxy, mav, 4, 1500) + self.sitl = util.start_SITL(self.binary, wipe=True, model=self.frame, home=self.home, + speedup=self.speedup_default) + self.mavproxy = util.start_MAVProxy_SITL('ArduCopter') - # fly south east 50m - set_rc(mavproxy, mav, 2, 1100) - if not wait_distance(mav, 50): - return False - set_rc(mavproxy, mav, 2, 1500) + progress("WAITING FOR PARAMETERS") + self.mavproxy.expect('Received [0-9]+ parameters') - # wait for copter to slow moving - if not wait_groundspeed(mav, 0, 2): - return False + # setup test parameters + vinfo = vehicleinfo.VehicleInfo() + if self.params is None: + self.params = vinfo.options["ArduCopter"]["frames"][self.frame]["default_params_filename"] + if not isinstance(self.params, list): + self.params = [self.params] + for x in self.params: + self.mavproxy.send("param load %s\n" % os.path.join(testdir, x)) + self.mavproxy.expect('Loaded [0-9]+ parameters') + self.set_parameter('LOG_REPLAY', 1) + self.set_parameter('LOG_DISARMED', 1) + progress("RELOADING SITL WITH NEW PARAMETERS") - success = True - m = mav.recv_match(type='VFR_HUD', blocking=True) - start_altitude = m.alt - start = mav.location() - tstart = get_sim_time(mav) - tholdstart = get_sim_time(mav) - progress("Holding loiter at %u meters for %u seconds" % (start_altitude, holdtime)) - while get_sim_time(mav) < tstart + holdtime: - m = mav.recv_match(type='VFR_HUD', blocking=True) - pos = mav.location() - delta = get_distance(start, pos) - alt_delta = math.fabs(m.alt - start_altitude) - progress("Loiter Dist: %.2fm, alt:%u" % (delta, m.alt)) - if alt_delta > maxaltchange: - progress("Loiter alt shifted %u meters (> limit of %u)" % (alt_delta, maxaltchange)) - success = False - if delta > maxdistchange: - progress("Loiter shifted %u meters (> limit of %u)" % (delta, maxdistchange)) - success = False - if success: - progress("Loiter OK for %u seconds" % holdtime) - else: - progress("Loiter FAILED") - return success + # restart with new parms + util.pexpect_close(self.mavproxy) + util.pexpect_close(self.sitl) + self.sitl = util.start_SITL(self.binary, model=self.frame, home=self.home, speedup=self.speedup, + valgrind=self.valgrind, gdb=self.gdb, gdbserver=self.gdbserver) + self.mavproxy = util.start_MAVProxy_SITL('ArduCopter', options=self.options) + self.mavproxy.expect('Telemetry log: (\S+)') + self.logfile = self.mavproxy.match.group(1) + progress("LOGFILE %s" % self.logfile) -def change_alt(mavproxy, mav, alt_min, climb_throttle=1920, descend_throttle=1080): - """Change altitude.""" - m = mav.recv_match(type='VFR_HUD', blocking=True) - if(m.alt < alt_min): - progress("Rise to alt:%u from %u" % (alt_min, m.alt)) - set_rc(mavproxy, mav, 3, climb_throttle) - wait_altitude(mav, alt_min, (alt_min + 5)) - else: - progress("Lower to alt:%u from %u" % (alt_min, m.alt)) - set_rc(mavproxy, mav, 3, descend_throttle) - wait_altitude(mav, (alt_min - 5), alt_min) - hover(mavproxy, mav) - return True + self.buildlog = util.reltopdir("../buildlogs/" + self.log_name + "-test.tlog") + progress("buildlog=%s" % self.buildlog) + self.copy_tlog = False + if os.path.exists(self.buildlog): + os.unlink(self.buildlog) + try: + os.link(self.logfile, self.buildlog) + except Exception: + progress("WARN: Failed to create symlink: " + self.logfile + " => " + self.buildlog + ", Will copy tlog manually to target location") + self.copy_tlog = True + self.mavproxy.expect('Received [0-9]+ parameters') -################################################# -# TESTS FLY -################################################# -# fly a square in stabilize mode -def fly_square(mavproxy, mav, side=50, timeout=300): - """Fly a square, flying N then E .""" - tstart = get_sim_time(mav) - success = True + util.expect_setup_callback(self.mavproxy, expect_callback) - # ensure all sticks in the middle - set_rc(mavproxy, mav, 1, 1500) - set_rc(mavproxy, mav, 2, 1500) - set_rc(mavproxy, mav, 3, 1500) - set_rc(mavproxy, mav, 4, 1500) + expect_list_clear() + expect_list_extend([self.sitl, self.mavproxy]) - # switch to loiter mode temporarily to stop us from rising - mavproxy.send('switch 5\n') - wait_mode(mav, 'LOITER') + progress("Started simulator") - # first aim north - progress("turn right towards north") - set_rc(mavproxy, mav, 4, 1580) - if not wait_heading(mav, 10): - progress("Failed to reach heading") - success = False - set_rc(mavproxy, mav, 4, 1500) - mav.recv_match(condition='RC_CHANNELS.chan4_raw==1500', blocking=True) + # get a mavlink connection going + try: + self.mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True) + except Exception as msg: + progress("Failed to start mavlink connection on 127.0.0.1:19550" % msg) + raise + self.mav.message_hooks.append(message_hook) + self.mav.idle_hooks.append(idle_hook) + self.hasInit = True + progress("Ready to start testing!") - # save bottom left corner of box as waypoint - progress("Save WP 1 & 2") - save_wp(mavproxy, mav) + def close(self): + if self.use_map: + self.mavproxy.send("module unload map\n") + self.mavproxy.expect("Unloaded module map") - # switch back to stabilize mode - set_rc(mavproxy, mav, 3, 1500) - mavproxy.send('switch 6\n') - wait_mode(mav, 'STABILIZE') + self.mav.close() + util.pexpect_close(self.mavproxy) + util.pexpect_close(self.sitl) - # pitch forward to fly north - progress("Going north %u meters" % side) - set_rc(mavproxy, mav, 2, 1300) - if not wait_distance(mav, side): - progress("Failed to reach distance of %u" % side) - success = False - set_rc(mavproxy, mav, 2, 1500) + valgrind_log = util.valgrind_log_filepath(binary=self.binary, model=self.frame) + if os.path.exists(valgrind_log): + os.chmod(valgrind_log, 0o644) + shutil.copy(valgrind_log, util.reltopdir("../buildlogs/" + self.log_name + "-valgrind.log")) - # save top left corner of square as waypoint - progress("Save WP 3") - save_wp(mavproxy, mav) + # [2014/05/07] FC Because I'm doing a cross machine build (source is on host, build is on guest VM) I cannot hard link + # This flag tells me that I need to copy the data out + if self.copy_tlog: + shutil.copy(self.logfile, self.buildlog) - # roll right to fly east - progress("Going east %u meters" % side) - set_rc(mavproxy, mav, 1, 1700) - if not wait_distance(mav, side): - progress("Failed to reach distance of %u" % side) - success = False - set_rc(mavproxy, mav, 1, 1500) + def takeoff(self, alt_min=30, takeoff_throttle=1700): + """Takeoff get to 30m altitude.""" + self.mavproxy.send('switch 6\n') # stabilize mode + self.wait_mode('STABILIZE') + self.set_rc(3, takeoff_throttle) + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + if m.alt < alt_min: + self.wait_altitude(alt_min, (alt_min + 5)) + self.hover() + progress("TAKEOFF COMPLETE") + return True - # save top right corner of square as waypoint - progress("Save WP 4") - save_wp(mavproxy, mav) + def land(self, timeout=60): + """Land the quad.""" + progress("STARTING LANDING") + self.mavproxy.send('switch 2\n') # land mode + self.wait_mode('LAND') + progress("Entered Landing Mode") + ret = self.wait_altitude(-5, 1) + progress("LANDING: ok= %s" % ret) + return ret - # pitch back to fly south - progress("Going south %u meters" % side) - set_rc(mavproxy, mav, 2, 1700) - if not wait_distance(mav, side): - progress("Failed to reach distance of %u" % side) - success = False - set_rc(mavproxy, mav, 2, 1500) + def hover(self, hover_throttle=1500): + self.set_rc(3, hover_throttle) + return True - # save bottom right corner of square as waypoint - progress("Save WP 5") - save_wp(mavproxy, mav) + # loiter - fly south west, then hold loiter within 5m position and altitude + def loiter(self, holdtime=10, maxaltchange=5, maxdistchange=5): + """Hold loiter position.""" + self.mavproxy.send('switch 5\n') # loiter mode + self.wait_mode('LOITER') - # roll left to fly west - progress("Going west %u meters" % side) - set_rc(mavproxy, mav, 1, 1300) - if not wait_distance(mav, side): - progress("Failed to reach distance of %u" % side) - success = False - set_rc(mavproxy, mav, 1, 1500) + # first aim south east + progress("turn south east") + self.set_rc(4, 1580) + if not self.wait_heading(170): + return False + self.set_rc(4, 1500) - # save bottom left corner of square (should be near home) as waypoint - progress("Save WP 6") - save_wp(mavproxy, mav) + # fly south east 50m + self.set_rc(2, 1100) + if not self.wait_distance(50): + return False + self.set_rc(2, 1500) - # descend to 10m - progress("Descend to 10m in Loiter") - mavproxy.send('switch 5\n') # loiter mode - wait_mode(mav, 'LOITER') - set_rc(mavproxy, mav, 3, 1300) - time_left = timeout - (get_sim_time(mav) - tstart) - progress("timeleft = %u" % time_left) - if time_left < 20: - time_left = 20 - if not wait_altitude(mav, -10, 10, time_left): - progress("Failed to reach alt of 10m") - success = False - save_wp(mavproxy, mav) + # wait for copter to slow moving + if not self.wait_groundspeed(0, 2): + return False - return success + success = True + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + start_altitude = m.alt + start = self.mav.location() + tstart = self.get_sim_time() + tholdstart = self.get_sim_time() + progress("Holding loiter at %u meters for %u seconds" % (start_altitude, holdtime)) + while self.get_sim_time() < tstart + holdtime: + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + pos = self.mav.location() + delta = self.get_distance(start, pos) + alt_delta = math.fabs(m.alt - start_altitude) + progress("Loiter Dist: %.2fm, alt:%u" % (delta, m.alt)) + if alt_delta > maxaltchange: + progress("Loiter alt shifted %u meters (> limit of %u)" % (alt_delta, maxaltchange)) + success = False + if delta > maxdistchange: + progress("Loiter shifted %u meters (> limit of %u)" % (delta, maxdistchange)) + success = False + if success: + progress("Loiter OK for %u seconds" % holdtime) + else: + progress("Loiter FAILED") + return success + def change_alt(self, alt_min, climb_throttle=1920, descend_throttle=1080): + """Change altitude.""" + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + if m.alt < alt_min: + progress("Rise to alt:%u from %u" % (alt_min, m.alt)) + self.set_rc(3, climb_throttle) + self.wait_altitude(alt_min, (alt_min + 5)) + else: + progress("Lower to alt:%u from %u" % (alt_min, m.alt)) + self.set_rc(3, descend_throttle) + self.wait_altitude((alt_min - 5), alt_min) + self.hover() + return True -def fly_RTL(mavproxy, mav, side=60, timeout=250): - """Return, land.""" - progress("# Enter RTL") - mavproxy.send('switch 3\n') - tstart = get_sim_time(mav) - while get_sim_time(mav) < tstart + timeout: - m = mav.recv_match(type='VFR_HUD', blocking=True) - pos = mav.location() - home_distance = get_distance(HOME, pos) - progress("Alt: %u HomeDistance: %.0f" % (m.alt, home_distance)) - if(m.alt <= 1 and home_distance < 10): - return True - return False + ################################################# + # TESTS FLY + ################################################# - -def fly_throttle_failsafe(mavproxy, mav, side=60, timeout=180): - """Fly east, Failsafe, return, land.""" - - # switch to loiter mode temporarily to stop us from rising - mavproxy.send('switch 5\n') - wait_mode(mav, 'LOITER') - - # first aim east - progress("turn east") - set_rc(mavproxy, mav, 4, 1580) - if not wait_heading(mav, 135): - return False - set_rc(mavproxy, mav, 4, 1500) - - # raise throttle slightly to avoid hitting the ground - set_rc(mavproxy, mav, 3, 1600) - - # switch to stabilize mode - mavproxy.send('switch 6\n') - wait_mode(mav, 'STABILIZE') - hover(mavproxy, mav) - - # fly east 60 meters - progress("# Going forward %u meters" % side) - set_rc(mavproxy, mav, 2, 1350) - if not wait_distance(mav, side, 5, 60): - return False - set_rc(mavproxy, mav, 2, 1500) - - # pull throttle low - progress("# Enter Failsafe") - set_rc(mavproxy, mav, 3, 900) - - tstart = get_sim_time(mav) - while get_sim_time(mav) < tstart + timeout: - m = mav.recv_match(type='VFR_HUD', blocking=True) - pos = mav.location() - home_distance = get_distance(HOME, pos) - progress("Alt: %u HomeDistance: %.0f" % (m.alt, home_distance)) - # check if we've reached home - if m.alt <= 1 and home_distance < 10: - # reduce throttle - set_rc(mavproxy, mav, 3, 1100) - # switch back to stabilize - mavproxy.send('switch 2\n') # land mode - wait_mode(mav, 'LAND') - progress("Waiting for disarm") - mav.motors_disarmed_wait() - progress("Reached failsafe home OK") - mavproxy.send('switch 6\n') # stabilize mode - wait_mode(mav, 'STABILIZE') - set_rc(mavproxy, mav, 3, 1000) - if not arm_vehicle(mavproxy, mav): - progress("Failed to re-arm") - return False - return True - progress("Failed to land on failsafe RTL - timed out after %u seconds" % timeout) - # reduce throttle - set_rc(mavproxy, mav, 3, 1100) - # switch back to stabilize mode - mavproxy.send('switch 2\n') # land mode - wait_mode(mav, 'LAND') - mavproxy.send('switch 6\n') # stabilize mode - wait_mode(mav, 'STABILIZE') - return False - - -def fly_battery_failsafe(mavproxy, mav, timeout=30): - # assume failure - success = False - - # switch to loiter mode so that we hold position - mavproxy.send('switch 5\n') - wait_mode(mav, 'LOITER') - mavproxy.send("rc 3 1500\n") - - # enable battery failsafe - mavproxy.send("param set FS_BATT_ENABLE 1\n") - - # trigger low voltage - mavproxy.send('param set SIM_BATT_VOLTAGE 10\n') - - # wait for LAND mode - new_mode = wait_mode(mav, 'LAND') - if new_mode == 'LAND': + # fly a square in stabilize mode + def fly_square(self, side=50, timeout=300): + """Fly a square, flying N then E .""" + tstart = self.get_sim_time() success = True - # disable battery failsafe - mavproxy.send('param set FS_BATT_ENABLE 0\n') + # ensure all sticks in the middle + self.set_rc(1, 1500) + self.set_rc(2, 1500) + self.set_rc(3, 1500) + self.set_rc(4, 1500) - # return status - if success: - progress("Successfully entered LAND mode after battery failsafe") - else: - progress("Failed to enter LAND mode after battery failsafe") + # switch to loiter mode temporarily to stop us from rising + self.mavproxy.send('switch 5\n') + self.wait_mode('LOITER') - return success - - -# fly_stability_patch - fly south, then hold loiter within 5m position and altitude and reduce 1 motor to 60% efficiency -def fly_stability_patch(mavproxy, mav, holdtime=30, maxaltchange=5, maxdistchange=10): - """Hold loiter position.""" - mavproxy.send('switch 5\n') # loiter mode - wait_mode(mav, 'LOITER') - - # first south - progress("turn south") - set_rc(mavproxy, mav, 4, 1580) - if not wait_heading(mav, 180): - return False - set_rc(mavproxy, mav, 4, 1500) - - # fly west 80m - set_rc(mavproxy, mav, 2, 1100) - if not wait_distance(mav, 80): - return False - set_rc(mavproxy, mav, 2, 1500) - - # wait for copter to slow moving - if not wait_groundspeed(mav, 0, 2): - return False - - success = True - m = mav.recv_match(type='VFR_HUD', blocking=True) - start_altitude = m.alt - start = mav.location() - tstart = get_sim_time(mav) - tholdstart = get_sim_time(mav) - progress("Holding loiter at %u meters for %u seconds" % (start_altitude, holdtime)) - - # cut motor 1 to 55% efficiency - progress("Cutting motor 1 to 60% efficiency") - mavproxy.send('param set SIM_ENGINE_MUL 0.60\n') - - while get_sim_time(mav) < tstart + holdtime: - m = mav.recv_match(type='VFR_HUD', blocking=True) - pos = mav.location() - delta = get_distance(start, pos) - alt_delta = math.fabs(m.alt - start_altitude) - progress("Loiter Dist: %.2fm, alt:%u" % (delta, m.alt)) - if alt_delta > maxaltchange: - progress("Loiter alt shifted %u meters (> limit of %u)" % (alt_delta, maxaltchange)) + # first aim north + progress("turn right towards north") + self.set_rc(4, 1580) + if not self.wait_heading(10): + progress("Failed to reach heading") success = False - if delta > maxdistchange: - progress("Loiter shifted %u meters (> limit of %u)" % (delta, maxdistchange)) + self.set_rc(4, 1500) + self.mav.recv_match(condition='RC_CHANNELS.chan4_raw==1500', blocking=True) + + # save bottom left corner of box as waypoint + progress("Save WP 1 & 2") + self.save_wp() + + # switch back to stabilize mode + self.set_rc(3, 1500) + self.mavproxy.send('switch 6\n') + self.wait_mode('STABILIZE') + + # pitch forward to fly north + progress("Going north %u meters" % side) + self.set_rc(2, 1300) + if not self.wait_distance(side): + progress("Failed to reach distance of %u" % side) success = False + self.set_rc(2, 1500) - # restore motor 1 to 100% efficiency - mavproxy.send('param set SIM_ENGINE_MUL 1.0\n') + # save top left corner of square as waypoint + progress("Save WP 3") + self.save_wp() - if success: - progress("Stability patch and Loiter OK for %u seconds" % holdtime) - else: - progress("Stability Patch FAILED") + # roll right to fly east + progress("Going east %u meters" % side) + self.set_rc(1, 1700) + if not self.wait_distance(side): + progress("Failed to reach distance of %u" % side) + success = False + self.set_rc(1, 1500) - return success + # save top right corner of square as waypoint + progress("Save WP 4") + self.save_wp() + # pitch back to fly south + progress("Going south %u meters" % side) + self.set_rc(2, 1700) + if not self.wait_distance(side): + progress("Failed to reach distance of %u" % side) + success = False + self.set_rc(2, 1500) -# fly_fence_test - fly east until you hit the horizontal circular fence -def fly_fence_test(mavproxy, mav, timeout=180): - """Hold loiter position.""" - mavproxy.send('switch 5\n') # loiter mode - wait_mode(mav, 'LOITER') + # save bottom right corner of square as waypoint + progress("Save WP 5") + self.save_wp() - # enable fence, disable avoidance - mavproxy.send('param set FENCE_ENABLE 1\n') - mavproxy.send('param set AVOID_ENABLE 0\n') + # roll left to fly west + progress("Going west %u meters" % side) + self.set_rc(1, 1300) + if not self.wait_distance(side): + progress("Failed to reach distance of %u" % side) + success = False + self.set_rc(1, 1500) - # first east - progress("turn east") - set_rc(mavproxy, mav, 4, 1580) - if not wait_heading(mav, 160): - return False - set_rc(mavproxy, mav, 4, 1500) + # save bottom left corner of square (should be near home) as waypoint + progress("Save WP 6") + self.save_wp() - # fly forward (east) at least 20m - pitching_forward = True - set_rc(mavproxy, mav, 2, 1100) - if not wait_distance(mav, 20): + # descend to 10m + progress("Descend to 10m in Loiter") + self.mavproxy.send('switch 5\n') # loiter mode + self.wait_mode('LOITER') + self.set_rc(3, 1300) + time_left = timeout - (self.get_sim_time() - tstart) + progress("timeleft = %u" % time_left) + if time_left < 20: + time_left = 20 + if not self.wait_altitude(-10, 10, time_left): + progress("Failed to reach alt of 10m") + success = False + self.save_wp() + + return success + + def fly_RTL(self, side=60, timeout=250): + """Return, land.""" + progress("# Enter RTL") + self.mavproxy.send('switch 3\n') + tstart = self.get_sim_time() + while self.get_sim_time() < tstart + timeout: + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + pos = self.mav.location() + home_distance = self.get_distance(HOME, pos) + progress("Alt: %u HomeDistance: %.0f" % (m.alt, home_distance)) + if m.alt <= 1 and home_distance < 10: + return True return False - # start timer - tstart = get_sim_time(mav) - while get_sim_time(mav) < tstart + timeout: - m = mav.recv_match(type='VFR_HUD', blocking=True) - pos = mav.location() - home_distance = get_distance(HOME, pos) - progress("Alt: %u HomeDistance: %.0f" % (m.alt, home_distance)) - # recenter pitch sticks once we reach home so we don't fly off again - if pitching_forward and home_distance < 10: - pitching_forward = False - set_rc(mavproxy, mav, 2, 1500) - # disable fence - mavproxy.send('param set FENCE_ENABLE 0\n') - if m.alt <= 1 and home_distance < 10: - # reduce throttle - set_rc(mavproxy, mav, 3, 1000) - # switch mode to stabilize - mavproxy.send('switch 2\n') # land mode - wait_mode(mav, 'LAND') - progress("Waiting for disarm") - mav.motors_disarmed_wait() - progress("Reached home OK") - mavproxy.send('switch 6\n') # stabilize mode - wait_mode(mav, 'STABILIZE') - set_rc(mavproxy, mav, 3, 1000) - mavproxy.send('arm uncheck all\n') # remove if we ever clear battery failsafe flag on disarm - if not arm_vehicle(mavproxy, mav): - progress("Failed to re-arm") - mavproxy.send('arm check all\n') # remove if we ever clear battery failsafe flag on disarm - return False - mavproxy.send('arm check all\n') # remove if we ever clear battery failsafe flag on disarm - progress("Reached home OK") - return True + def fly_throttle_failsafe(self, side=60, timeout=180): + """Fly east, Failsafe, return, land.""" - # disable fence, enable avoidance - mavproxy.send('param set FENCE_ENABLE 0\n') - mavproxy.send('param set AVOID_ENABLE 1\n') + # switch to loiter mode temporarily to stop us from rising + self.mavproxy.send('switch 5\n') + self.wait_mode('LOITER') - # reduce throttle - set_rc(mavproxy, mav, 3, 1000) - # switch mode to stabilize - mavproxy.send('switch 2\n') # land mode - wait_mode(mav, 'LAND') - mavproxy.send('switch 6\n') # stabilize mode - wait_mode(mav, 'STABILIZE') - progress("Fence test failed to reach home - timed out after %u seconds" % timeout) - return False + # first aim east + progress("turn east") + self.set_rc(4, 1580) + if not self.wait_heading(135): + return False + self.set_rc(4, 1500) + # raise throttle slightly to avoid hitting the ground + self.set_rc(3, 1600) -# fly_alt_fence_test - fly up until you hit the fence -def fly_alt_max_fence_test(mavproxy, mav, timeout=180): - """Hold loiter position.""" - mavproxy.send('switch 5\n') # loiter mode - wait_mode(mav, 'LOITER') + # switch to stabilize mode + self.mavproxy.send('switch 6\n') + self.wait_mode('STABILIZE') + self.hover() - # enable fence, disable avoidance - set_parameter(mavproxy, 'FENCE_ENABLE', 1) - set_parameter(mavproxy, 'AVOID_ENABLE', 0) - set_parameter(mavproxy, 'FENCE_TYPE', 1) + # fly east 60 meters + progress("# Going forward %u meters" % side) + self.set_rc(2, 1350) + if not self.wait_distance(side, 5, 60): + return False + self.set_rc(2, 1500) - if not change_alt(mavproxy, mav, 10): - failed_test_msg = "change_alt climb failed" - progress(failed_test_msg) + # pull throttle low + progress("# Enter Failsafe") + self.set_rc(3, 900) + + tstart = self.get_sim_time() + while self.get_sim_time() < tstart + timeout: + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + pos = self.mav.location() + home_distance = self.get_distance(HOME, pos) + progress("Alt: %u HomeDistance: %.0f" % (m.alt, home_distance)) + # check if we've reached home + if m.alt <= 1 and home_distance < 10: + # reduce throttle + self.set_rc(3, 1100) + # switch back to stabilize + self.mavproxy.send('switch 2\n') # land mode + self.wait_mode('LAND') + progress("Waiting for disarm") + self.mav.motors_disarmed_wait() + progress("Reached failsafe home OK") + self.mavproxy.send('switch 6\n') # stabilize mode + self.wait_mode('STABILIZE') + self.set_rc(3, 1000) + if not self.arm_vehicle(): + progress("Failed to re-arm") + return False + return True + progress("Failed to land on failsafe RTL - timed out after %u seconds" % timeout) + # reduce throttle + self.set_rc(3, 1100) + # switch back to stabilize mode + self.mavproxy.send('switch 2\n') # land mode + self.wait_mode('LAND') + self.mavproxy.send('switch 6\n') # stabilize mode + self.wait_mode('STABILIZE') return False - # first east - progress("turn east") - set_rc(mavproxy, mav, 4, 1580) - if not wait_heading(mav, 160): - return False - set_rc(mavproxy, mav, 4, 1500) + def fly_battery_failsafe(self, timeout=30): + # assume failure + success = False - # fly forward (east) at least 20m - set_rc(mavproxy, mav, 2, 1100) - if not wait_distance(mav, 20): - return False + # switch to loiter mode so that we hold position + self.mavproxy.send('switch 5\n') + self.wait_mode('LOITER') + self.set_rc(3, 1500) - # stop flying forward and start flying up: - set_rc(mavproxy, mav, 2, 1500) - set_rc(mavproxy, mav, 3, 1800) + # enable battery failsafe + self.set_parameter('FS_BATT_ENABLE', 1) - # wait for fence to trigger - wait_mode(mav, 'RTL') + # trigger low voltage + self.set_parameter('SIM_BATT_VOLTAGE', 10) - progress("Waiting for disarm") - mav.motors_disarmed_wait() + # wait for LAND mode + new_mode = self.wait_mode('LAND', 300) + if new_mode == 'LAND': + success = True - set_rc(mavproxy, mav, 3, 1000) + # disable battery failsafe + self.set_parameter('FS_BATT_ENABLE', 0) - mavproxy.send('switch 6\n') # stabilize mode - wait_mode(mav, 'STABILIZE') - mavproxy.send('arm uncheck all\n') # remove if we ever clear battery failsafe flag on disarm - if not arm_vehicle(mavproxy, mav): - progress("Failed to re-arm") - mavproxy.send('arm check all\n') # remove if we ever clear battery failsafe flag on disarm - return False - mavproxy.send('arm check all\n') # remove if we ever clear battery failsafe flag on disarm + # return status + if success: + progress("Successfully entered LAND mode after battery failsafe") + else: + progress("Failed to enter LAND mode after battery failsafe") - return True + return success -def fly_gps_glitch_loiter_test(mavproxy, mav, use_map=False, timeout=30, max_distance=20): - """fly_gps_glitch_loiter_test. + # fly_stability_patch - fly south, then hold loiter within 5m position and altitude and reduce 1 motor to 60% efficiency + def fly_stability_patch(self, holdtime=30, maxaltchange=5, maxdistchange=10): + """Hold loiter position.""" + self.mavproxy.send('switch 5\n') # loiter mode + self.wait_mode('LOITER') - Fly south east in loiter and test reaction to gps glitch.""" - mavproxy.send('switch 5\n') # loiter mode - wait_mode(mav, 'LOITER') + # first south + progress("turn south") + self.set_rc(4, 1580) + if not self.wait_heading(180): + return False + self.set_rc(4, 1500) - # turn on simulator display of gps and actual position - if (use_map): - show_gps_and_sim_positions(mavproxy, True) + # fly west 80m + self.set_rc(2, 1100) + if not self.wait_distance(80): + return False + self.set_rc(2, 1500) - # set-up gps glitch array - glitch_lat = [0.0002996, 0.0006958, 0.0009431, 0.0009991, 0.0009444, 0.0007716, 0.0006221] - glitch_lon = [0.0000717, 0.0000912, 0.0002761, 0.0002626, 0.0002807, 0.0002049, 0.0001304] - glitch_num = len(glitch_lat) - progress("GPS Glitches:") - for i in range(1, glitch_num): - progress("glitch %d %.7f %.7f" % (i, glitch_lat[i], glitch_lon[i])) + # wait for copter to slow moving + if not self.wait_groundspeed(0, 2): + return False - # turn south east - progress("turn south east") - set_rc(mavproxy, mav, 4, 1580) - if not wait_heading(mav, 150): - if (use_map): - show_gps_and_sim_positions(mavproxy, False) - return False - set_rc(mavproxy, mav, 4, 1500) + success = True + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + start_altitude = m.alt + start = self.mav.location() + tstart = self.get_sim_time() + tholdstart = self.get_sim_time() + progress("Holding loiter at %u meters for %u seconds" % (start_altitude, holdtime)) - # fly forward (south east) at least 60m - set_rc(mavproxy, mav, 2, 1100) - if not wait_distance(mav, 60): - if (use_map): - show_gps_and_sim_positions(mavproxy, False) - return False - set_rc(mavproxy, mav, 2, 1500) + # cut motor 1 to 55% efficiency + progress("Cutting motor 1 to 60% efficiency") + self.mavproxy.send('param set SIM_ENGINE_MUL 0.60\n') - # wait for copter to slow down - if not wait_groundspeed(mav, 0, 1): - if (use_map): - show_gps_and_sim_positions(mavproxy, False) - return False - - # record time and position - tstart = get_sim_time(mav) - tnow = tstart - start_pos = sim_location(mav) - success = True - - # initialise current glitch - glitch_current = 0 - progress("Apply first glitch") - mavproxy.send('param set SIM_GPS_GLITCH_X %.7f\n' % glitch_lat[glitch_current]) - mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current]) - - # record position for 30 seconds - while tnow < tstart + timeout: - tnow = get_sim_time(mav) - desired_glitch_num = int((tnow - tstart) * 2.2) - if desired_glitch_num > glitch_current and glitch_current != -1: - glitch_current = desired_glitch_num - # turn off glitching if we've reached the end of the glitch list - if glitch_current >= glitch_num: - glitch_current = -1 - progress("Completed Glitches") - mavproxy.send('param set SIM_GPS_GLITCH_X 0\n') - mavproxy.send('param set SIM_GPS_GLITCH_Y 0\n') - else: - progress("Applying glitch %u" % glitch_current) - # move onto the next glitch - mavproxy.send('param set SIM_GPS_GLITCH_X %.7f\n' % glitch_lat[glitch_current]) - mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current]) - - # start displaying distance moved after all glitches applied - if (glitch_current == -1): - m = mav.recv_match(type='VFR_HUD', blocking=True) - curr_pos = sim_location(mav) - moved_distance = get_distance(curr_pos, start_pos) - progress("Alt: %u Moved: %.0f" % (m.alt, moved_distance)) - if moved_distance > max_distance: - progress("Moved over %u meters, Failed!" % max_distance) + while self.get_sim_time() < tstart + holdtime: + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + pos = self.mav.location() + delta = self.get_distance(start, pos) + alt_delta = math.fabs(m.alt - start_altitude) + progress("Loiter Dist: %.2fm, alt:%u" % (delta, m.alt)) + if alt_delta > maxaltchange: + progress("Loiter alt shifted %u meters (> limit of %u)" % (alt_delta, maxaltchange)) + success = False + if delta > maxdistchange: + progress("Loiter shifted %u meters (> limit of %u)" % (delta, maxdistchange)) success = False - # disable gps glitch - if glitch_current != -1: - glitch_current = -1 - mavproxy.send('param set SIM_GPS_GLITCH_X 0\n') - mavproxy.send('param set SIM_GPS_GLITCH_Y 0\n') - if (use_map): - show_gps_and_sim_positions(mavproxy, False) + # restore motor 1 to 100% efficiency + self.mavproxy.send('param set SIM_ENGINE_MUL 1.0\n') - if success: - progress("GPS glitch test passed! stayed within %u meters for %u seconds" % (max_distance, timeout)) - else: - progress("GPS glitch test FAILED!") - return success + if success: + progress("Stability patch and Loiter OK for %u seconds" % holdtime) + else: + progress("Stability Patch FAILED") + return success -# fly_gps_glitch_auto_test - fly mission and test reaction to gps glitch -def fly_gps_glitch_auto_test(mavproxy, mav, use_map=False, timeout=120): + # fly_fence_test - fly east until you hit the horizontal circular fence + def fly_fence_test(self, timeout=180): + """Hold loiter position.""" + self.mavproxy.send('switch 5\n') # loiter mode + self.wait_mode('LOITER') - # set-up gps glitch array - glitch_lat = [0.0002996, 0.0006958, 0.0009431, 0.0009991, 0.0009444, 0.0007716, 0.0006221] - glitch_lon = [0.0000717, 0.0000912, 0.0002761, 0.0002626, 0.0002807, 0.0002049, 0.0001304] - glitch_num = len(glitch_lat) - progress("GPS Glitches:") - for i in range(1, glitch_num): - progress("glitch %d %.7f %.7f" % (i, glitch_lat[i], glitch_lon[i])) + # enable fence, disable avoidance + self.mavproxy.send('param set FENCE_ENABLE 1\n') + self.mavproxy.send('param set AVOID_ENABLE 0\n') - # Fly mission #1 - progress("# Load copter_glitch_mission") - # load the waypoint count - global homeloc - global num_wp - num_wp = load_mission_from_file(mavproxy, os.path.join(testdir, "copter_glitch_mission.txt")) - if not num_wp: - progress("load copter_glitch_mission failed") + # first east + progress("turn east") + self.set_rc(4, 1580) + if not self.wait_heading(160): + return False + self.set_rc(4, 1500) + + # fly forward (east) at least 20m + pitching_forward = True + self.set_rc(2, 1100) + if not self.wait_distance(20): + return False + + # start timer + tstart = self.get_sim_time() + while self.get_sim_time() < tstart + timeout: + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + pos = self.mav.location() + home_distance = self.get_distance(HOME, pos) + progress("Alt: %u HomeDistance: %.0f" % (m.alt, home_distance)) + # recenter pitch sticks once we reach home so we don't fly off again + if pitching_forward and home_distance < 10: + pitching_forward = False + self.set_rc(2, 1500) + # disable fence + self.mavproxy.send('param set FENCE_ENABLE 0\n') + if m.alt <= 1 and home_distance < 10: + # reduce throttle + self.set_rc(3, 1000) + # switch mode to stabilize + self.mavproxy.send('switch 2\n') # land mode + self.wait_mode('LAND') + progress("Waiting for disarm") + self.mav.motors_disarmed_wait() + progress("Reached home OK") + self.mavproxy.send('switch 6\n') # stabilize mode + self.wait_mode('STABILIZE') + self.set_rc(3, 1000) + self.mavproxy.send('arm uncheck all\n') # remove if we ever clear battery failsafe flag on disarm + if not self.arm_vehicle(): + progress("Failed to re-arm") + self.mavproxy.send('arm check all\n') # remove if we ever clear battery failsafe flag on disarm + return False + self.mavproxy.send('arm check all\n') # remove if we ever clear battery failsafe flag on disarm + progress("Reached home OK") + return True + + # disable fence, enable avoidance + self.mavproxy.send('param set FENCE_ENABLE 0\n') + self.mavproxy.send('param set AVOID_ENABLE 1\n') + + # reduce throttle + self.set_rc(3, 1000) + # switch mode to stabilize + self.mavproxy.send('switch 2\n') # land mode + self.wait_mode('LAND') + self.mavproxy.send('switch 6\n') # stabilize mode + self.wait_mode('STABILIZE') + progress("Fence test failed to reach home - timed out after %u seconds" % timeout) return False - # turn on simulator display of gps and actual position - if (use_map): - show_gps_and_sim_positions(mavproxy, True) - - progress("test: Fly a mission from 1 to %u" % num_wp) - mavproxy.send('wp set 1\n') - - # switch into AUTO mode and raise throttle - mavproxy.send('switch 4\n') # auto mode - wait_mode(mav, 'AUTO') - set_rc(mavproxy, mav, 3, 1500) - - # wait until 100m from home - if not wait_distance(mav, 100, 5, 60): - if (use_map): - show_gps_and_sim_positions(mavproxy, False) - return False - - # record time and position - tstart = get_sim_time(mav) - tnow = tstart - - # initialise current glitch - glitch_current = 0 - progress("Apply first glitch") - mavproxy.send('param set SIM_GPS_GLITCH_X %.7f\n' % glitch_lat[glitch_current]) - mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current]) - - # record position for 30 seconds - while glitch_current < glitch_num: - tnow = get_sim_time(mav) - desired_glitch_num = int((tnow - tstart) * 2.2) - if desired_glitch_num > glitch_current and glitch_current != -1: - glitch_current = desired_glitch_num - # apply next glitch - if glitch_current < glitch_num: - progress("Applying glitch %u" % glitch_current) - mavproxy.send('param set SIM_GPS_GLITCH_X %.7f\n' % glitch_lat[glitch_current]) - mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current]) - - # turn off glitching - progress("Completed Glitches") - mavproxy.send('param set SIM_GPS_GLITCH_X 0\n') - mavproxy.send('param set SIM_GPS_GLITCH_Y 0\n') - - # continue with the mission - ret = wait_waypoint(mav, 0, num_wp-1, timeout=500) - - # wait for arrival back home - m = mav.recv_match(type='VFR_HUD', blocking=True) - pos = mav.location() - dist_to_home = get_distance(HOME, pos) - while dist_to_home > 5: - if get_sim_time(mav) > (tstart + timeout): - progress("GPS Glitch testing failed - exceeded timeout %u seconds" % timeout) - ret = False - break - m = mav.recv_match(type='VFR_HUD', blocking=True) - pos = mav.location() - dist_to_home = get_distance(HOME, pos) - progress("Dist from home: %u" % dist_to_home) - - # turn off simulator display of gps and actual position - if (use_map): - show_gps_and_sim_positions(mavproxy, False) - - progress("GPS Glitch test Auto completed: passed=%s" % ret) - - return ret - - -# fly_simple - assumes the simple bearing is initialised to be directly north -# flies a box with 100m west, 15 seconds north, 50 seconds east, 15 seconds south -def fly_simple(mavproxy, mav, side=50, timeout=120): - - failed = False - - # hold position in loiter - mavproxy.send('switch 5\n') # loiter mode - wait_mode(mav, 'LOITER') - - # set SIMPLE mode for all flight modes - mavproxy.send('param set SIMPLE 63\n') - - # switch to stabilize mode - mavproxy.send('switch 6\n') - wait_mode(mav, 'STABILIZE') - set_rc(mavproxy, mav, 3, 1500) - - # fly south 50m - progress("# Flying south %u meters" % side) - set_rc(mavproxy, mav, 1, 1300) - if not wait_distance(mav, side, 5, 60): - failed = True - set_rc(mavproxy, mav, 1, 1500) - - # fly west 8 seconds - progress("# Flying west for 8 seconds") - set_rc(mavproxy, mav, 2, 1300) - tstart = get_sim_time(mav) - while get_sim_time(mav) < (tstart + 8): - m = mav.recv_match(type='VFR_HUD', blocking=True) - delta = (get_sim_time(mav) - tstart) - # progress("%u" % delta) - set_rc(mavproxy, mav, 2, 1500) - - # fly north 25 meters - progress("# Flying north %u meters" % (side/2.0)) - set_rc(mavproxy, mav, 1, 1700) - if not wait_distance(mav, side/2, 5, 60): - failed = True - set_rc(mavproxy, mav, 1, 1500) - - # fly east 8 seconds - progress("# Flying east for 8 seconds") - set_rc(mavproxy, mav, 2, 1700) - tstart = get_sim_time(mav) - while get_sim_time(mav) < (tstart + 8): - m = mav.recv_match(type='VFR_HUD', blocking=True) - delta = (get_sim_time(mav) - tstart) - # progress("%u" % delta) - set_rc(mavproxy, mav, 2, 1500) - - # restore to default - mavproxy.send('param set SIMPLE 0\n') - - # hover in place - hover(mavproxy, mav) - return not failed - - -# fly_super_simple - flies a circle around home for 45 seconds -def fly_super_simple(mavproxy, mav, timeout=45): - - failed = False - - # hold position in loiter - mavproxy.send('switch 5\n') # loiter mode - wait_mode(mav, 'LOITER') - - # fly forward 20m - progress("# Flying forward 20 meters") - set_rc(mavproxy, mav, 2, 1300) - if not wait_distance(mav, 20, 5, 60): - failed = True - set_rc(mavproxy, mav, 2, 1500) - - # set SUPER SIMPLE mode for all flight modes - mavproxy.send('param set SUPER_SIMPLE 63\n') - - # switch to stabilize mode - mavproxy.send('switch 6\n') - wait_mode(mav, 'STABILIZE') - set_rc(mavproxy, mav, 3, 1500) - - # start copter yawing slowly - set_rc(mavproxy, mav, 4, 1550) - - # roll left for timeout seconds - progress("# rolling left from pilot's point of view for %u seconds" % timeout) - set_rc(mavproxy, mav, 1, 1300) - tstart = get_sim_time(mav) - while get_sim_time(mav) < (tstart + timeout): - m = mav.recv_match(type='VFR_HUD', blocking=True) - delta = (get_sim_time(mav) - tstart) - - # stop rolling and yawing - set_rc(mavproxy, mav, 1, 1500) - set_rc(mavproxy, mav, 4, 1500) - - # restore simple mode parameters to default - mavproxy.send('param set SUPER_SIMPLE 0\n') - - # hover in place - hover(mavproxy, mav) - return not failed - - -# fly_circle - flies a circle with 20m radius -def fly_circle(mavproxy, mav, maxaltchange=10, holdtime=36): - - # hold position in loiter - mavproxy.send('switch 5\n') # loiter mode - wait_mode(mav, 'LOITER') - - # face west - progress("turn west") - set_rc(mavproxy, mav, 4, 1580) - if not wait_heading(mav, 270): - return False - set_rc(mavproxy, mav, 4, 1500) - - # set CIRCLE radius - mavproxy.send('param set CIRCLE_RADIUS 3000\n') - - # fly forward (east) at least 100m - set_rc(mavproxy, mav, 2, 1100) - if not wait_distance(mav, 100): - return False - - # return pitch stick back to middle - set_rc(mavproxy, mav, 2, 1500) - - # set CIRCLE mode - mavproxy.send('switch 1\n') # circle mode - wait_mode(mav, 'CIRCLE') - - # wait - m = mav.recv_match(type='VFR_HUD', blocking=True) - start_altitude = m.alt - tstart = get_sim_time(mav) - tholdstart = get_sim_time(mav) - progress("Circle at %u meters for %u seconds" % (start_altitude, holdtime)) - while get_sim_time(mav) < tstart + holdtime: - m = mav.recv_match(type='VFR_HUD', blocking=True) - progress("heading %u" % m.heading) - - progress("CIRCLE OK for %u seconds" % holdtime) - return True - - -# fly_auto_test - fly mission which tests a significant number of commands -def fly_auto_test(mavproxy, mav): - - # Fly mission #1 - progress("# Load copter_mission") - # load the waypoint count - global homeloc - global num_wp - num_wp = load_mission_from_file(mavproxy, os.path.join(testdir, "copter_mission.txt")) - if not num_wp: - progress("load copter_mission failed") - return False - - progress("test: Fly a mission from 1 to %u" % num_wp) - mavproxy.send('wp set 1\n') - - # switch into AUTO mode and raise throttle - mavproxy.send('switch 4\n') # auto mode - wait_mode(mav, 'AUTO') - set_rc(mavproxy, mav, 3, 1500) - - # fly the mission - ret = wait_waypoint(mav, 0, num_wp-1, timeout=500) - - # land if mission failed - if ret is False: - land(mavproxy, mav) - - # set throttle to minimum - set_rc(mavproxy, mav, 3, 1000) - - # wait for disarm - mav.motors_disarmed_wait() - progress("MOTORS DISARMED OK") - - progress("Auto mission completed: passed=%s" % ret) - - return ret - - -# fly_avc_test - fly AVC mission -def fly_avc_test(mavproxy, mav): - - # upload mission from file - progress("# Load copter_AVC2013_mission") - # load the waypoint count - global homeloc - global num_wp - num_wp = load_mission_from_file(mavproxy, os.path.join(testdir, "copter_AVC2013_mission.txt")) - if not num_wp: - progress("load copter_AVC2013_mission failed") - return False - - progress("Fly AVC mission from 1 to %u" % num_wp) - mavproxy.send('wp set 1\n') - - # wait for motor runup - wait_seconds(mav, 20) - - # switch into AUTO mode and raise throttle - mavproxy.send('switch 4\n') # auto mode - wait_mode(mav, 'AUTO') - set_rc(mavproxy, mav, 3, 1500) - - # fly the mission - ret = wait_waypoint(mav, 0, num_wp-1, timeout=500) - - # set throttle to minimum - set_rc(mavproxy, mav, 3, 1000) - - # wait for disarm - mav.motors_disarmed_wait() - progress("MOTORS DISARMED OK") - - progress("AVC mission completed: passed=%s" % ret) - - return ret - - -def fly_mission(mavproxy, mav, height_accuracy=-1.0, target_altitude=None): - """Fly a mission from a file.""" - global homeloc - global num_wp - progress("test: Fly a mission from 1 to %u" % num_wp) - mavproxy.send('wp set 1\n') - mavproxy.send('switch 4\n') # auto mode - wait_mode(mav, 'AUTO') - ret = wait_waypoint(mav, 0, num_wp-1, timeout=500) - progress("test: MISSION COMPLETE: passed=%s" % ret) - # wait here until ready - mavproxy.send('switch 5\n') # loiter mode - wait_mode(mav, 'LOITER') - return ret - - -def fly_ArduCopter(binary, viewerip=None, use_map=False, valgrind=False, gdb=False, frame=None, params=None, gdbserver=False, speedup=10): - """Fly ArduCopter in SITL. - - you can pass viewerip as an IP address to optionally send fg and - mavproxy packets too for local viewing of the flight in real time - """ - global homeloc - - if frame is None: - frame = '+' - - home = "%f,%f,%u,%u" % (HOME.lat, HOME.lng, HOME.alt, HOME.heading) - sitl = util.start_SITL(binary, wipe=True, model=frame, home=home, speedup=speedup) - mavproxy = util.start_MAVProxy_SITL('ArduCopter') - mavproxy.expect('Received [0-9]+ parameters') - - # setup test parameters - if params is None: - params = vinfo.options["ArduCopter"]["frames"][frame]["default_params_filename"] - if not isinstance(params, list): - params = [params] - for x in params: - mavproxy.send("param load %s\n" % os.path.join(testdir, x)) - mavproxy.expect('Loaded [0-9]+ parameters') - mavproxy.send("param set LOG_REPLAY 1\n") - mavproxy.send("param set LOG_DISARMED 1\n") - time.sleep(3) - - # reboot with new parameters - util.pexpect_close(mavproxy) - util.pexpect_close(sitl) - - sitl = util.start_SITL(binary, model=frame, home=home, speedup=speedup, valgrind=valgrind, gdb=gdb, gdbserver=gdbserver) - options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter --streamrate=5' - if viewerip: - options += ' --out=%s:14550' % viewerip - if use_map: - options += ' --map' - mavproxy = util.start_MAVProxy_SITL('ArduCopter', options=options) - mavproxy.expect('Telemetry log: (\S+)\r\n') - logfile = mavproxy.match.group(1) - progress("LOGFILE %s" % logfile) - - buildlog = util.reltopdir("../buildlogs/ArduCopter-test.tlog") - progress("buildlog=%s" % buildlog) - copy_tlog = False - if os.path.exists(buildlog): - os.unlink(buildlog) - try: - os.link(logfile, buildlog) - except Exception: - progress("WARN: Failed to create symlink: " + logfile + " => " + buildlog + ", Will copy tlog manually to target location") - copy_tlog = True - - # the received parameters can come before or after the ready to fly message - mavproxy.expect(['Received [0-9]+ parameters', 'Ready to FLY']) - mavproxy.expect(['Received [0-9]+ parameters', 'Ready to FLY']) - - util.expect_setup_callback(mavproxy, expect_callback) - - expect_list_clear() - expect_list_extend([sitl, mavproxy]) - - # get a mavlink connection going - try: - mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True) - except Exception as msg: - progress("Failed to start mavlink connection on 127.0.0.1:19550" % msg) - raise - mav.message_hooks.append(message_hook) - mav.idle_hooks.append(idle_hook) - - failed = False - failed_test_msg = "None" - - try: - progress("Waiting for a heartbeat with mavlink protocol %s" % mav.WIRE_PROTOCOL_VERSION) - mav.wait_heartbeat() - progress("Setting up RC parameters") - set_rc_default(mavproxy) - set_rc(mavproxy, mav, 3, 1000) - homeloc = mav.location() - - progress("Home location: %s" % homeloc) - mavproxy.send('switch 6\n') # stabilize mode - mav.wait_heartbeat() - wait_mode(mav, 'STABILIZE') - progress("Waiting reading for arm") - wait_ready_to_arm(mav) - - # Arm - progress("# Arm motors") - if not arm_vehicle(mavproxy, mav): - failed_test_msg = "arm_motors failed" - progress(failed_test_msg) - failed = True - - progress("# Takeoff") - if not takeoff(mavproxy, mav, 10): - failed_test_msg = "takeoff failed" - progress(failed_test_msg) - failed = True - - # Fly a square in Stabilize mode - progress("#") - progress("########## Fly a square and save WPs with CH7 switch ##########") - progress("#") - if not fly_square(mavproxy, mav): - failed_test_msg = "fly_square failed" - progress(failed_test_msg) - failed = True - - # save the stored mission to file - progress("# Save out the CH7 mission to file") - global num_wp - num_wp = save_mission_to_file(mavproxy, os.path.join(testdir, "ch7_mission.txt")) - if not num_wp: - failed_test_msg = "save_mission_to_file failed" - progress(failed_test_msg) - failed = True - - # fly the stored mission - progress("# Fly CH7 saved mission") - if not fly_mission(mavproxy, mav, height_accuracy=0.5, target_altitude=10): - failed_test_msg = "fly ch7_mission failed" - progress(failed_test_msg) - failed = True - - # Throttle Failsafe - progress("#") - progress("########## Test Failsafe ##########") - progress("#") - if not fly_throttle_failsafe(mavproxy, mav): - failed_test_msg = "fly_throttle_failsafe failed" - progress(failed_test_msg) - failed = True - - # Takeoff - progress("# Takeoff") - if not takeoff(mavproxy, mav, 10): - failed_test_msg = "takeoff failed" - progress(failed_test_msg) - failed = True - - # Battery failsafe - if not fly_battery_failsafe(mavproxy, mav): - failed_test_msg = "fly_battery_failsafe failed" - progress(failed_test_msg) - failed = True - - # Takeoff - progress("# Takeoff") - if not takeoff(mavproxy, mav, 10): - failed_test_msg = "takeoff failed" - progress(failed_test_msg) - failed = True - - # Stability patch - progress("#") - progress("########## Test Stability Patch ##########") - progress("#") - if not fly_stability_patch(mavproxy, mav, 30): - failed_test_msg = "fly_stability_patch failed" - progress(failed_test_msg) - failed = True - - # RTL - progress("# RTL #") - if not fly_RTL(mavproxy, mav): - failed_test_msg = "fly_RTL after stab patch failed" - progress(failed_test_msg) - failed = True - - # Takeoff - progress("# Takeoff") - if not takeoff(mavproxy, mav, 10): - failed_test_msg = "takeoff failed" - progress(failed_test_msg) - failed = True - - # Fence test - progress("#") - progress("########## Test Horizontal Fence ##########") - progress("#") - if not fly_fence_test(mavproxy, mav, 180): - failed_test_msg = "fly_fence_test failed" - progress(failed_test_msg) - failed = True - - # Fence test - progress("#") - progress("########## Test Max Alt Fence ##########") - progress("#") - if not fly_alt_max_fence_test(mavproxy, mav, 180): - failed_test_msg = "fly_alt_max_fence_test failed" - progress(failed_test_msg) - failed = True - - # Takeoff - progress("# Takeoff") - if not takeoff(mavproxy, mav, 10): - failed_test_msg = "takeoff failed" - progress(failed_test_msg) - failed = True - - # Fly GPS Glitch Loiter test - progress("# GPS Glitch Loiter Test") - if not fly_gps_glitch_loiter_test(mavproxy, mav, use_map): - failed_test_msg = "fly_gps_glitch_loiter_test failed" - progress(failed_test_msg) - failed = True - - # RTL after GPS Glitch Loiter test - progress("# RTL #") - if not fly_RTL(mavproxy, mav): - failed_test_msg = "fly_RTL failed" - progress(failed_test_msg) - failed = True - - # Fly GPS Glitch test in auto mode - progress("# GPS Glitch Auto Test") - if not fly_gps_glitch_auto_test(mavproxy, mav, use_map): - failed_test_msg = "fly_gps_glitch_auto_test failed" - progress(failed_test_msg) - failed = True - - # take-off ahead of next test - progress("# Takeoff") - if not takeoff(mavproxy, mav, 10): - failed_test_msg = "takeoff failed" - progress(failed_test_msg) - failed = True - - # Loiter for 10 seconds - progress("#") - progress("########## Test Loiter for 10 seconds ##########") - progress("#") - if not loiter(mavproxy, mav): - failed_test_msg = "loiter failed" - progress(failed_test_msg) - failed = True - - # Loiter Climb - progress("#") - progress("# Loiter - climb to 30m") - progress("#") - if not change_alt(mavproxy, mav, 30): + # fly_alt_fence_test - fly up until you hit the fence + def fly_alt_max_fence_test(self, timeout=180): + """Hold loiter position.""" + self.mavproxy.send('switch 5\n') # loiter mode + self.wait_mode('LOITER') + + # enable fence, disable avoidance + self.set_parameter('FENCE_ENABLE', 1) + self.set_parameter('AVOID_ENABLE', 0) + self.set_parameter('FENCE_TYPE', 1) + + if not self.change_alt(10): failed_test_msg = "change_alt climb failed" progress(failed_test_msg) - failed = True + return False - # Loiter Descend - progress("#") - progress("# Loiter - descend to 20m") - progress("#") - if not change_alt(mavproxy, mav, 20): - failed_test_msg = "change_alt descend failed" - progress(failed_test_msg) - failed = True + # first east + progress("turn east") + self.set_rc(4, 1580) + if not self.wait_heading(160): + return False + self.set_rc(4, 1500) - # RTL - progress("#") - progress("########## Test RTL ##########") - progress("#") - if not fly_RTL(mavproxy, mav): - failed_test_msg = "fly_RTL after Loiter climb/descend failed" - progress(failed_test_msg) - failed = True + # fly forward (east) at least 20m + self.set_rc(2, 1100) + if not self.wait_distance(20): + return False - # Takeoff - progress("# Takeoff") - if not takeoff(mavproxy, mav, 10): - failed_test_msg = "takeoff failed" - progress(failed_test_msg) - failed = True + # stop flying forward and start flying up: + self.set_rc(2, 1500) + self.set_rc(3, 1800) - # Simple mode - progress("# Fly in SIMPLE mode") - if not fly_simple(mavproxy, mav): - failed_test_msg = "fly_simple failed" - progress(failed_test_msg) - failed = True + # wait for fence to trigger + self.wait_mode('RTL') - # RTL - progress("#") - progress("########## Test RTL ##########") - progress("#") - if not fly_RTL(mavproxy, mav): - failed_test_msg = "fly_RTL after simple mode failed" - progress(failed_test_msg) - failed = True + progress("Waiting for disarm") + self.mav.motors_disarmed_wait() - # Takeoff - progress("# Takeoff") - if not takeoff(mavproxy, mav, 10): - failed_test_msg = "takeoff failed" - progress(failed_test_msg) - failed = True + self.set_rc(3, 1000) - # Fly a circle in super simple mode - progress("# Fly a circle in SUPER SIMPLE mode") - if not fly_super_simple(mavproxy, mav): - failed_test_msg = "fly_super_simple failed" - progress(failed_test_msg) - failed = True + self.mavproxy.send('switch 6\n') # stabilize mode + self.wait_mode('STABILIZE') + self.mavproxy.send('arm uncheck all\n') # remove if we ever clear battery failsafe flag on disarm + if not self.arm_vehicle(): + progress("Failed to re-arm") + self.mavproxy.send('arm check all\n') # remove if we ever clear battery failsafe flag on disarm + return False + self.mavproxy.send('arm check all\n') # remove if we ever clear battery failsafe flag on disarm - # RTL - progress("# RTL #") - if not fly_RTL(mavproxy, mav): - failed_test_msg = "fly_RTL after super simple mode failed" - progress(failed_test_msg) - failed = True + return True - # Takeoff - progress("# Takeoff") - if not takeoff(mavproxy, mav, 10): - failed_test_msg = "takeoff failed" - progress(failed_test_msg) - failed = True + def fly_gps_glitch_loiter_test(self, timeout=30, max_distance=20): + """fly_gps_glitch_loiter_test. - # Circle mode - progress("# Fly CIRCLE mode") - if not fly_circle(mavproxy, mav): - failed_test_msg = "fly_circle failed" - progress(failed_test_msg) - failed = True + Fly south east in loiter and test reaction to gps glitch.""" + self.mavproxy.send('switch 5\n') # loiter mode + self.wait_mode('LOITER') - # RTL - progress("#") - progress("########## Test RTL ##########") - progress("#") - if not fly_RTL(mavproxy, mav): - failed_test_msg = "fly_RTL after circle failed" - progress(failed_test_msg) - failed = True + # turn on simulator display of gps and actual position + if self.use_map: + self.show_gps_and_sim_positions(True) - progress("# Fly copter mission") - if not fly_auto_test(mavproxy, mav): - failed_test_msg = "fly_auto_test failed" - progress(failed_test_msg) - failed = True + # set-up gps glitch array + glitch_lat = [0.0002996, 0.0006958, 0.0009431, 0.0009991, 0.0009444, 0.0007716, 0.0006221] + glitch_lon = [0.0000717, 0.0000912, 0.0002761, 0.0002626, 0.0002807, 0.0002049, 0.0001304] + glitch_num = len(glitch_lat) + progress("GPS Glitches:") + for i in range(1, glitch_num): + progress("glitch %d %.7f %.7f" % (i, glitch_lat[i], glitch_lon[i])) + + # turn south east + progress("turn south east") + self.set_rc(4, 1580) + if not self.wait_heading(150): + if self.use_map: + self.show_gps_and_sim_positions(False) + return False + self.set_rc(4, 1500) + + # fly forward (south east) at least 60m + self.set_rc(2, 1100) + if not self.wait_distance(60): + if self.use_map: + self.show_gps_and_sim_positions(False) + return False + self.set_rc(2, 1500) + + # wait for copter to slow down + if not self.wait_groundspeed(0, 1): + if self.use_map: + self.show_gps_and_sim_positions(False) + return False + + # record time and position + tstart = self.get_sim_time() + tnow = tstart + start_pos = self.sim_location() + success = True + + # initialise current glitch + glitch_current = 0 + progress("Apply first glitch") + self.mavproxy.send('param set SIM_GPS_GLITCH_X %.7f\n' % glitch_lat[glitch_current]) + self.mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current]) + + # record position for 30 seconds + while tnow < tstart + timeout: + tnow = self.get_sim_time() + desired_glitch_num = int((tnow - tstart) * 2.2) + if desired_glitch_num > glitch_current and glitch_current != -1: + glitch_current = desired_glitch_num + # turn off glitching if we've reached the end of the glitch list + if glitch_current >= glitch_num: + glitch_current = -1 + progress("Completed Glitches") + self.mavproxy.send('param set SIM_GPS_GLITCH_X 0\n') + self.mavproxy.send('param set SIM_GPS_GLITCH_Y 0\n') + else: + progress("Applying glitch %u" % glitch_current) + # move onto the next glitch + self.mavproxy.send('param set SIM_GPS_GLITCH_X %.7f\n' % glitch_lat[glitch_current]) + self.mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current]) + + # start displaying distance moved after all glitches applied + if glitch_current == -1: + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + curr_pos = self.sim_location() + moved_distance = self.get_distance(curr_pos, start_pos) + progress("Alt: %u Moved: %.0f" % (m.alt, moved_distance)) + if moved_distance > max_distance: + progress("Moved over %u meters, Failed!" % max_distance) + success = False + + # disable gps glitch + if glitch_current != -1: + glitch_current = -1 + self.mavproxy.send('param set SIM_GPS_GLITCH_X 0\n') + self.mavproxy.send('param set SIM_GPS_GLITCH_Y 0\n') + if self.use_map: + self.show_gps_and_sim_positions(False) + + if success: + progress("GPS glitch test passed! stayed within %u meters for %u seconds" % (max_distance, timeout)) else: - progress("Flew copter mission OK") + progress("GPS glitch test FAILED!") + return success + + # fly_gps_glitch_auto_test - fly mission and test reaction to gps glitch + def fly_gps_glitch_auto_test(self, timeout=120): + + # set-up gps glitch array + glitch_lat = [0.0002996, 0.0006958, 0.0009431, 0.0009991, 0.0009444, 0.0007716, 0.0006221] + glitch_lon = [0.0000717, 0.0000912, 0.0002761, 0.0002626, 0.0002807, 0.0002049, 0.0001304] + glitch_num = len(glitch_lat) + progress("GPS Glitches:") + for i in range(1, glitch_num): + progress("glitch %d %.7f %.7f" % (i, glitch_lat[i], glitch_lon[i])) + + # Fly mission #1 + progress("# Load copter_glitch_mission") + # load the waypoint count + global num_wp + num_wp = self.load_mission_from_file(os.path.join(testdir, "copter_glitch_mission.txt")) + if not num_wp: + progress("load copter_glitch_mission failed") + return False + + # turn on simulator display of gps and actual position + if self.use_map: + self.show_gps_and_sim_positions(True) + + progress("test: Fly a mission from 1 to %u" % num_wp) + self.mavproxy.send('wp set 1\n') + + # switch into AUTO mode and raise throttle + self.mavproxy.send('switch 4\n') # auto mode + self.wait_mode('AUTO') + self.set_rc(3, 1500) + + # wait until 100m from home + if not self.wait_distance(100, 5, 60): + if self.use_map: + self.show_gps_and_sim_positions(False) + return False + + # record time and position + tstart = self.get_sim_time() + tnow = tstart + + # initialise current glitch + glitch_current = 0 + progress("Apply first glitch") + self.mavproxy.send('param set SIM_GPS_GLITCH_X %.7f\n' % glitch_lat[glitch_current]) + self.mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current]) + + # record position for 30 seconds + while glitch_current < glitch_num: + tnow = self.get_sim_time() + desired_glitch_num = int((tnow - tstart) * 2.2) + if desired_glitch_num > glitch_current and glitch_current != -1: + glitch_current = desired_glitch_num + # apply next glitch + if glitch_current < glitch_num: + progress("Applying glitch %u" % glitch_current) + self.mavproxy.send('param set SIM_GPS_GLITCH_X %.7f\n' % glitch_lat[glitch_current]) + self.mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current]) + + # turn off glitching + progress("Completed Glitches") + self.mavproxy.send('param set SIM_GPS_GLITCH_X 0\n') + self.mavproxy.send('param set SIM_GPS_GLITCH_Y 0\n') + + # continue with the mission + ret = self.wait_waypoint(0, num_wp-1, timeout=500) + + # wait for arrival back home + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + pos = self.mav.location() + dist_to_home = self.get_distance(HOME, pos) + while dist_to_home > 5: + if self.get_sim_time() > (tstart + timeout): + progress("GPS Glitch testing failed - exceeded timeout %u seconds" % timeout) + ret = False + break + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + pos = self.mav.location() + dist_to_home = self.get_distance(HOME, pos) + progress("Dist from home: %u" % dist_to_home) + + # turn off simulator display of gps and actual position + if self.use_map: + self.show_gps_and_sim_positions(False) + + progress("GPS Glitch test Auto completed: passed=%s" % ret) + + return ret + + # fly_simple - assumes the simple bearing is initialised to be directly north + # flies a box with 100m west, 15 seconds north, 50 seconds east, 15 seconds south + def fly_simple(self, side=50, timeout=120): + + failed = False + + # hold position in loiter + self.mavproxy.send('switch 5\n') # loiter mode + self.wait_mode('LOITER') + + # set SIMPLE mode for all flight modes + self.mavproxy.send('param set SIMPLE 63\n') + + # switch to stabilize mode + self.mavproxy.send('switch 6\n') + self.wait_mode('STABILIZE') + self.set_rc(3, 1500) + + # fly south 50m + progress("# Flying south %u meters" % side) + self.set_rc(1, 1300) + if not self.wait_distance(side, 5, 60): + failed = True + self.set_rc(1, 1500) + + # fly west 8 seconds + progress("# Flying west for 8 seconds") + self.set_rc(2, 1300) + tstart = self.get_sim_time() + while self.get_sim_time() < (tstart + 8): + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + delta = (self.get_sim_time() - tstart) + # progress("%u" % delta) + self.set_rc(2, 1500) + + # fly north 25 meters + progress("# Flying north %u meters" % (side/2.0)) + self.set_rc(1, 1700) + if not self.wait_distance(side/2, 5, 60): + failed = True + self.set_rc(1, 1500) + + # fly east 8 seconds + progress("# Flying east for 8 seconds") + self.set_rc(2, 1700) + tstart = self.get_sim_time() + while self.get_sim_time() < (tstart + 8): + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + delta = (self.get_sim_time() - tstart) + # progress("%u" % delta) + self.set_rc(2, 1500) + + # restore to default + self.mavproxy.send('param set SIMPLE 0\n') + + # hover in place + self.hover() + return not failed + + # fly_super_simple - flies a circle around home for 45 seconds + def fly_super_simple(self, timeout=45): + + failed = False + + # hold position in loiter + self.mavproxy.send('switch 5\n') # loiter mode + self.wait_mode('LOITER') + + # fly forward 20m + progress("# Flying forward 20 meters") + self.set_rc(2, 1300) + if not self.wait_distance(20, 5, 60): + failed = True + self.set_rc(2, 1500) + + # set SUPER SIMPLE mode for all flight modes + self.mavproxy.send('param set SUPER_SIMPLE 63\n') + + # switch to stabilize mode + self.mavproxy.send('switch 6\n') + self.wait_mode('STABILIZE') + self.set_rc(3, 1500) + + # start copter yawing slowly + self.set_rc(4, 1550) + + # roll left for timeout seconds + progress("# rolling left from pilot's point of view for %u seconds" % timeout) + self.set_rc(1, 1300) + tstart = self.get_sim_time() + while self.get_sim_time() < (tstart + timeout): + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + delta = (self.get_sim_time() - tstart) + + # stop rolling and yawing + self.set_rc(1, 1500) + self.set_rc(4, 1500) + + # restore simple mode parameters to default + self.mavproxy.send('param set SUPER_SIMPLE 0\n') + + # hover in place + self.hover() + return not failed + + # fly_circle - flies a circle with 20m radius + def fly_circle(self, maxaltchange=10, holdtime=36): + + # hold position in loiter + self.mavproxy.send('switch 5\n') # loiter mode + self.wait_mode('LOITER') + + # face west + progress("turn west") + self.set_rc(4, 1580) + if not self.wait_heading(270): + return False + self.set_rc(4, 1500) + + # set CIRCLE radius + self.mavproxy.send('param set CIRCLE_RADIUS 3000\n') + + # fly forward (east) at least 100m + self.set_rc(2, 1100) + if not self.wait_distance(100): + return False + + # return pitch stick back to middle + self.set_rc(2, 1500) + + # set CIRCLE mode + self.mavproxy.send('switch 1\n') # circle mode + self.wait_mode('CIRCLE') + + # wait + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + start_altitude = m.alt + tstart = self.get_sim_time() + tholdstart = self.get_sim_time() + progress("Circle at %u meters for %u seconds" % (start_altitude, holdtime)) + while self.get_sim_time() < tstart + holdtime: + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + progress("heading %u" % m.heading) + + progress("CIRCLE OK for %u seconds" % holdtime) + return True + + # fly_auto_test - fly mission which tests a significant number of commands + def fly_auto_test(self): + + # Fly mission #1 + progress("# Load copter_mission") + # load the waypoint count + global num_wp + num_wp = self.load_mission_from_file(os.path.join(testdir, "copter_mission.txt")) + if not num_wp: + progress("load copter_mission failed") + return False + + progress("test: Fly a mission from 1 to %u" % num_wp) + self.mavproxy.send('wp set 1\n') + + # switch into AUTO mode and raise throttle + self.mavproxy.send('switch 4\n') # auto mode + self.wait_mode('AUTO') + self.set_rc(3, 1500) + + # fly the mission + ret = self.wait_waypoint(0, num_wp-1, timeout=500) + + # land if mission failed + if ret is False: + self.land() + + # set throttle to minimum + self.set_rc(3, 1000) # wait for disarm - mav.motors_disarmed_wait() + self.mav.motors_disarmed_wait() + progress("MOTORS DISARMED OK") - if not log_download(mavproxy, mav, util.reltopdir("../buildlogs/ArduCopter-log.bin")): - failed_test_msg = "log_download failed" - progress(failed_test_msg) + progress("Auto mission completed: passed=%s" % ret) + + return ret + + # fly_avc_test - fly AVC mission + def fly_avc_test(self): + + # upload mission from file + progress("# Load copter_AVC2013_mission") + # load the waypoint count + global num_wp + num_wp = self.load_mission_from_file(os.path.join(testdir, "copter_AVC2013_mission.txt")) + if not num_wp: + progress("load copter_AVC2013_mission failed") + return False + + progress("Fly AVC mission from 1 to %u" % num_wp) + self.mavproxy.send('wp set 1\n') + + # wait for motor runup + self.wait_seconds(20) + + # switch into AUTO mode and raise throttle + self.mavproxy.send('switch 4\n') # auto mode + self.wait_mode('AUTO') + self.set_rc(3, 1500) + + # fly the mission + ret = self.wait_waypoint(0, num_wp-1, timeout=500) + + # set throttle to minimum + self.set_rc(3, 1000) + + # wait for disarm + self.mav.motors_disarmed_wait() + progress("MOTORS DISARMED OK") + + progress("AVC mission completed: passed=%s" % ret) + + return ret + + def fly_mission(self, height_accuracy=-1.0, target_altitude=None): + """Fly a mission from a file.""" + global num_wp + progress("test: Fly a mission from 1 to %u" % num_wp) + self.mavproxy.send('wp set 1\n') + self.mavproxy.send('switch 4\n') # auto mode + self.wait_mode('AUTO') + ret = self.wait_waypoint(0, num_wp-1, timeout=500) + progress("test: MISSION COMPLETE: passed=%s" % ret) + # wait here until ready + self.mavproxy.send('switch 5\n') # loiter mode + self.wait_mode('LOITER') + return ret + + def autotest(self): + """Autotest ArduCopter in SITL.""" + self.frame = '+' + if not self.hasInit: + self.init() + + failed = False + failed_test_msg = "None" + + try: + progress("Waiting for a heartbeat with mavlink protocol %s" % self.mav.WIRE_PROTOCOL_VERSION) + self.mav.wait_heartbeat() + progress("Setting up RC parameters") + self.set_rc_default() + self.set_rc(3, 1000) + self.homeloc = self.mav.location() + progress("Home location: %s" % self.homeloc) + self.mavproxy.send('switch 6\n') # stabilize mode + self.mav.wait_heartbeat() + self.wait_mode('STABILIZE') + progress("Waiting reading for arm") + self.wait_ready_to_arm() + + # Arm + progress("# Arm motors") + if not self.arm_vehicle(): + failed_test_msg = "arm_motors failed" + progress(failed_test_msg) + failed = True + + progress("# Takeoff") + if not self.takeoff(10): + failed_test_msg = "takeoff failed" + progress(failed_test_msg) + failed = True + + # Fly a square in Stabilize mode + progress("#") + progress("########## Fly a square and save WPs with CH7 switch ##########") + progress("#") + if not self.fly_square(): + failed_test_msg = "fly_square failed" + progress(failed_test_msg) + failed = True + + # save the stored mission to file + progress("# Save out the CH7 mission to file") + global num_wp + num_wp = self.save_mission_to_file(os.path.join(testdir, "ch7_mission.txt")) + if not num_wp: + failed_test_msg = "save_mission_to_file failed" + progress(failed_test_msg) + failed = True + + # fly the stored mission + progress("# Fly CH7 saved mission") + if not self.fly_mission(height_accuracy=0.5, target_altitude=10): + failed_test_msg = "fly ch7_mission failed" + progress(failed_test_msg) + failed = True + + # Throttle Failsafe + progress("#") + progress("########## Test Failsafe ##########") + progress("#") + if not self.fly_throttle_failsafe(): + failed_test_msg = "fly_throttle_failsafe failed" + progress(failed_test_msg) + failed = True + + # Takeoff + progress("# Takeoff") + if not self.takeoff(10): + failed_test_msg = "takeoff failed" + progress(failed_test_msg) + failed = True + + # Battery failsafe + if not self.fly_battery_failsafe(): + failed_test_msg = "fly_battery_failsafe failed" + progress(failed_test_msg) + failed = True + + # Takeoff + progress("# Takeoff") + if not self.takeoff(10): + failed_test_msg = "takeoff failed" + progress(failed_test_msg) + failed = True + + # Stability patch + progress("#") + progress("########## Test Stability Patch ##########") + progress("#") + if not self.fly_stability_patch(30): + failed_test_msg = "fly_stability_patch failed" + progress(failed_test_msg) + failed = True + + # RTL + progress("# RTL #") + if not self.fly_RTL(): + failed_test_msg = "fly_RTL after stab patch failed" + progress(failed_test_msg) + failed = True + + # Takeoff + progress("# Takeoff") + if not self.takeoff(10): + failed_test_msg = "takeoff failed" + progress(failed_test_msg) + failed = True + + # Fence test + progress("#") + progress("########## Test Horizontal Fence ##########") + progress("#") + if not self.fly_fence_test(180): + failed_test_msg = "fly_fence_test failed" + progress(failed_test_msg) + failed = True + + # Fence test + progress("#") + progress("########## Test Max Alt Fence ##########") + progress("#") + if not self.fly_alt_max_fence_test(180): + failed_test_msg = "fly_alt_max_fence_test failed" + progress(failed_test_msg) + failed = True + + # Takeoff + progress("# Takeoff") + if not self.takeoff(10): + failed_test_msg = "takeoff failed" + progress(failed_test_msg) + failed = True + + # Fly GPS Glitch Loiter test + progress("# GPS Glitch Loiter Test") + if not self.fly_gps_glitch_loiter_test(): + failed_test_msg = "fly_gps_glitch_loiter_test failed" + progress(failed_test_msg) + failed = True + + # RTL after GPS Glitch Loiter test + progress("# RTL #") + if not self.fly_RTL(): + failed_test_msg = "fly_RTL failed" + progress(failed_test_msg) + failed = True + + # Fly GPS Glitch test in auto mode + progress("# GPS Glitch Auto Test") + if not self.fly_gps_glitch_auto_test(): + failed_test_msg = "fly_gps_glitch_auto_test failed" + progress(failed_test_msg) + failed = True + + # take-off ahead of next test + progress("# Takeoff") + if not self.takeoff(10): + failed_test_msg = "takeoff failed" + progress(failed_test_msg) + failed = True + + # Loiter for 10 seconds + progress("#") + progress("########## Test Loiter for 10 seconds ##########") + progress("#") + if not self.loiter(): + failed_test_msg = "loiter failed" + progress(failed_test_msg) + failed = True + + # Loiter Climb + progress("#") + progress("# Loiter - climb to 30m") + progress("#") + if not self.change_alt(30): + failed_test_msg = "change_alt climb failed" + progress(failed_test_msg) + failed = True + + # Loiter Descend + progress("#") + progress("# Loiter - descend to 20m") + progress("#") + if not self.change_alt(20): + failed_test_msg = "change_alt descend failed" + progress(failed_test_msg) + failed = True + + # RTL + progress("#") + progress("########## Test RTL ##########") + progress("#") + if not self.fly_RTL(): + failed_test_msg = "fly_RTL after Loiter climb/descend failed" + progress(failed_test_msg) + failed = True + + # Takeoff + progress("# Takeoff") + if not self.takeoff(10): + failed_test_msg = "takeoff failed" + progress(failed_test_msg) + failed = True + + # Simple mode + progress("# Fly in SIMPLE mode") + if not self.fly_simple(): + failed_test_msg = "fly_simple failed" + progress(failed_test_msg) + failed = True + + # RTL + progress("#") + progress("########## Test RTL ##########") + progress("#") + if not self.fly_RTL(): + failed_test_msg = "fly_RTL after simple mode failed" + progress(failed_test_msg) + failed = True + + # Takeoff + progress("# Takeoff") + if not self.takeoff(10): + failed_test_msg = "takeoff failed" + progress(failed_test_msg) + failed = True + + # Fly a circle in super simple mode + progress("# Fly a circle in SUPER SIMPLE mode") + if not self.fly_super_simple(): + failed_test_msg = "fly_super_simple failed" + progress(failed_test_msg) + failed = True + + # RTL + progress("# RTL #") + if not self.fly_RTL(): + failed_test_msg = "fly_RTL after super simple mode failed" + progress(failed_test_msg) + failed = True + + # Takeoff + progress("# Takeoff") + if not self.takeoff(10): + failed_test_msg = "takeoff failed" + progress(failed_test_msg) + failed = True + + # Circle mode + progress("# Fly CIRCLE mode") + if not self.fly_circle(): + failed_test_msg = "fly_circle failed" + progress(failed_test_msg) + failed = True + + # RTL + progress("#") + progress("########## Test RTL ##########") + progress("#") + if not self.fly_RTL(): + failed_test_msg = "fly_RTL after circle failed" + progress(failed_test_msg) + failed = True + + progress("# Fly copter mission") + if not self.fly_auto_test(): + failed_test_msg = "fly_auto_test failed" + progress(failed_test_msg) + failed = True + else: + progress("Flew copter mission OK") + + # wait for disarm + self.mav.motors_disarmed_wait() + + if not self.log_download(util.reltopdir("../buildlogs/ArduCopter-log.bin")): + failed_test_msg = "log_download failed" + progress(failed_test_msg) + failed = True + + except pexpect.TIMEOUT as e: + progress("Failed with timeout") failed = True - except pexpect.TIMEOUT as e: - progress("Failed with timeout") - failed = True + self.close() - mav.close() - util.pexpect_close(mavproxy) - util.pexpect_close(sitl) + if failed: + progress("FAILED: %s" % failed_test_msg) + return False + return True - valgrind_log = util.valgrind_log_filepath(binary=binary, model='+') - if os.path.exists(valgrind_log): - os.chmod(valgrind_log, 0o644) - shutil.copy(valgrind_log, util.reltopdir("../buildlogs/ArduCopter-valgrind.log")) + def autotest_heli(self): + """Autotest Helicopter in SITL with AVC2013 mission.""" + self.frame = 'heli' + if not self.hasInit: + self.init() - # [2014/05/07] FC Because I'm doing a cross machine build (source is on host, build is on guest VM) I cannot hard link - # This flag tells me that I need to copy the data out - if copy_tlog: - shutil.copy(logfile, buildlog) + failed = False + failed_test_msg = "None" - if failed: - progress("FAILED: %s" % failed_test_msg) - return False - return True + try: + self.mav.wait_heartbeat() + self.set_rc_default() + self.set_rc(3, 1000) + self.homeloc = self.mav.location() + progress("Lowering rotor speed") + self.set_rc(8, 1000) + self.mavproxy.send('switch 6\n') # stabilize mode + self.wait_mode('STABILIZE') + self.wait_ready_to_arm() -def fly_CopterAVC(binary, viewerip=None, use_map=False, valgrind=False, gdb=False, frame=None, params=None, gdbserver=False, speedup=10): - """Fly ArduCopter in SITL for AVC2013 mission.""" - global homeloc + # Arm + progress("# Arm motors") + if not self.arm_vehicle(): + failed_test_msg = "arm_motors failed" + progress(failed_test_msg) + failed = True - if frame is None: - frame = 'heli' + progress("Raising rotor speed") + self.set_rc(8, 2000) - home = "%f,%f,%u,%u" % (AVCHOME.lat, AVCHOME.lng, AVCHOME.alt, AVCHOME.heading) - sitl = util.start_SITL(binary, wipe=True, model=frame, home=home, speedup=speedup) - mavproxy = util.start_MAVProxy_SITL('ArduCopter') - mavproxy.expect('Received [0-9]+ parameters') + progress("# Fly AVC mission") + if not self.fly_avc_test(): + failed_test_msg = "fly_avc_test failed" + progress(failed_test_msg) + failed = True + else: + progress("Flew AVC mission OK") - # setup test parameters - if params is None: - params = vinfo.options["ArduCopter"]["frames"][frame]["default_params_filename"] - if not isinstance(params, list): - params = [params] - for x in params: - mavproxy.send("param load %s\n" % os.path.join(testdir, x)) - mavproxy.expect('Loaded [0-9]+ parameters') - mavproxy.send("param set LOG_REPLAY 1\n") - mavproxy.send("param set LOG_DISARMED 1\n") - time.sleep(3) + progress("Lowering rotor speed") + self.set_rc(8, 1000) - # reboot with new parameters - util.pexpect_close(mavproxy) - util.pexpect_close(sitl) + # mission includes disarm at end so should be ok to download logs now + if not self.log_download(util.reltopdir("../buildlogs/Helicopter-log.bin")): + failed_test_msg = "log_download failed" + progress(failed_test_msg) + failed = True - sitl = util.start_SITL(binary, model='heli', home=home, speedup=speedup, valgrind=valgrind, gdb=gdb, gdbserver=gdbserver) - options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=5' - if viewerip: - options += ' --out=%s:14550' % viewerip - if use_map: - options += ' --map' - mavproxy = util.start_MAVProxy_SITL('ArduCopter', options=options) - mavproxy.expect('Telemetry log: (\S+)\r\n') - logfile = mavproxy.match.group(1) - progress("LOGFILE %s" % logfile) - - buildlog = util.reltopdir("../buildlogs/CopterAVC-test.tlog") - progress("buildlog=%s" % buildlog) - if os.path.exists(buildlog): - os.unlink(buildlog) - try: - os.link(logfile, buildlog) - except Exception: - pass - - # the received parameters can come before or after the ready to fly message - mavproxy.expect(['Received [0-9]+ parameters', 'Ready to FLY']) - mavproxy.expect(['Received [0-9]+ parameters', 'Ready to FLY']) - - util.expect_setup_callback(mavproxy, expect_callback) - - expect_list_clear() - expect_list_extend([sitl, mavproxy]) - - if use_map: - mavproxy.send('map icon 40.072467969730496 -105.2314389590174\n') - mavproxy.send('map icon 40.072600990533829 -105.23146100342274\n') - - # get a mavlink connection going - try: - mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True) - except Exception as msg: - progress("Failed to start mavlink connection on 127.0.0.1:19550" % msg) - raise - mav.message_hooks.append(message_hook) - mav.idle_hooks.append(idle_hook) - - failed = False - failed_test_msg = "None" - - try: - mav.wait_heartbeat() - set_rc_default(mavproxy) - set_rc(mavproxy, mav, 3, 1000) - homeloc = mav.location() - - progress("Lowering rotor speed") - set_rc(mavproxy, mav, 8, 1000) - - mavproxy.send('switch 6\n') # stabilize mode - wait_mode(mav, 'STABILIZE') - wait_ready_to_arm(mav) - - # Arm - progress("# Arm motors") - if not arm_vehicle(mavproxy, mav): - failed_test_msg = "arm_motors failed" - progress(failed_test_msg) + except pexpect.TIMEOUT as failed_test_msg: + failed_test_msg = "Timeout" failed = True - progress("Raising rotor speed") - set_rc(mavproxy, mav, 8, 2000) + self.close() - progress("# Fly AVC mission") - if not fly_avc_test(mavproxy, mav): - failed_test_msg = "fly_avc_test failed" - progress(failed_test_msg) - failed = True - else: - progress("Flew AVC mission OK") - - progress("Lowering rotor speed") - set_rc(mavproxy, mav, 8, 1000) - - # mission includes disarm at end so should be ok to download logs now - if not log_download(mavproxy, mav, util.reltopdir("../buildlogs/CopterAVC-log.bin")): - failed_test_msg = "log_download failed" - progress(failed_test_msg) - failed = True - - except pexpect.TIMEOUT as failed_test_msg: - failed_test_msg = "Timeout" - failed = True - - mav.close() - util.pexpect_close(mavproxy) - util.pexpect_close(sitl) - - valgrind_log = util.valgrind_log_filepath(binary=binary, model='heli') - if os.path.exists(valgrind_log): - os.chmod(valgrind_log, 0o644) - shutil.copy(valgrind_log, util.reltopdir("../buildlogs/Helicopter-valgrind.log")) - - if failed: - progress("FAILED: %s" % failed_test_msg) - return False - return True + if failed: + progress("FAILED: %s" % failed_test_msg) + return False + return True diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 28bfca026e..ad7a2c24fd 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -14,556 +14,568 @@ from pysim import util # get location of scripts testdir = os.path.dirname(os.path.realpath(__file__)) - - -HOME_LOCATION = '-35.362938,149.165085,585,354' +HOME = mavutil.location(-35.362938, 149.165085, 585, 354) WIND = "0,180,0.2" # speed,direction,variance -homeloc = None +class AutotestPlane(Autotest): + def __init__(self, binary, viewerip=None, use_map=False, valgrind=False, gdb=False, speedup=10, frame=None, params=None, gdbserver=False): + super(AutotestPlane, self).__init__() + self.binary = binary + self.options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=10' + self.viewerip = viewerip + self.use_map = use_map + self.valgrind = valgrind + self.gdb = gdb + self.frame = frame + self.params = params + self.gdbserver = gdbserver -def takeoff(mavproxy, mav): - """Takeoff get to 30m altitude.""" + self.home = "%f,%f,%u,%u" % (HOME.lat, HOME.lng, HOME.alt, HOME.heading) + self.homeloc = None + self.speedup = speedup + self.speedup_default = 10 - wait_ready_to_arm(mav) - arm_vehicle(mavproxy, mav) + self.sitl = None + self.hasInit = False - mavproxy.send('switch 4\n') - wait_mode(mav, 'FBWA') + def init(self): + if self.frame is None: + self.frame = 'plane-elevrev' - # some rudder to counteract the prop torque - set_rc(mavproxy, mav, 4, 1700) + if self.viewerip: + self.options += " --out=%s:14550" % self.viewerip + if self.use_map: + self.options += ' --map' - # some up elevator to keep the tail down - set_rc(mavproxy, mav, 2, 1200) + self.sitl = util.start_SITL(self.binary, wipe=True, model=self.frame, home=self.home, speedup=self.speedup, + defaults_file=os.path.join(testdir, 'default_params/plane-jsbsim.parm'), + valgrind=self.valgrind, gdb=self.gdb, gdbserver=self.gdbserver) + self.mavproxy = util.start_MAVProxy_SITL('ArduPlane', options=self.options) + self.mavproxy.expect('Telemetry log: (\S+)') + logfile = self.mavproxy.match.group(1) + progress("LOGFILE %s" % logfile) - # get it moving a bit first - set_rc(mavproxy, mav, 3, 1300) - mav.recv_match(condition='VFR_HUD.groundspeed>6', blocking=True) + buildlog = util.reltopdir("../buildlogs/ArduPlane-test.tlog") + progress("buildlog=%s" % buildlog) + if os.path.exists(buildlog): + os.unlink(buildlog) + try: + os.link(logfile, buildlog) + except Exception: + pass - # a bit faster again, straighten rudder - set_rc(mavproxy, mav, 3, 1600) - set_rc(mavproxy, mav, 4, 1500) - mav.recv_match(condition='VFR_HUD.groundspeed>12', blocking=True) + self.mavproxy.expect('Received [0-9]+ parameters') - # hit the gas harder now, and give it some more elevator - set_rc(mavproxy, mav, 2, 1100) - set_rc(mavproxy, mav, 3, 2000) + util.expect_setup_callback(self.mavproxy, expect_callback) - # gain a bit of altitude - if not wait_altitude(mav, homeloc.alt+150, homeloc.alt+180, timeout=30): + expect_list_clear() + expect_list_extend([self.sitl, self.mavproxy]) + + progress("Started simulator") + + # get a mavlink connection going + try: + self.mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True) + except Exception as msg: + progress("Failed to start mavlink connection on 127.0.0.1:19550" % msg) + raise + self.mav.message_hooks.append(message_hook) + self.mav.idle_hooks.append(idle_hook) + self.hasInit = True + progress("Ready to start testing!") + + def close(self): + if self.use_map: + self.mavproxy.send("module unload map\n") + self.mavproxy.expect("Unloaded module map") + + self.mav.close() + util.pexpect_close(self.mavproxy) + util.pexpect_close(self.sitl) + + valgrind_log = util.valgrind_log_filepath(binary=self.binary, model=self.frame) + if os.path.exists(valgrind_log): + os.chmod(valgrind_log, 0o644) + shutil.copy(valgrind_log, util.reltopdir("../buildlogs/ArduPlane-valgrind.log")) + + 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) + + 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 + + progress("Flying left circuit") + # do 4 turns + for i in range(0, 4): + # hard left + 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) + progress("Starting leg %u" % i) + if not self.wait_distance(100, accuracy=20): + return False + progress("Circuit complete") + return True + + def fly_RTL(self): + """Fly to home.""" + 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 + progress("RTL Complete") + return True + + def fly_LOITER(self, num_circles=4): + """Loiter where we are.""" + 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 + 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 + progress("Loiter %u circles left" % num_circles) + + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + final_alt = m.alt + 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: + progress("Failed to maintain altitude") + return False + + progress("Completed Loiter OK") + return True + + def fly_CIRCLE(self, num_circles=1): + """Circle where we are.""" + 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 + 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 + progress("CIRCLE %u circles left" % num_circles) + + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + final_alt = m.alt + 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: + progress("Failed to maintain altitude") + return False + + progress("Completed CIRCLE OK") + return True + + def wait_level_flight(self, accuracy=5, timeout=30): + """Wait for level flight.""" + tstart = self.get_sim_time() + 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) + progress("Roll=%.1f Pitch=%.1f" % (roll, pitch)) + if math.fabs(roll) <= accuracy and math.fabs(pitch) <= accuracy: + progress("Attained level flight") + return True + progress("Failed to attain level flight") return False - # level off - set_rc(mavproxy, mav, 2, 1500) - - progress("TAKEOFF COMPLETE") - return True - - -def fly_left_circuit(mavproxy, mav): - """Fly a left circuit, 200m on a side.""" - mavproxy.send('switch 4\n') - wait_mode(mav, 'FBWA') - set_rc(mavproxy, mav, 3, 2000) - if not wait_level_flight(mavproxy, mav): - return False - - progress("Flying left circuit") - # do 4 turns - for i in range(0, 4): - # hard left - progress("Starting turn %u" % i) - set_rc(mavproxy, mav, 1, 1000) - if not wait_heading(mav, 270 - (90*i), accuracy=10): + 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 - set_rc(mavproxy, mav, 1, 1500) - progress("Starting leg %u" % i) - if not wait_distance(mav, 100, accuracy=20): + self.set_rc(2, 1500) + 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 - progress("Circuit complete") - return True + # fly the roll in manual + self.mavproxy.send('switch 6\n') + self.wait_mode('MANUAL') -def fly_RTL(mavproxy, mav): - """Fly to home.""" - progress("Flying home in RTL") - mavproxy.send('switch 2\n') - wait_mode(mav, 'RTL') - if not wait_location(mav, homeloc, accuracy=120, - target_altitude=homeloc.alt+100, height_accuracy=20, - timeout=180): - return False - progress("RTL Complete") - return True + while count > 0: + 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 fly_LOITER(mavproxy, mav, num_circles=4): - """Loiter where we are.""" - progress("Testing LOITER for %u turns" % num_circles) - mavproxy.send('loiter\n') - wait_mode(mav, 'LOITER') - - m = mav.recv_match(type='VFR_HUD', blocking=True) - initial_alt = m.alt - progress("Initial altitude %u\n" % initial_alt) - - while num_circles > 0: - if not wait_heading(mav, 0, accuracy=10, timeout=60): + 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 - if not wait_heading(mav, 180, accuracy=10, timeout=60): + + # fly the loop in manual + self.mavproxy.send('switch 6\n') + self.wait_mode('MANUAL') + + while count > 0: + 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 - num_circles -= 1 - progress("Loiter %u circles left" % num_circles) + self.set_rc(2, 1500) - m = mav.recv_match(type='VFR_HUD', blocking=True) - final_alt = m.alt - progress("Final altitude %u initial %u\n" % (final_alt, initial_alt)) + self.mavproxy.send("mode STABILIZE\n") + self.wait_mode('STABILIZE') - mavproxy.send('mode FBWA\n') - wait_mode(mav, 'FBWA') + count = 1 + while count > 0: + 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 - if abs(final_alt - initial_alt) > 20: - progress("Failed to maintain altitude") - return False - - progress("Completed Loiter OK") - return True - - -def fly_CIRCLE(mavproxy, mav, num_circles=1): - """Circle where we are.""" - progress("Testing CIRCLE for %u turns" % num_circles) - mavproxy.send('mode CIRCLE\n') - wait_mode(mav, 'CIRCLE') - - m = mav.recv_match(type='VFR_HUD', blocking=True) - initial_alt = m.alt - progress("Initial altitude %u\n" % initial_alt) - - while num_circles > 0: - if not wait_heading(mav, 0, accuracy=10, timeout=60): + self.set_rc(1, 1500) + if not self.wait_roll(0, accuracy=5): return False - if not wait_heading(mav, 180, accuracy=10, timeout=60): + + # 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 - num_circles -= 1 - progress("CIRCLE %u circles left" % num_circles) + self.set_rc(2, 1500) - m = mav.recv_match(type='VFR_HUD', blocking=True) - final_alt = m.alt - progress("Final altitude %u initial %u\n" % (final_alt, initial_alt)) + self.mavproxy.send("mode ACRO\n") + self.wait_mode('ACRO') - mavproxy.send('mode FBWA\n') - wait_mode(mav, 'FBWA') + count = 1 + while count > 0: + 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) - if abs(final_alt - initial_alt) > 20: - progress("Failed to maintain altitude") - return False + # back to FBWA + self.mavproxy.send('mode FBWA\n') + self.wait_mode('FBWA') - progress("Completed CIRCLE OK") - return True + self.wait_level_flight() + self.mavproxy.send("mode ACRO\n") + self.wait_mode('ACRO') -def wait_level_flight(mavproxy, mav, accuracy=5, timeout=30): - """Wait for level flight.""" - tstart = get_sim_time(mav) - progress("Waiting for level flight") - set_rc(mavproxy, mav, 1, 1500) - set_rc(mavproxy, mav, 2, 1500) - set_rc(mavproxy, mav, 4, 1500) - while get_sim_time(mav) < tstart + timeout: - m = mav.recv_match(type='ATTITUDE', blocking=True) - roll = math.degrees(m.roll) - pitch = math.degrees(m.pitch) - progress("Roll=%.1f Pitch=%.1f" % (roll, pitch)) - if math.fabs(roll) <= accuracy and math.fabs(pitch) <= accuracy: - progress("Attained level flight") - return True - progress("Failed to attain level flight") - return False + count = 2 + while count > 0: + 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) -def change_altitude(mavproxy, mav, altitude, accuracy=30): - """Get to a given altitude.""" - mavproxy.send('mode FBWA\n') - wait_mode(mav, 'FBWA') - alt_error = mav.messages['VFR_HUD'].alt - altitude - if alt_error > 0: - set_rc(mavproxy, mav, 2, 2000) - else: - set_rc(mavproxy, mav, 2, 1000) - if not wait_altitude(mav, altitude-accuracy/2, altitude+accuracy/2): - return False - set_rc(mavproxy, mav, 2, 1500) - progress("Reached target altitude at %u" % mav.messages['VFR_HUD'].alt) - return wait_level_flight(mavproxy, mav) + # 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) -def axial_left_roll(mavproxy, mav, count=1): - """Fly a left axial roll.""" - # full throttle! - set_rc(mavproxy, mav, 3, 2000) - if not change_altitude(mavproxy, mav, homeloc.alt+300): - return False + # 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) - # fly the roll in manual - mavproxy.send('switch 6\n') - wait_mode(mav, 'MANUAL') + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + initial_alt = m.alt + progress("Initial altitude %u\n" % initial_alt) - while count > 0: - progress("Starting roll") - set_rc(mavproxy, mav, 1, 1000) - if not wait_roll(mav, -150, accuracy=90): - set_rc(mavproxy, mav, 1, 1500) + progress("Flying right circuit") + # do 4 turns + for i in range(0, 4): + # hard left + 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) + progress("Starting leg %u" % i) + if not self.wait_distance(100, accuracy=20): + return False + progress("Circuit complete") + + progress("Flying rudder left circuit") + # do 4 turns + for i in range(0, 4): + # hard left + 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) + progress("Starting leg %u" % i) + if not self.wait_distance(100, accuracy=20): + return False + progress("Circuit complete") + + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + final_alt = m.alt + 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: + progress("Failed to maintain altitude") return False - if not wait_roll(mav, 150, accuracy=90): - set_rc(mavproxy, mav, 1, 1500) + + return self.wait_level_flight() + + def fly_mission(self, filename, height_accuracy=-1, target_altitude=None): + """Fly a mission from a file.""" + 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 wait_roll(mav, 0, accuracy=90): - set_rc(mavproxy, mav, 1, 1500) + if not self.wait_groundspeed(0, 0.5, timeout=60): return False - count -= 1 + self.mavproxy.expect("Auto disarmed") + progress("Mission OK") + return True - # back to FBWA - set_rc(mavproxy, mav, 1, 1500) - mavproxy.send('switch 4\n') - wait_mode(mav, 'FBWA') - set_rc(mavproxy, mav, 3, 1700) - return wait_level_flight(mavproxy, mav) + def autotest(self): + """Autotest ArduPlane in SITL.""" + if not self.hasInit: + self.init() - -def inside_loop(mavproxy, mav, count=1): - """Fly a inside loop.""" - # full throttle! - set_rc(mavproxy, mav, 3, 2000) - if not change_altitude(mavproxy, mav, homeloc.alt+300): - return False - - # fly the loop in manual - mavproxy.send('switch 6\n') - wait_mode(mav, 'MANUAL') - - while count > 0: - progress("Starting loop") - set_rc(mavproxy, mav, 2, 1000) - if not wait_pitch(mav, -60, accuracy=20): - return False - if not wait_pitch(mav, 0, accuracy=20): - return False - count -= 1 - - # back to FBWA - set_rc(mavproxy, mav, 2, 1500) - mavproxy.send('switch 4\n') - wait_mode(mav, 'FBWA') - set_rc(mavproxy, mav, 3, 1700) - return wait_level_flight(mavproxy, mav) - - -def test_stabilize(mavproxy, mav, count=1): - """Fly stabilize mode.""" - # full throttle! - set_rc(mavproxy, mav, 3, 2000) - set_rc(mavproxy, mav, 2, 1300) - if not change_altitude(mavproxy, mav, homeloc.alt+300): - return False - set_rc(mavproxy, mav, 2, 1500) - - mavproxy.send("mode STABILIZE\n") - wait_mode(mav, 'STABILIZE') - - count = 1 - while count > 0: - progress("Starting roll") - set_rc(mavproxy, mav, 1, 2000) - if not wait_roll(mav, -150, accuracy=90): - return False - if not wait_roll(mav, 150, accuracy=90): - return False - if not wait_roll(mav, 0, accuracy=90): - return False - count -= 1 - - set_rc(mavproxy, mav, 1, 1500) - if not wait_roll(mav, 0, accuracy=5): - return False - - # back to FBWA - mavproxy.send('mode FBWA\n') - wait_mode(mav, 'FBWA') - set_rc(mavproxy, mav, 3, 1700) - return wait_level_flight(mavproxy, mav) - - -def test_acro(mavproxy, mav, count=1): - """Fly ACRO mode.""" - # full throttle! - set_rc(mavproxy, mav, 3, 2000) - set_rc(mavproxy, mav, 2, 1300) - if not change_altitude(mavproxy, mav, homeloc.alt+300): - return False - set_rc(mavproxy, mav, 2, 1500) - - mavproxy.send("mode ACRO\n") - wait_mode(mav, 'ACRO') - - count = 1 - while count > 0: - progress("Starting roll") - set_rc(mavproxy, mav, 1, 1000) - if not wait_roll(mav, -150, accuracy=90): - return False - if not wait_roll(mav, 150, accuracy=90): - return False - if not wait_roll(mav, 0, accuracy=90): - return False - count -= 1 - set_rc(mavproxy, mav, 1, 1500) - - # back to FBWA - mavproxy.send('mode FBWA\n') - wait_mode(mav, 'FBWA') - - wait_level_flight(mavproxy, mav) - - mavproxy.send("mode ACRO\n") - wait_mode(mav, 'ACRO') - - count = 2 - while count > 0: - progress("Starting loop") - set_rc(mavproxy, mav, 2, 1000) - if not wait_pitch(mav, -60, accuracy=20): - return False - if not wait_pitch(mav, 0, accuracy=20): - return False - count -= 1 - - set_rc(mavproxy, mav, 2, 1500) - - # back to FBWA - mavproxy.send('mode FBWA\n') - wait_mode(mav, 'FBWA') - set_rc(mavproxy, mav, 3, 1700) - return wait_level_flight(mavproxy, mav) - - -def test_FBWB(mavproxy, mav, count=1, mode='FBWB'): - """Fly FBWB or CRUISE mode.""" - mavproxy.send("mode %s\n" % mode) - wait_mode(mav, mode) - set_rc(mavproxy, mav, 3, 1700) - set_rc(mavproxy, mav, 2, 1500) - - # lock in the altitude by asking for an altitude change then releasing - set_rc(mavproxy, mav, 2, 1000) - wait_distance(mav, 50, accuracy=20) - set_rc(mavproxy, mav, 2, 1500) - wait_distance(mav, 50, accuracy=20) - - m = mav.recv_match(type='VFR_HUD', blocking=True) - initial_alt = m.alt - progress("Initial altitude %u\n" % initial_alt) - - progress("Flying right circuit") - # do 4 turns - for i in range(0, 4): - # hard left - progress("Starting turn %u" % i) - set_rc(mavproxy, mav, 1, 1800) - if not wait_heading(mav, 0 + (90*i), accuracy=20, timeout=60): - set_rc(mavproxy, mav, 1, 1500) - return False - set_rc(mavproxy, mav, 1, 1500) - progress("Starting leg %u" % i) - if not wait_distance(mav, 100, accuracy=20): - return False - progress("Circuit complete") - - progress("Flying rudder left circuit") - # do 4 turns - for i in range(0, 4): - # hard left - progress("Starting turn %u" % i) - set_rc(mavproxy, mav, 4, 1900) - if not wait_heading(mav, 360 - (90*i), accuracy=20, timeout=60): - set_rc(mavproxy, mav, 4, 1500) - return False - set_rc(mavproxy, mav, 4, 1500) - progress("Starting leg %u" % i) - if not wait_distance(mav, 100, accuracy=20): - return False - progress("Circuit complete") - - m = mav.recv_match(type='VFR_HUD', blocking=True) - final_alt = m.alt - progress("Final altitude %u initial %u\n" % (final_alt, initial_alt)) - - # back to FBWA - mavproxy.send('mode FBWA\n') - wait_mode(mav, 'FBWA') - - if abs(final_alt - initial_alt) > 20: - progress("Failed to maintain altitude") - return False - - return wait_level_flight(mavproxy, mav) - - -def fly_mission(mavproxy, mav, filename, height_accuracy=-1, target_altitude=None): - """Fly a mission from a file.""" - global homeloc - progress("Flying mission %s" % filename) - mavproxy.send('wp load %s\n' % filename) - mavproxy.expect('Flight plan received') - mavproxy.send('wp list\n') - mavproxy.expect('Requesting [0-9]+ waypoints') - mavproxy.send('switch 1\n') # auto mode - wait_mode(mav, 'AUTO') - if not wait_waypoint(mav, 1, 7, max_dist=60): - return False - if not wait_groundspeed(mav, 0, 0.5, timeout=60): - return False - mavproxy.expect("Auto disarmed") - progress("Mission OK") - return True - - -def fly_ArduPlane(binary, viewerip=None, use_map=False, valgrind=False, gdb=False, gdbserver=False, speedup=10): - """Fly ArduPlane in SITL. - - you can pass viewerip as an IP address to optionally send fg and - mavproxy packets too for local viewing of the flight in real time - """ - global homeloc - - options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=10' - if viewerip: - options += " --out=%s:14550" % viewerip - if use_map: - options += ' --map' - - sitl = util.start_SITL(binary, model='plane-elevrev', home=HOME_LOCATION, speedup=speedup, - valgrind=valgrind, gdb=gdb, gdbserver=gdbserver, - defaults_file=os.path.join(testdir, 'default_params/plane-jsbsim.parm')) - mavproxy = util.start_MAVProxy_SITL('ArduPlane', options=options) - mavproxy.expect('Telemetry log: (\S+)\r\n') - logfile = mavproxy.match.group(1) - progress("LOGFILE %s" % logfile) - - buildlog = util.reltopdir("../buildlogs/ArduPlane-test.tlog") - progress("buildlog=%s" % buildlog) - if os.path.exists(buildlog): - os.unlink(buildlog) - try: - os.link(logfile, buildlog) - except Exception: - pass - - util.expect_setup_callback(mavproxy, expect_callback) - - mavproxy.expect('Received [0-9]+ parameters') - - expect_list_clear() - expect_list_extend([sitl, mavproxy]) - - progress("Started simulator") - - # get a mavlink connection going - try: - mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True) - except Exception as msg: - progress("Failed to start mavlink connection on 127.0.0.1:19550" % msg) - raise - mav.message_hooks.append(message_hook) - mav.idle_hooks.append(idle_hook) - - failed = False - fail_list = [] - e = 'None' - try: - progress("Waiting for a heartbeat with mavlink protocol %s" % mav.WIRE_PROTOCOL_VERSION) - mav.wait_heartbeat() - progress("Setting up RC parameters") - set_rc_default(mavproxy) - set_rc(mavproxy, mav, 3, 1000) - set_rc(mavproxy, mav, 8, 1800) - progress("Waiting for GPS fix") - mav.recv_match(condition='VFR_HUD.alt>10', blocking=True) - mav.wait_gps_fix() - while mav.location().alt < 10: - mav.wait_gps_fix() - homeloc = mav.location() - progress("Home location: %s" % homeloc) - if not takeoff(mavproxy, mav): - progress("Failed takeoff") + failed = False + fail_list = [] + e = 'None' + try: + progress("Waiting for a heartbeat with mavlink protocol %s" % self.mav.WIRE_PROTOCOL_VERSION) + self.mav.wait_heartbeat() + progress("Setting up RC parameters") + self.set_rc_default() + self.set_rc(3, 1000) + self.set_rc(8, 1800) + 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() + progress("Home location: %s" % self.homeloc) + if not self.takeoff(): + progress("Failed takeoff") + failed = True + fail_list.append("takeoff") + if not self.fly_left_circuit(): + progress("Failed left circuit") + failed = True + fail_list.append("left_circuit") + if not self.axial_left_roll(1): + progress("Failed left roll") + failed = True + fail_list.append("left_roll") + if not self.inside_loop(): + progress("Failed inside loop") + failed = True + fail_list.append("inside_loop") + if not self.test_stabilize(): + progress("Failed stabilize test") + failed = True + fail_list.append("stabilize") + if not self.test_acro(): + progress("Failed ACRO test") + failed = True + fail_list.append("acro") + if not self.test_FBWB(): + progress("Failed FBWB test") + failed = True + fail_list.append("fbwb") + if not self.test_FBWB(mode='CRUISE'): + progress("Failed CRUISE test") + failed = True + fail_list.append("cruise") + if not self.fly_RTL(): + progress("Failed RTL") + failed = True + fail_list.append("RTL") + if not self.fly_LOITER(): + progress("Failed LOITER") + failed = True + fail_list.append("LOITER") + if not self.fly_CIRCLE(): + 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): + progress("Failed mission") + failed = True + fail_list.append("mission") + if not self.log_download(util.reltopdir("../buildlogs/ArduPlane-log.bin")): + progress("Failed log download") + failed = True + fail_list.append("log_download") + except pexpect.TIMEOUT as e: + progress("Failed with timeout") failed = True - fail_list.append("takeoff") - if not fly_left_circuit(mavproxy, mav): - progress("Failed left circuit") - failed = True - fail_list.append("left_circuit") - if not axial_left_roll(mavproxy, mav, 1): - progress("Failed left roll") - failed = True - fail_list.append("left_roll") - if not inside_loop(mavproxy, mav): - progress("Failed inside loop") - failed = True - fail_list.append("inside_loop") - if not test_stabilize(mavproxy, mav): - progress("Failed stabilize test") - failed = True - fail_list.append("stabilize") - if not test_acro(mavproxy, mav): - progress("Failed ACRO test") - failed = True - fail_list.append("acro") - if not test_FBWB(mavproxy, mav): - progress("Failed FBWB test") - failed = True - fail_list.append("fbwb") - if not test_FBWB(mavproxy, mav, mode='CRUISE'): - progress("Failed CRUISE test") - failed = True - fail_list.append("cruise") - if not fly_RTL(mavproxy, mav): - progress("Failed RTL") - failed = True - fail_list.append("RTL") - if not fly_LOITER(mavproxy, mav): - progress("Failed LOITER") - failed = True - fail_list.append("LOITER") - if not fly_CIRCLE(mavproxy, mav): - progress("Failed CIRCLE") - failed = True - fail_list.append("LOITER") - if not fly_mission(mavproxy, mav, os.path.join(testdir, "ap1.txt"), height_accuracy=10, - target_altitude=homeloc.alt+100): - progress("Failed mission") - failed = True - fail_list.append("mission") - if not log_download(mavproxy, mav, util.reltopdir("../buildlogs/ArduPlane-log.bin")): - progress("Failed log download") - failed = True - fail_list.append("log_download") - except pexpect.TIMEOUT as e: - progress("Failed with timeout") - failed = True - fail_list.append("timeout") + fail_list.append("timeout") - mav.close() - util.pexpect_close(mavproxy) - util.pexpect_close(sitl) + self.close() - valgrind_log = util.valgrind_log_filepath(binary=binary, model='plane-elevrev') - if os.path.exists(valgrind_log): - os.chmod(valgrind_log, 0o644) - shutil.copy(valgrind_log, util.reltopdir("../buildlogs/ArduPlane-valgrind.log")) - - if failed: - progress("FAILED: %s" % e) - progress("Fail list: %s" % fail_list) - return False - return True + if failed: + progress("FAILED: %s" % e) + progress("Fail list: %s" % fail_list) + return False + return True diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py index 93d6c2a7e0..19f59c7fbf 100644 --- a/Tools/autotest/ardusub.py +++ b/Tools/autotest/ardusub.py @@ -10,179 +10,216 @@ from pymavlink import mavutil from common import * from pysim import util +from pysim import vehicleinfo # get location of scripts testdir = os.path.dirname(os.path.realpath(__file__)) - - HOME = mavutil.location(33.810313, -118.393867, 0, 185) -homeloc = None -def dive_manual(mavproxy, mav): - set_rc(mavproxy, mav, 3, 1600) - set_rc(mavproxy, mav, 5, 1600) - set_rc(mavproxy, mav, 6, 1550) +class AutotestSub(Autotest): + def __init__(self, binary, viewerip=None, use_map=False, valgrind=False, gdb=False, speedup=10, frame=None, params=None, gdbserver=False): + super(AutotestSub, self).__init__() + self.binary = binary + self.options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=10' + self.viewerip = viewerip + self.use_map = use_map + self.valgrind = valgrind + self.gdb = gdb + self.frame = frame + self.params = params + self.gdbserver = gdbserver - if not wait_distance(mav, 50, accuracy=7, timeout=200): - return False - - set_rc(mavproxy, mav, 4, 1550) - - if not wait_heading(mav, 0): - return False - - set_rc(mavproxy, mav, 4, 1500) - - if not wait_distance(mav, 50, accuracy=7, timeout=100): - return False - - set_rc(mavproxy, mav, 4, 1550) - - if not wait_heading(mav, 0): - return False - - set_rc(mavproxy, mav, 4, 1500) - set_rc(mavproxy, mav, 5, 1500) - set_rc(mavproxy, mav, 6, 1100) - - if not wait_distance(mav, 75, accuracy=7, timeout=100): - return False + self.home = "%f,%f,%u,%u" % (HOME.lat, HOME.lng, HOME.alt, HOME.heading) + self.homeloc = None + self.speedup = speedup + self.speedup_default = 10 - set_rc_default(mavproxy) + self.sitl = None + self.hasInit = False - disarm_vehicle(mavproxy, mav) - progress("Manual dive OK") - return True + def init(self): + if self.frame is None: + self.frame = 'vectored' + if self.viewerip: + self.options += " --out=%s:14550" % self.viewerip + if self.use_map: + self.options += ' --map' -def dive_mission(mavproxy, mav, filename): - - progress("Executing mission %s" % filename) - mavproxy.send('wp load %s\n' % filename) - mavproxy.expect('Flight plan received') - mavproxy.send('wp list\n') - mavproxy.expect('Saved [0-9]+ waypoints') - set_rc_default(mavproxy) - - if not arm_vehicle(mavproxy, mav): - progress("Failed to ARM") - return False - - mavproxy.send('mode auto\n') - wait_mode(mav, 'AUTO') - - if not wait_waypoint(mav, 1, 5, max_dist=5): - return False + self.sitl = util.start_SITL(self.binary, wipe=True, model=self.frame, home=self.home, + speedup=self.speedup_default) + self.mavproxy = util.start_MAVProxy_SITL('ArduSub') - disarm_vehicle(mavproxy, mav) + progress("WAITING FOR PARAMETERS") + self.mavproxy.expect('Received [0-9]+ parameters') - progress("Mission OK") - return True + # setup test parameters + vinfo = vehicleinfo.VehicleInfo() + if self.params is None: + self.params = vinfo.options["ArduSub"]["frames"][self.frame]["default_params_filename"] + if not isinstance(self.params, list): + self.params = [self.params] + for x in self.params: + self.mavproxy.send("param load %s\n" % os.path.join(testdir, x)) + self.mavproxy.expect('Loaded [0-9]+ parameters') + self.set_parameter('LOG_REPLAY', 1) + self.set_parameter('LOG_DISARMED', 1) + progress("RELOADING SITL WITH NEW PARAMETERS") + # restart with new parms + util.pexpect_close(self.mavproxy) + util.pexpect_close(self.sitl) -def dive_ArduSub(binary, viewerip=None, use_map=False, valgrind=False, gdb=False, gdbserver=False, speedup=10): - """Dive ArduSub in SITL. + self.sitl = util.start_SITL(self.binary, model=self.frame, home=self.home, speedup=self.speedup, + valgrind=self.valgrind, gdb=self.gdb, gdbserver=self.gdbserver) + self.mavproxy = util.start_MAVProxy_SITL('ArduSub', options=self.options) + self.mavproxy.expect('Telemetry log: (\S+)') + logfile = self.mavproxy.match.group(1) + progress("LOGFILE %s" % logfile) - you can pass viewerip as an IP address to optionally send fg and - mavproxy packets too for local viewing of the mission in real time - """ - options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=10' - if viewerip: - options += " --out=%s:14550" % viewerip - if use_map: - options += ' --map' + buildlog = util.reltopdir("../buildlogs/ArduSub-test.tlog") + progress("buildlog=%s" % buildlog) + if os.path.exists(buildlog): + os.unlink(buildlog) + try: + os.link(logfile, buildlog) + except Exception: + pass - home = "%f,%f,%u,%u" % (HOME.lat, HOME.lng, HOME.alt, HOME.heading) - sitl = util.start_SITL(binary, model='vectored', wipe=True, home=home, speedup=speedup) - mavproxy = util.start_MAVProxy_SITL('ArduSub') - mavproxy.expect('Received [0-9]+ parameters') + self.mavproxy.expect('Received [0-9]+ parameters') - # setup test parameters - mavproxy.send("param load %s/default_params/sub.parm\n" % testdir) - mavproxy.expect('Loaded [0-9]+ parameters') - mavproxy.send('param set FS_GCS_ENABLE 0\n') - mavproxy.send("param set LOG_REPLAY 1\n") - mavproxy.send("param set LOG_DISARMED 1\n") - time.sleep(3) + util.expect_setup_callback(self.mavproxy, expect_callback) - # reboot with new parameters - util.pexpect_close(mavproxy) - util.pexpect_close(sitl) + expect_list_clear() + expect_list_extend([self.sitl, self.mavproxy]) - sitl = util.start_SITL(binary, model='vectored', home=home, speedup=speedup, valgrind=valgrind, gdb=gdb, gdbserver=gdbserver) - mavproxy = util.start_MAVProxy_SITL('ArduSub', options=options) - mavproxy.expect('Telemetry log: (\S+)\r\n') - logfile = mavproxy.match.group(1) - progress("LOGFILE %s" % logfile) + progress("Started simulator") - buildlog = util.reltopdir("../buildlogs/ArduSub-test.tlog") - progress("buildlog=%s" % buildlog) - if os.path.exists(buildlog): - os.unlink(buildlog) - try: - os.link(logfile, buildlog) - except Exception: - pass + # get a mavlink connection going + try: + self.mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True) + except Exception as msg: + progress("Failed to start mavlink connection on 127.0.0.1:19550" % msg) + raise + self.mav.message_hooks.append(message_hook) + self.mav.idle_hooks.append(idle_hook) + self.hasInit = True + progress("Ready to start testing!") - mavproxy.expect('Received [0-9]+ parameters') + def close(self): + if self.use_map: + self.mavproxy.send("module unload map\n") + self.mavproxy.expect("Unloaded module map") - util.expect_setup_callback(mavproxy, expect_callback) + self.mav.close() + util.pexpect_close(self.mavproxy) + util.pexpect_close(self.sitl) - expect_list_clear() - expect_list_extend([sitl, mavproxy]) + valgrind_log = util.valgrind_log_filepath(binary=self.binary, model=self.frame) + if os.path.exists(valgrind_log): + os.chmod(valgrind_log, 0o644) + shutil.copy(valgrind_log, util.reltopdir("../buildlogs/ArduSub-valgrind.log")) - progress("Started simulator") + def dive_manual(self): + self.set_rc(3, 1600) + self.set_rc(5, 1600) + self.set_rc(6, 1550) - # get a mavlink connection going - try: - mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True) - except Exception as msg: - progress("Failed to start mavlink connection on 127.0.0.1:19550" % msg) - raise - mav.message_hooks.append(message_hook) - mav.idle_hooks.append(idle_hook) + if not self.wait_distance(50, accuracy=7, timeout=200): + return False - failed = False - e = 'None' - try: - progress("Waiting for a heartbeat with mavlink protocol %s" % mav.WIRE_PROTOCOL_VERSION) - mav.wait_heartbeat() - progress("Waiting for GPS fix") - mav.wait_gps_fix() - - # wait for EKF and GPS checks to pass - mavproxy.expect('IMU0 is using GPS') - - homeloc = mav.location() - progress("Home location: %s" % homeloc) - set_rc_default(mavproxy) - if not arm_vehicle(mavproxy, mav): + self.set_rc(4, 1550) + + if not self.wait_heading(0): + return False + + self.set_rc(4, 1500) + + if not self.wait_distance(50, accuracy=7, timeout=100): + return False + + self.set_rc(4, 1550) + + if not self.wait_heading(0): + return False + + self.set_rc(4, 1500) + self.set_rc(5, 1500) + self.set_rc(6, 1100) + + if not self.wait_distance(75, accuracy=7, timeout=100): + return False + + self.set_rc_default() + + self.disarm_vehicle() + progress("Manual dive OK") + return True + + def dive_mission(self, filename): + + progress("Executing 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('Saved [0-9]+ waypoints') + self.set_rc_default() + + if not self.arm_vehicle(): progress("Failed to ARM") - failed = True - if not dive_manual(mavproxy, mav): - progress("Failed manual dive") - failed = True - if not dive_mission(mavproxy, mav, os.path.join(testdir, "sub_mission.txt")): - progress("Failed auto mission") - failed = True - if not log_download(mavproxy, mav, util.reltopdir("../buildlogs/ArduSub-log.bin")): - progress("Failed log download") - failed = True - except pexpect.TIMEOUT as e: - progress("Failed with timeout") - failed = True + return False - mav.close() - util.pexpect_close(mavproxy) - util.pexpect_close(sitl) + self.mavproxy.send('mode auto\n') + self.wait_mode('AUTO') - valgrind_log = util.valgrind_log_filepath(binary=binary, model='sub') - if os.path.exists(valgrind_log): - os.chmod(valgrind_log, 0o644) - shutil.copy(valgrind_log, util.reltopdir("../buildlogs/APMrover2-valgrind.log")) + if not self.wait_waypoint(1, 5, max_dist=5): + return False - if failed: - progress("FAILED: %s" % e) - return False - return True + self.disarm_vehicle() + + progress("Mission OK") + return True + + def autotest(self): + """Autotest ArduSub in SITL.""" + if not self.hasInit: + self.init() + + failed = False + e = 'None' + try: + progress("Waiting for a heartbeat with mavlink protocol %s" % self.mav.WIRE_PROTOCOL_VERSION) + self.mav.wait_heartbeat() + self.mavproxy.send('param set FS_GCS_ENABLE 0\n') + progress("Waiting for GPS fix") + self.mav.wait_gps_fix() + + # wait for EKF and GPS checks to pass + self.mavproxy.expect('IMU0 is using GPS') + + self.homeloc = self.mav.location() + progress("Home location: %s" % self.homeloc) + self.set_rc_default() + if not self.arm_vehicle(): + progress("Failed to ARM") + failed = True + if not self.dive_manual(): + progress("Failed manual dive") + failed = True + if not self.dive_mission(os.path.join(testdir, "sub_mission.txt")): + progress("Failed auto mission") + failed = True + if not self.log_download(util.reltopdir("../buildlogs/ArduSub-log.bin")): + progress("Failed log download") + failed = True + except pexpect.TIMEOUT as e: + progress("Failed with timeout") + failed = True + + self.close() + + if failed: + progress("FAILED: %s" % e) + return False + return True diff --git a/Tools/autotest/autotest.py b/Tools/autotest/autotest.py index 4f7f908063..bb66d2fc4f 100755 --- a/Tools/autotest/autotest.py +++ b/Tools/autotest/autotest.py @@ -15,11 +15,12 @@ import sys import time import traceback -import apmrover2 -import arducopter -import arduplane -import quadplane -import ardusub +from apmrover2 import * +from arducopter import * +from quadplane import * +from arduplane import * +from ardusub import * + from pysim import util from pymavlink import mavutil from pymavlink.generator import mavtemplate @@ -260,22 +261,28 @@ def run_step(step): fly_opts["speedup"] = opts.speedup if step == 'fly.ArduCopter': - return arducopter.fly_ArduCopter(binary, frame=opts.frame, **fly_opts) + arducopter = AutotestCopter(binary, frame=opts.frame, **fly_opts) + return arducopter.autotest() if step == 'fly.CopterAVC': - return arducopter.fly_CopterAVC(binary, **fly_opts) + arducopter = AutotestCopter(binary, **fly_opts) + return arducopter.autotest_heli() if step == 'fly.ArduPlane': - return arduplane.fly_ArduPlane(binary, **fly_opts) + arduplane = AutotestPlane(binary, **fly_opts) + return arduplane.autotest() if step == 'fly.QuadPlane': - return quadplane.fly_QuadPlane(binary, **fly_opts) + quadplane = AutotestQuadPlane(binary, **fly_opts) + return quadplane.autotest() if step == 'drive.APMrover2': - return apmrover2.drive_APMrover2(binary, frame=opts.frame, **fly_opts) + apmrover2 = AutotestRover(binary, frame=opts.frame, **fly_opts) + return apmrover2.autotest() if step == 'dive.ArduSub': - return ardusub.dive_ArduSub(binary, **fly_opts) + ardusub = AutotestSub(binary, **fly_opts) + return ardusub.autotest() if step == 'build.All': return build_all() diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 9bb290072d..9663d9f26d 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -6,10 +6,28 @@ from pymavlink import mavwp, mavutil from pysim import util +import sys +import abc +import os + # a list of pexpect objects to read while waiting for # messages. This keeps the output to stdout flowing expect_list = [] +# get location of scripts +testdir = os.path.dirname(os.path.realpath(__file__)) + +# Check python version for abstract base class +if sys.version_info[0] >= 3 and sys.version_info[1] >= 4: + ABC = abc.ABC +else: + ABC = abc.ABCMeta('ABC', (), {}) + + +def progress(text): + """Display autotest progress text.""" + print("AUTOTEST: " + text) + class AutoTestTimeoutException(Exception): pass @@ -52,393 +70,530 @@ def expect_callback(e): util.pexpect_drain(p) -################################################# -# SIM UTILITIES -################################################# -def progress(text): - """Display autotest progress text.""" - print("AUTOTEST: " + text) +class Autotest(ABC): + """Base abstract class. + It implements the common function for all vehicle types. + """ + def __init__(self): + self.mavproxy = None + self.mav = None + ################################################# + # SIM UTILITIES + ################################################# + def get_sim_time(self): + """Get SITL time.""" + m = self.mav.recv_match(type='SYSTEM_TIME', blocking=True) + return m.time_boot_ms * 1.0e-3 -def get_sim_time(mav): - """Get SITL time.""" - m = mav.recv_match(type='SYSTEM_TIME', blocking=True) - return m.time_boot_ms * 1.0e-3 + def sim_location(self): + """Return current simulator location.""" + m = self.mav.recv_match(type='SIMSTATE', blocking=True) + return mavutil.location(m.lat*1.0e-7, m.lng*1.0e-7, 0, math.degrees(m.yaw)) + def save_wp(self): + """Trigger RC 7 to save waypoint.""" + self.mavproxy.send('rc 7 1000\n') + self.mav.recv_match(condition='RC_CHANNELS.chan7_raw==1000', blocking=True) + self.wait_seconds(1) + self.mavproxy.send('rc 7 2000\n') + self.mav.recv_match(condition='RC_CHANNELS.chan7_raw==2000', blocking=True) + self.wait_seconds(1) + self.mavproxy.send('rc 7 1000\n') + self.mav.recv_match(condition='RC_CHANNELS.chan7_raw==1000', blocking=True) + self.wait_seconds(1) -def sim_location(mav): - """Return current simulator location.""" - m = mav.recv_match(type='SIMSTATE', blocking=True) - return mavutil.location(m.lat*1.0e-7, m.lng*1.0e-7, 0, math.degrees(m.yaw)) + def log_download(self, filename, timeout=360): + """Download latest log.""" + self.disarm_vehicle() + self.mav.wait_heartbeat() + self.mavproxy.send("log list\n") + self.mavproxy.expect("numLogs") + self.mav.wait_heartbeat() + self.mav.wait_heartbeat() + self.mavproxy.send("set shownoise 0\n") + self.mavproxy.send("log download latest %s\n" % filename) + self.mavproxy.expect("Finished downloading", timeout=timeout) + self.mav.wait_heartbeat() + self.mav.wait_heartbeat() + return True + def show_gps_and_sim_positions(self, on_off): + """Allow to display gps and actual position on map.""" + if on_off is True: + # turn on simulator display of gps and actual position + self.mavproxy.send('map set showgpspos 1\n') + self.mavproxy.send('map set showsimpos 1\n') + else: + # turn off simulator display of gps and actual position + self.mavproxy.send('map set showgpspos 0\n') + self.mavproxy.send('map set showsimpos 0\n') -def save_wp(mavproxy, mav): - """Trigger RC 7 to save waypoint.""" - mavproxy.send('rc 7 1000\n') - mav.recv_match(condition='RC_CHANNELS.chan7_raw==1000', blocking=True) - wait_seconds(mav, 1) - mavproxy.send('rc 7 2000\n') - mav.recv_match(condition='RC_CHANNELS.chan7_raw==2000', blocking=True) - wait_seconds(mav, 1) - mavproxy.send('rc 7 1000\n') - mav.recv_match(condition='RC_CHANNELS.chan7_raw==1000', blocking=True) - wait_seconds(mav, 1) + def mission_count(self, filename): + """Load a mission from a file and return number of waypoints.""" + wploader = mavwp.MAVWPLoader() + wploader.load(filename) + num_wp = wploader.count() + return num_wp + def load_mission_from_file(self, filename): + """Load a mission from a file to flight controller.""" + 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') -def log_download(mavproxy, mav, filename, timeout=360): - """Download latest log.""" - mavproxy.send("log list\n") - mavproxy.expect("numLogs") - mav.wait_heartbeat() - mav.wait_heartbeat() - mavproxy.send("set shownoise 0\n") - mavproxy.send("log download latest %s\n" % filename) - mavproxy.expect("Finished downloading", timeout=timeout) - mav.wait_heartbeat() - mav.wait_heartbeat() - return True + # update num_wp + wploader = mavwp.MAVWPLoader() + wploader.load(filename) + num_wp = wploader.count() + return num_wp + def save_mission_to_file(self, filename): + """Save a mission to a file""" + self.mavproxy.send('wp save %s\n' % filename) + self.mavproxy.expect('Saved ([0-9]+) waypoints') + num_wp = int(self.mavproxy.match.group(1)) + progress("num_wp: %d" % num_wp) + return num_wp -def show_gps_and_sim_positions(mavproxy, on_off): - """Allow to display gps and actual position on map.""" - if on_off is True: - # turn on simulator display of gps and actual position - mavproxy.send('map set showgpspos 1\n') - mavproxy.send('map set showsimpos 1\n') - else: - # turn off simulator display of gps and actual position - mavproxy.send('map set showgpspos 0\n') - mavproxy.send('map set showsimpos 0\n') + def set_rc_default(self): + """Setup all simulated RC control to 1500.""" + for chan in range(1, 16): + self.mavproxy.send('rc %u 1500\n' % chan) - -def mission_count(filename): - """Load a mission from a file and return number of waypoints.""" - wploader = mavwp.MAVWPLoader() - wploader.load(filename) - num_wp = wploader.count() - return num_wp - - -def load_mission_from_file(mavproxy, filename): - """Load a mission from a file to flight controller.""" - mavproxy.send('wp load %s\n' % filename) - mavproxy.expect('Flight plan received') - mavproxy.send('wp list\n') - mavproxy.expect('Requesting [0-9]+ waypoints') - - # update num_wp - wploader = mavwp.MAVWPLoader() - wploader.load(filename) - num_wp = wploader.count() - return num_wp - - -def save_mission_to_file(mavproxy, filename): - """Save a mission to a file""" - mavproxy.send('wp save %s\n' % filename) - mavproxy.expect('Saved ([0-9]+) waypoints') - num_wp = int(mavproxy.match.group(1)) - progress("num_wp: %d" % num_wp) - return num_wp - - -def set_rc_default(mavproxy): - """Setup all simulated RC control to 1500.""" - for chan in range(1, 16): - mavproxy.send('rc %u 1500\n' % chan) - - -def set_rc(mavproxy, mav, chan, pwm, timeout=5): - """Setup a simulated RC control to a PWM value""" - tstart = get_sim_time(mav) - while get_sim_time(mav) < tstart + timeout: - mavproxy.send('rc %u %u\n' % (chan, pwm)) - m = mav.recv_match(type='RC_CHANNELS', blocking=True) - chan_pwm = getattr(m, "chan" + str(chan) + "_raw") - if chan_pwm == pwm: - return True - progress("Failed to send RC commands") - return False - - -def arm_vehicle(mavproxy, mav): - """Arm vehicle with mavlink arm message.""" - mavproxy.send('arm throttle\n') - mav.motors_armed_wait() - progress("ARMED") - return True - - -def disarm_vehicle(mavproxy, mav): - """Disarm vehicle with mavlink disarm message.""" - mavproxy.send('disarm\n') - mav.motors_disarmed_wait() - progress("DISARMED") - return True - - -def set_parameter(mavproxy, name, value): - for i in range(1, 10): - mavproxy.send("param set %s %s\n" % (name, str(value))) - mavproxy.send("param fetch %s\n" % (name)) - mavproxy.expect("%s = (.*)" % (name,)) - returned_value = mavproxy.match.group(1) - if float(returned_value) == float(value): - # yes, exactly equal. - break - progress("Param fetch returned incorrect value (%s) vs (%s)" % (returned_value, value)) - - -def get_parameter(mavproxy, name): - mavproxy.send("param fetch %s\n" % (name)) - mavproxy.expect("%s = (.*)" % (name,)) - return float(mavproxy.match.group(1)) - - -################################################# -# UTILITIES -################################################# -def get_distance(loc1, loc2): - """Get ground distance between two locations.""" - dlat = loc2.lat - loc1.lat - dlong = loc2.lng - loc1.lng - return math.sqrt((dlat*dlat) + (dlong*dlong)) * 1.113195e5 - - -def get_bearing(loc1, loc2): - """Get bearing from loc1 to loc2.""" - off_x = loc2.lng - loc1.lng - off_y = loc2.lat - loc1.lat - bearing = 90.00 + math.atan2(-off_y, off_x) * 57.2957795 - if bearing < 0: - bearing += 360.00 - return bearing - - -def do_get_autopilot_capabilities(mavproxy, mav): - mavproxy.send("long REQUEST_AUTOPILOT_CAPABILITIES 1\n") - m = mav.recv_match(type='AUTOPILOT_VERSION', blocking=True, timeout=10) - if m is None: - progress("AUTOPILOT_VERSION not received") + def set_rc(self, chan, pwm, timeout=5): + """Setup a simulated RC control to a PWM value""" + tstart = self.get_sim_time() + while self.get_sim_time() < tstart + timeout: + self.mavproxy.send('rc %u %u\n' % (chan, pwm)) + m = self.mav.recv_match(type='RC_CHANNELS', blocking=True) + chan_pwm = getattr(m, "chan" + str(chan) + "_raw") + if chan_pwm == pwm: + return True + progress("Failed to send RC commands") return False - progress("AUTOPILOT_VERSION received") - return True + def arm_vehicle(self): + """Arm vehicle with mavlink arm message.""" + self.mavproxy.send('arm throttle\n') + self.mav.motors_armed_wait() + progress("ARMED") + return True -def do_set_mode_via_command_long(mavproxy, mav): - base_mode = mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED - custom_mode = 4 # hold - start = time.time() - while time.time() - start < 5: - mavproxy.send("long DO_SET_MODE %u %u\n" % (base_mode,custom_mode)) - m = mav.recv_match(type='HEARTBEAT', blocking=True, timeout=10) + def disarm_vehicle(self): + """Disarm vehicle with mavlink disarm message.""" + self.mavproxy.send('disarm\n') + self.mav.motors_disarmed_wait() + progress("DISARMED") + return True + + def set_parameter(self, name, value): + for i in range(1, 10): + self.mavproxy.send("param set %s %s\n" % (name, str(value))) + self.mavproxy.send("param fetch %s\n" % name) + self.mavproxy.expect("%s = (.*)" % (name,)) + returned_value = self.mavproxy.match.group(1) + if float(returned_value) == float(value): + # yes, exactly equal. + break + progress("Param fetch returned incorrect value (%s) vs (%s)" % (returned_value, value)) + + def get_parameter(self, name): + self.mavproxy.send("param fetch %s\n" % name) + self.mavproxy.expect("%s = (.*)" % (name,)) + return float(self.mavproxy.match.group(1)) + + ################################################# + # UTILITIES + ################################################# + @staticmethod + def get_distance(loc1, loc2): + """Get ground distance between two locations.""" + dlat = loc2.lat - loc1.lat + dlong = loc2.lng - loc1.lng + return math.sqrt((dlat*dlat) + (dlong*dlong)) * 1.113195e5 + + @staticmethod + def get_bearing(loc1, loc2): + """Get bearing from loc1 to loc2.""" + off_x = loc2.lng - loc1.lng + off_y = loc2.lat - loc1.lat + bearing = 90.00 + math.atan2(-off_y, off_x) * 57.2957795 + if bearing < 0: + bearing += 360.00 + return bearing + + def do_get_autopilot_capabilities(self): + self.mavproxy.send("long REQUEST_AUTOPILOT_CAPABILITIES 1\n") + m = self.mav.recv_match(type='AUTOPILOT_VERSION', blocking=True, timeout=10) if m is None: + progress("AUTOPILOT_VERSION not received") return False - if m.custom_mode == custom_mode: - return True - time.sleep(0.1) - return False + progress("AUTOPILOT_VERSION received") + return True + def do_set_mode_via_command_long(self): + base_mode = mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED + custom_mode = 4 # hold + start = time.time() + while time.time() - start < 5: + self.mavproxy.send("long DO_SET_MODE %u %u\n" % (base_mode, custom_mode)) + m = self.mav.recv_match(type='HEARTBEAT', blocking=True, timeout=10) + if m is None: + return False + if m.custom_mode == custom_mode: + return True + time.sleep(0.1) + return False -################################################# -# WAIT UTILITIES -################################################# -def wait_seconds(mav, seconds_to_wait): - """Wait some second in SITL time.""" - tstart = get_sim_time(mav) - tnow = tstart - while tstart + seconds_to_wait > tnow: - tnow = get_sim_time(mav) + def reach_heading_manual(self, heading): + """Manually direct the vehicle to the target heading.""" + if self.mav.mav_type in [mavutil.mavlink.MAV_TYPE_QUADROTOR, + mavutil.mavlink.MAV_TYPE_HELICOPTER, + mavutil.mavlink.MAV_TYPE_HEXAROTOR, + mavutil.mavlink.MAV_TYPE_OCTOROTOR, + mavutil.mavlink.MAV_TYPE_COAXIAL, + mavutil.mavlink.MAV_TYPE_TRICOPTER]: + self.mavproxy.send('rc 4 1580\n') + if not self.wait_heading(heading): + progress("Failed to reach heading") + return False + self.mavproxy.send('rc 4 1500\n') + self.mav.recv_match(condition='RC_CHANNELS.chan4_raw==1500', blocking=True) + if self.mav.mav_type == mavutil.mavlink.MAV_TYPE_FIXED_WING: + progress("NOT IMPLEMENTED") + if self.mav.mav_type == mavutil.mavlink.MAV_TYPE_GROUND_ROVER: + self.mavproxy.send('rc 1 1700\n') + self.mavproxy.send('rc 3 1550\n') + if not self.wait_heading(heading): + progress("Failed to reach heading") + return False + self.mavproxy.send('rc 3 1500\n') + self.mav.recv_match(condition='RC_CHANNELS.chan3_raw==1500', blocking=True) + self.mavproxy.send('rc 1 1500\n') + self.mav.recv_match(condition='RC_CHANNELS.chan1_raw==1500', blocking=True) + return True + def reach_distance_manual(self, distance): + """Manually direct the vehicle to the target distance from home.""" + if self.mav.mav_type in [mavutil.mavlink.MAV_TYPE_QUADROTOR, + mavutil.mavlink.MAV_TYPE_HELICOPTER, + mavutil.mavlink.MAV_TYPE_HEXAROTOR, + mavutil.mavlink.MAV_TYPE_OCTOROTOR, + mavutil.mavlink.MAV_TYPE_COAXIAL, + mavutil.mavlink.MAV_TYPE_TRICOPTER]: + self.mavproxy.send('rc 2 1350\n') + if not self.wait_distance(distance, accuracy=5, timeout=60): + progress("Failed to reach distance of %u" % distance) + return False + self.mavproxy.send('rc 2 1500\n') + self.mav.recv_match(condition='RC_CHANNELS.chan2_raw==1500', blocking=True) + if self.mav.mav_type == mavutil.mavlink.MAV_TYPE_FIXED_WING: + progress("NOT IMPLEMENTED") + if self.mav.mav_type == mavutil.mavlink.MAV_TYPE_GROUND_ROVER: + self.mavproxy.send('rc 3 1700\n') + if not self.wait_distance(distance, accuracy=2): + progress("Failed to reach distance of %u" % distance) + return False + self.mavproxy.send('rc 3 1500\n') + self.mav.recv_match(condition='RC_CHANNELS.chan3_raw==1500', blocking=True) + return True -def wait_altitude(mav, alt_min, alt_max, timeout=30): - """Wait for a given altitude range.""" - climb_rate = 0 - previous_alt = 0 + ################################################# + # WAIT UTILITIES + ################################################# + def wait_seconds(self, seconds_to_wait): + """Wait some second in SITL time.""" + tstart = self.get_sim_time() + tnow = tstart + while tstart + seconds_to_wait > tnow: + tnow = self.get_sim_time() - tstart = get_sim_time(mav) - progress("Waiting for altitude between %u and %u" % (alt_min, alt_max)) - while get_sim_time(mav) < tstart + timeout: - m = mav.recv_match(type='VFR_HUD', blocking=True) - climb_rate = m.alt - previous_alt - previous_alt = m.alt - progress("Wait Altitude: Cur:%u, min_alt:%u, climb_rate: %u" % (m.alt, alt_min, climb_rate)) - if m.alt >= alt_min and m.alt <= alt_max: - progress("Altitude OK") - return True - progress("Failed to attain altitude range") - return False + def wait_altitude(self, alt_min, alt_max, timeout=30): + """Wait for a given altitude range.""" + climb_rate = 0 + previous_alt = 0 + tstart = self.get_sim_time() + progress("Waiting for altitude between %u and %u" % (alt_min, alt_max)) + while self.get_sim_time() < tstart + timeout: + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + climb_rate = m.alt - previous_alt + previous_alt = m.alt + progress("Wait Altitude: Cur:%u, min_alt:%u, climb_rate: %u" % (m.alt, alt_min, climb_rate)) + if m.alt >= alt_min and m.alt <= alt_max: + progress("Altitude OK") + return True + progress("Failed to attain altitude range") + return False -def wait_groundspeed(mav, gs_min, gs_max, timeout=30): - """Wait for a given ground speed range.""" - tstart = get_sim_time(mav) - progress("Waiting for groundspeed between %.1f and %.1f" % (gs_min, gs_max)) - while get_sim_time(mav) < tstart + timeout: - m = mav.recv_match(type='VFR_HUD', blocking=True) - progress("Wait groundspeed %.1f, target:%.1f" % (m.groundspeed, gs_min)) - if m.groundspeed >= gs_min and m.groundspeed <= gs_max: - return True - progress("Failed to attain groundspeed range") - return False + def wait_groundspeed(self, gs_min, gs_max, timeout=30): + """Wait for a given ground speed range.""" + tstart = self.get_sim_time() + progress("Waiting for groundspeed between %.1f and %.1f" % (gs_min, gs_max)) + while self.get_sim_time() < tstart + timeout: + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + progress("Wait groundspeed %.1f, target:%.1f" % (m.groundspeed, gs_min)) + if m.groundspeed >= gs_min and m.groundspeed <= gs_max: + return True + progress("Failed to attain groundspeed range") + return False + def wait_roll(self, roll, accuracy, timeout=30): + """Wait for a given roll in degrees.""" + tstart = self.get_sim_time() + progress("Waiting for roll of %d at %s" % (roll, time.ctime())) + while self.get_sim_time() < tstart + timeout: + m = self.mav.recv_match(type='ATTITUDE', blocking=True) + p = math.degrees(m.pitch) + r = math.degrees(m.roll) + progress("Roll %d Pitch %d" % (r, p)) + if math.fabs(r - roll) <= accuracy: + progress("Attained roll %d" % roll) + return True + progress("Failed to attain roll %d" % roll) + return False -def wait_roll(mav, roll, accuracy, timeout=30): - """Wait for a given roll in degrees.""" - tstart = get_sim_time(mav) - progress("Waiting for roll of %d at %s" % (roll, time.ctime())) - while get_sim_time(mav) < tstart + timeout: - m = mav.recv_match(type='ATTITUDE', blocking=True) - p = math.degrees(m.pitch) - r = math.degrees(m.roll) - progress("Roll %d Pitch %d" % (r, p)) - if math.fabs(r - roll) <= accuracy: - progress("Attained roll %d" % roll) - return True - progress("Failed to attain roll %d" % roll) - return False + def wait_pitch(self, pitch, accuracy, timeout=30): + """Wait for a given pitch in degrees.""" + tstart = self.get_sim_time() + progress("Waiting for pitch of %u at %s" % (pitch, time.ctime())) + while self.get_sim_time() < tstart + timeout: + m = self.mav.recv_match(type='ATTITUDE', blocking=True) + p = math.degrees(m.pitch) + r = math.degrees(m.roll) + progress("Pitch %d Roll %d" % (p, r)) + if math.fabs(p - pitch) <= accuracy: + progress("Attained pitch %d" % pitch) + return True + progress("Failed to attain pitch %d" % pitch) + return False + def wait_heading(self, heading, accuracy=5, timeout=30): + """Wait for a given heading.""" + tstart = self.get_sim_time() + progress("Waiting for heading %u with accuracy %u" % (heading, accuracy)) + while self.get_sim_time() < tstart + timeout: + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + progress("Heading %u" % m.heading) + if math.fabs(m.heading - heading) <= accuracy: + progress("Attained heading %u" % heading) + return True + progress("Failed to attain heading %u" % heading) + return False -def wait_pitch(mav, pitch, accuracy, timeout=30): - """Wait for a given pitch in degrees.""" - tstart = get_sim_time(mav) - progress("Waiting for pitch of %u at %s" % (pitch, time.ctime())) - while get_sim_time(mav) < tstart + timeout: - m = mav.recv_match(type='ATTITUDE', blocking=True) - p = math.degrees(m.pitch) - r = math.degrees(m.roll) - progress("Pitch %d Roll %d" % (p, r)) - if math.fabs(p - pitch) <= accuracy: - progress("Attained pitch %d" % pitch) - return True - progress("Failed to attain pitch %d" % pitch) - return False + def wait_distance(self, distance, accuracy=5, timeout=30): + """Wait for flight of a given distance.""" + tstart = self.get_sim_time() + start = self.mav.location() + while self.get_sim_time() < tstart + timeout: + pos = self.mav.location() + delta = self.get_distance(start, pos) + progress("Distance %.2f meters" % delta) + if math.fabs(delta - distance) <= accuracy: + progress("Attained distance %.2f meters OK" % delta) + return True + if delta > (distance + accuracy): + progress("Failed distance - overshoot delta=%f distance=%f" % (delta, distance)) + return False + progress("Failed to attain distance %u" % distance) + return False + def wait_location(self, loc, accuracy=5, timeout=30, target_altitude=None, height_accuracy=-1): + """Wait for arrival at a location.""" + tstart = self.get_sim_time() + if target_altitude is None: + target_altitude = loc.alt + progress("Waiting for location %.4f,%.4f at altitude %.1f height_accuracy=%.1f" % ( + loc.lat, loc.lng, target_altitude, height_accuracy)) + while self.get_sim_time() < tstart + timeout: + pos = self.mav.location() + delta = self.get_distance(loc, pos) + progress("Distance %.2f meters alt %.1f" % (delta, pos.alt)) + if delta <= accuracy: + if height_accuracy != -1 and math.fabs(pos.alt - target_altitude) > height_accuracy: + continue + progress("Reached location (%.2f meters)" % delta) + return True + progress("Failed to attain location") + return False -def wait_heading(mav, heading, accuracy=5, timeout=30): - """Wait for a given heading.""" - tstart = get_sim_time(mav) - progress("Waiting for heading %u with accuracy %u" % (heading, accuracy)) - while get_sim_time(mav) < tstart + timeout: - m = mav.recv_match(type='VFR_HUD', blocking=True) - progress("Heading %u" % m.heading) - if math.fabs(m.heading - heading) <= accuracy: - progress("Attained heading %u" % heading) - return True - progress("Failed to attain heading %u" % heading) - return False + def wait_waypoint(self, wpnum_start, wpnum_end, allow_skip=True, max_dist=2, timeout=400): + """Wait for waypoint ranges.""" + tstart = self.get_sim_time() + # this message arrives after we set the current WP + start_wp = self.mav.waypoint_current() + current_wp = start_wp + mode = self.mav.flightmode + progress("\ntest: wait for waypoint ranges start=%u end=%u\n\n" % (wpnum_start, wpnum_end)) + # if start_wp != wpnum_start: + # progress("test: Expected start waypoint %u but got %u" % (wpnum_start, start_wp)) + # return False -def wait_distance(mav, distance, accuracy=5, timeout=30): - """Wait for flight of a given distance.""" - tstart = get_sim_time(mav) - start = mav.location() - while get_sim_time(mav) < tstart + timeout: - pos = mav.location() - delta = get_distance(start, pos) - progress("Distance %.2f meters" % delta) - if math.fabs(delta - distance) <= accuracy: - progress("Attained distance %.2f meters OK" % delta) - return True - if delta > (distance + accuracy): - progress("Failed distance - overshoot delta=%f distance=%f" % (delta, distance)) - return False - progress("Failed to attain distance %u" % distance) - return False + while self.get_sim_time() < tstart + timeout: + seq = self.mav.waypoint_current() + m = self.mav.recv_match(type='NAV_CONTROLLER_OUTPUT', blocking=True) + wp_dist = m.wp_dist + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + # if we changed mode, fail + if self.mav.flightmode != mode: + progress('Exited %s mode' % mode) + return False -def wait_location(mav, loc, accuracy=5, timeout=30, target_altitude=None, height_accuracy=-1): - """Wait for arrival at a location.""" - tstart = get_sim_time(mav) - if target_altitude is None: - target_altitude = loc.alt - progress("Waiting for location %.4f,%.4f at altitude %.1f height_accuracy=%.1f" % ( - loc.lat, loc.lng, target_altitude, height_accuracy)) - while get_sim_time(mav) < tstart + timeout: - pos = mav.location() - delta = get_distance(loc, pos) - progress("Distance %.2f meters alt %.1f" % (delta, pos.alt)) - if delta <= accuracy: - if height_accuracy != -1 and math.fabs(pos.alt - target_altitude) > height_accuracy: - continue - progress("Reached location (%.2f meters)" % delta) - return True - progress("Failed to attain location") - return False + progress("test: WP %u (wp_dist=%u Alt=%d), current_wp: %u, wpnum_end: %u" % (seq, wp_dist, m.alt, current_wp, wpnum_end)) + if seq == current_wp+1 or (seq > current_wp+1 and allow_skip): + progress("test: Starting new waypoint %u" % seq) + tstart = self.get_sim_time() + current_wp = seq + # the wp_dist check is a hack until we can sort out the right seqnum + # for end of mission + # if current_wp == wpnum_end or (current_wp == wpnum_end-1 and wp_dist < 2): + if current_wp == wpnum_end and wp_dist < max_dist: + progress("Reached final waypoint %u" % seq) + return True + if seq >= 255: + progress("Reached final waypoint %u" % seq) + return True + if seq > current_wp+1: + progress("Failed: Skipped waypoint! Got wp %u expected %u" % (seq, current_wp+1)) + return False + progress("Failed: Timed out waiting for waypoint %u of %u" % (wpnum_end, wpnum_end)) + return False + def wait_mode(self, mode, timeout=None): + """Wait for mode to change.""" + progress("Waiting for mode %s" % mode) + tstart = self.get_sim_time() + hastimeout = False + while self.mav.flightmode.upper() != mode.upper() and not hastimeout: + if timeout is not None: + hastimeout = self.get_sim_time() > tstart + timeout + self.mav.wait_heartbeat() + progress("Got mode %s" % mode) + return self.mav.flightmode -def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, max_dist=2, timeout=400): - """Wait for waypoint ranges.""" - tstart = get_sim_time(mav) - # this message arrives after we set the current WP - start_wp = mav.waypoint_current() - current_wp = start_wp - mode = mav.flightmode + def wait_ready_to_arm(self, timeout=None): + # wait for EKF checks to pass + return self.wait_ekf_happy(timeout=timeout) - progress("\ntest: wait for waypoint ranges start=%u end=%u\n\n" % (wpnum_start, wpnum_end)) - # if start_wp != wpnum_start: - # progress("test: Expected start waypoint %u but got %u" % (wpnum_start, start_wp)) - # return False + def wait_ekf_happy(self, timeout=30): + """Wait for EKF to be happy""" - while get_sim_time(mav) < tstart + timeout: - 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) + tstart = self.get_sim_time() + required_value = 831 + progress("Waiting for EKF value %u" % required_value) + while timeout is None or self.get_sim_time() < tstart + timeout: + m = self.mav.recv_match(type='EKF_STATUS_REPORT', blocking=True) + current = m.flags + if (tstart - self.get_sim_time()) % 5 == 0: + progress("Wait EKF.flags: required:%u current:%u" % (required_value, current)) + if current == required_value: + progress("EKF Flags OK") + return + progress("Failed to get EKF.flags=%u" % required_value) + raise AutoTestTimeoutException() - # if we changed mode, fail - if mav.flightmode != mode: - progress('Exited %s mode' % mode) - return False + @abc.abstractmethod + def init(self): + """Initilialize autotest feature.""" + pass - progress("test: WP %u (wp_dist=%u Alt=%d), current_wp: %u, wpnum_end: %u" % (seq, wp_dist, m.alt, current_wp, wpnum_end)) - if seq == current_wp+1 or (seq > current_wp+1 and allow_skip): - progress("test: Starting new waypoint %u" % seq) - tstart = get_sim_time(mav) - current_wp = seq - # the wp_dist check is a hack until we can sort out the right seqnum - # for end of mission - # if current_wp == wpnum_end or (current_wp == wpnum_end-1 and wp_dist < 2): - if (current_wp == wpnum_end and wp_dist < max_dist): - progress("Reached final waypoint %u" % seq) - return True - if (seq >= 255): - progress("Reached final waypoint %u" % seq) - return True - if seq > current_wp+1: - progress("Failed: Skipped waypoint! Got wp %u expected %u" % (seq, current_wp+1)) - return False - progress("Failed: Timed out waiting for waypoint %u of %u" % (wpnum_end, wpnum_end)) - return False + # def test_common_feature(self): + # """Common feature to test.""" + # sucess = True + # # TEST ARMING/DISARM + # if not self.arm_vehicle(): + # progress("Failed to ARM") + # sucess = False + # if not self.disarm_vehicle(): + # progress("Failed to DISARM") + # sucess = False + # if not self.test_arm_motors_radio(): + # progress("Failed to ARM with radio") + # sucess = False + # if not self.test_disarm_motors_radio(): + # progress("Failed to ARM with radio") + # sucess = False + # if not self.test_autodisarm_motors(): + # progress("Failed to AUTO DISARM") + # sucess = False + # # TODO: Test failure on arm (with arming check) + # # TEST MISSION FILE + # # TODO : rework that to work on autotest server + # # progress("TEST LOADING MISSION") + # # num_wp = self.load_mission_from_file(os.path.join(testdir, "fake_mission.txt")) + # # if num_wp == 0: + # # progress("Failed to load all_msg_mission") + # # sucess = False + # # + # # progress("TEST SAVING MISSION") + # # num_wp_old = num_wp + # # num_wp = self.save_mission_to_file(os.path.join(testdir, "fake_mission2.txt")) + # # if num_wp != num_wp_old: + # # progress("Failed to save all_msg_mission") + # # sucess = False + # + # progress("TEST CLEARING MISSION") + # self.mavproxy.send("wp clear\n") + # self.mavproxy.send('wp list\n') + # self.mavproxy.expect('Requesting [0-9]+ waypoints') + # num_wp = mavwp.MAVWPLoader().count() + # if num_wp != 0: + # progress("Failed to clear mission ") + # sucess = False + # + # return sucess + # + # # TESTS FAILSAFE + # @abc.abstractmethod + # def test_throttle_failsafe(self, home, distance_min=10, side=60, timeout=180): + # """Test that RTL success in case of thottle failsafe.""" + # pass + # + # # TEST ARM RADIO + # @abc.abstractmethod + # def test_arm_motors_radio(self): + # """Test arming with RC sticks.""" + # pass + # + # # TEST DISARM RADIO + # @abc.abstractmethod + # def test_disarm_motors_radio(self): + # """Test disarming with RC sticks.""" + # pass + # + # # TEST AUTO DISARM + # @abc.abstractmethod + # def test_autodisarm_motors(self): + # """Test auto disarming.""" + # pass + # + # # TEST RC OVERRIDE + # # TEST RC OVERRIDE TIMEOUT + # @abc.abstractmethod + # def test_rtl(self, home, distance_min=10, timeout=250): + # """Test that RTL success.""" + # progress("# Enter RTL") + # self.mavproxy.send('switch 3\n') + # tstart = self.get_sim_time() + # while self.get_sim_time() < tstart + timeout: + # m = self.mav.recv_match(type='VFR_HUD', blocking=True) + # pos = self.mav.location() + # home_distance = self.get_distance(home, pos) + # progress("Alt: %u HomeDistance: %.0f" % (m.alt, home_distance)) + # if m.alt <= 1 and home_distance < distance_min: + # progress("RTL Complete") + # return True + # return False + # + # @abc.abstractmethod + # def test_mission(self, filename): + # pass - -def wait_mode(mav, mode, timeout=None): - """Wait for mode to change.""" - progress("Waiting for mode %s" % mode) - mav.wait_heartbeat() - mav.recv_match(condition='MAV.flightmode.upper()=="%s".upper()' % mode, timeout=timeout, blocking=True) - progress("Got mode %s" % mode) - return mav.flightmode - - -def wait_ready_to_arm(mav, timeout=None): - # wait for EKF checks to pass - return wait_ekf_happy(mav, timeout=timeout) - - -def wait_ekf_happy(mav, timeout=30): - """Wait for EKF to be happy""" - - tstart = get_sim_time(mav) - required_value = 831 - progress("Waiting for EKF value %u" % (required_value)) - while timeout is None or get_sim_time(mav) < tstart + timeout: - m = mav.recv_match(type='EKF_STATUS_REPORT', blocking=True) - current = m.flags - if (tstart - get_sim_time(mav)) % 5 == 0: - progress("Wait EKF.flags: required:%u current:%u" % (required_value, current)) - if current == required_value: - progress("EKF Flags OK") - return - progress("Failed to get EKF.flags=%u" % required_value) - raise AutoTestTimeoutException() + @abc.abstractmethod + def autotest(self): + """Autotest used by ArduPilot autotest CI.""" + pass diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index 0b7e2f194e..fa65de63a3 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -12,124 +12,174 @@ from pysim import util # get location of scripts testdir = os.path.dirname(os.path.realpath(__file__)) - - -HOME_LOCATION = '-27.274439,151.290064,343,8.7' +HOME = mavutil.location(-27.274439, 151.290064, 343, 8.7) MISSION = 'ArduPlane-Missions/Dalby-OBC2016.txt' FENCE = 'ArduPlane-Missions/Dalby-OBC2016-fence.txt' WIND = "0,180,0.2" # speed,direction,variance -homeloc = None +class AutotestQuadPlane(Autotest): + def __init__(self, binary, viewerip=None, use_map=False, valgrind=False, gdb=False, speedup=10, frame=None, params=None, gdbserver=False): + super(AutotestQuadPlane, self).__init__() + self.binary = binary + self.options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=10' + self.viewerip = viewerip + self.use_map = use_map + self.valgrind = valgrind + self.gdb = gdb + self.frame = frame + self.params = params + self.gdbserver = gdbserver -def fly_mission(mavproxy, mav, filename, fence, height_accuracy=-1): - """Fly a mission from a file.""" - progress("Flying mission %s" % filename) - mavproxy.send('wp load %s\n' % filename) - mavproxy.expect('Flight plan received') - mavproxy.send('fence load %s\n' % fence) - mavproxy.send('wp list\n') - mavproxy.expect('Requesting [0-9]+ waypoints') - mavproxy.send('mode AUTO\n') - wait_mode(mav, 'AUTO') - if not wait_waypoint(mav, 1, 19, max_dist=60, timeout=1200): - return False - mavproxy.expect('DISARMED') - # wait for blood sample here - mavproxy.send('wp set 20\n') - arm_vehicle(mavproxy, mav) - if not wait_waypoint(mav, 20, 34, max_dist=60, timeout=1200): - return False - mavproxy.expect('DISARMED') - progress("Mission OK") - return True + 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.log_name = "ArduCopter" + self.logfile = None + self.buildlog = None + self.copy_tlog = False -def fly_QuadPlane(binary, viewerip=None, use_map=False, valgrind=False, gdb=False, gdbserver=False, speedup=10): - """Fly QuadPlane in SITL. + self.sitl = None + self.hasInit = False - you can pass viewerip as an IP address to optionally send fg and - mavproxy packets too for local viewing of the flight in real time. - """ - global homeloc + def init(self): + if self.frame is None: + self.frame = 'quadplane' - options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=10' - if viewerip: - options += " --out=%s:14550" % viewerip - if use_map: - options += ' --map' + if self.viewerip: + self.options += " --out=%s:14550" % self.viewerip + if self.use_map: + self.options += ' --map' - sitl = util.start_SITL(binary, model='quadplane', wipe=True, home=HOME_LOCATION, speedup=speedup, - defaults_file=os.path.join(testdir, 'default_params/quadplane.parm'), valgrind=valgrind, gdb=gdb, gdbserver=gdbserver) - mavproxy = util.start_MAVProxy_SITL('QuadPlane', options=options) - mavproxy.expect('Telemetry log: (\S+)') - logfile = mavproxy.match.group(1) - progress("LOGFILE %s" % logfile) + self.sitl = util.start_SITL(self.binary, wipe=True, model=self.frame, home=self.home, speedup=self.speedup, + defaults_file=os.path.join(testdir, 'default_params/quadplane.parm'), + valgrind=self.valgrind, gdb=self.gdb, gdbserver=self.gdbserver) + self.mavproxy = util.start_MAVProxy_SITL('QuadPlane', options=self.options) + self.mavproxy.expect('Telemetry log: (\S+)') + logfile = self.mavproxy.match.group(1) + progress("LOGFILE %s" % logfile) - buildlog = util.reltopdir("../buildlogs/QuadPlane-test.tlog") - progress("buildlog=%s" % buildlog) - if os.path.exists(buildlog): - os.unlink(buildlog) - try: - os.link(logfile, buildlog) - except Exception: - pass + buildlog = util.reltopdir("../buildlogs/QuadPlane-test.tlog") + progress("buildlog=%s" % buildlog) + if os.path.exists(buildlog): + os.unlink(buildlog) + try: + os.link(logfile, buildlog) + except Exception: + pass - util.expect_setup_callback(mavproxy, expect_callback) + self.mavproxy.expect('Received [0-9]+ parameters') - mavproxy.expect('Received [0-9]+ parameters') + util.expect_setup_callback(self.mavproxy, expect_callback) - expect_list_clear() - expect_list_extend([sitl, mavproxy]) + expect_list_clear() + expect_list_extend([self.sitl, self.mavproxy]) - progress("Started simulator") + progress("Started simulator") - # get a mavlink connection going - try: - mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True) - except Exception as msg: - progress("Failed to start mavlink connection on 127.0.0.1:19550" % msg) - raise - mav.message_hooks.append(message_hook) - mav.idle_hooks.append(idle_hook) + # get a mavlink connection going + try: + self.mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True) + except Exception as msg: + progress("Failed to start mavlink connection on 127.0.0.1:19550" % msg) + raise + self.mav.message_hooks.append(message_hook) + self.mav.idle_hooks.append(idle_hook) + self.hasInit = True + progress("Ready to start testing!") - failed = False - e = 'None' - try: - progress("Waiting for a heartbeat with mavlink protocol %s" % mav.WIRE_PROTOCOL_VERSION) - mav.wait_heartbeat() - progress("Waiting for GPS fix") - mav.recv_match(condition='VFR_HUD.alt>10', blocking=True) - mav.wait_gps_fix() - while mav.location().alt < 10: - mav.wait_gps_fix() - homeloc = mav.location() - progress("Home location: %s" % homeloc) + def close(self): + if self.use_map: + self.mavproxy.send("module unload map\n") + self.mavproxy.expect("Unloaded module map") - # wait for EKF and GPS checks to pass - wait_seconds(mav, 30) + self.mav.close() + util.pexpect_close(self.mavproxy) + util.pexpect_close(self.sitl) - arm_vehicle(mavproxy, mav) + valgrind_log = util.valgrind_log_filepath(binary=self.binary, model=self.frame) + if os.path.exists(valgrind_log): + os.chmod(valgrind_log, 0o644) + shutil.copy(valgrind_log, util.reltopdir("../buildlogs/QuadPlane-valgrind.log")) - if not fly_mission(mavproxy, mav, - os.path.join(testdir, "ArduPlane-Missions/Dalby-OBC2016.txt"), - os.path.join(testdir, "ArduPlane-Missions/Dalby-OBC2016-fence.txt")): - progress("Failed mission") + # def test_arm_motors_radio(self): + # super(AutotestQuadPlane, self).test_arm_motors_radio() + # + # def test_disarm_motors_radio(self): + # super(AutotestQuadPlane, self).test_disarm_motors_radio() + # + # def test_autodisarm_motors(self): + # super(AutotestQuadPlane, self).test_autodisarm_motors() + # + # def test_rtl(self, home, distance_min=10, timeout=250): + # super(AutotestQuadPlane, self).test_rtl(home, distance_min=10, timeout=250) + # + # def test_throttle_failsafe(self, home, distance_min=10, side=60, timeout=180): + # super(AutotestQuadPlane, self).test_throttle_failsafe(home, distance_min=10, side=60, timeout=180) + # + # def test_mission(self, filename): + # super(AutotestQuadPlane, self).test_mission(filename) + + def fly_mission(self, filename, fence, height_accuracy=-1): + """Fly a mission from a file.""" + progress("Flying mission %s" % filename) + self.mavproxy.send('wp load %s\n' % filename) + self.mavproxy.expect('Flight plan received') + self.mavproxy.send('fence load %s\n' % fence) + self.mavproxy.send('wp list\n') + self.mavproxy.expect('Requesting [0-9]+ waypoints') + self.mavproxy.send('mode AUTO\n') + self.wait_mode('AUTO') + if not self.wait_waypoint(1, 19, max_dist=60, timeout=1200): + return False + self.mavproxy.expect('DISARMED') + # wait for blood sample here + self.mavproxy.send('wp set 20\n') + self.arm_vehicle() + if not self.wait_waypoint(20, 34, max_dist=60, timeout=1200): + return False + self.mavproxy.expect('DISARMED') + progress("Mission OK") + return True + + def autotest(self): + """Autotest QuadPlane in SITL.""" + self.frame = 'quadplane' + if not self.hasInit: + self.init() + + failed = False + e = 'None' + try: + progress("Waiting for a heartbeat with mavlink protocol %s" % self.mav.WIRE_PROTOCOL_VERSION) + self.mav.wait_heartbeat() + 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() + progress("Home location: %s" % self.homeloc) + + # wait for EKF and GPS checks to pass + progress("Waiting reading for arm") + self.wait_seconds(30) + + self.arm_vehicle() + + if not self.fly_mission(os.path.join(testdir, "ArduPlane-Missions/Dalby-OBC2016.txt"), + os.path.join(testdir, "ArduPlane-Missions/Dalby-OBC2016-fence.txt")): + progress("Failed mission") + failed = True + except pexpect.TIMEOUT as e: + progress("Failed with timeout") failed = True - except pexpect.TIMEOUT as e: - progress("Failed with timeout") - failed = True - mav.close() - util.pexpect_close(mavproxy) - util.pexpect_close(sitl) + self.close() - valgrind_log = util.valgrind_log_filepath(binary=binary, model='quadplane') - if os.path.exists(valgrind_log): - os.chmod(valgrind_log, 0o644) - shutil.copy(valgrind_log, util.reltopdir("../buildlogs/QuadPlane-valgrind.log")) - - if failed: - progress("FAILED: %s" % e) - return False - return True + if failed: + progress("FAILED: %s" % e) + return False + return True