#!/usr/bin/env python # Fly ArduCopter in SITL from __future__ import print_function import math import os import shutil import time import pexpect from pymavlink import mavutil from pysim import util from common import AutoTest from common import NotAchievedException, AutoTestTimeoutException, PreconditionFailedException # get location of scripts testdir = os.path.dirname(os.path.realpath(__file__)) HOME = mavutil.location(-35.362938, 149.165085, 584, 270) AVCHOME = mavutil.location(40.072842, -105.230575, 1586, 0) # Flight mode switch positions are set-up in arducopter.param to be # switch 1 = Circle # switch 2 = Land # switch 3 = RTL # switch 4 = Auto # switch 5 = Loiter # switch 6 = Stabilize class AutoTestCopter(AutoTest): def __init__(self, binary, valgrind=False, gdb=False, speedup=10, frame=None, params=None, gdbserver=False, breakpoints=[], **kwargs): super(AutoTestCopter, self).__init__(**kwargs) self.binary = binary self.valgrind = valgrind self.gdb = gdb self.frame = frame self.params = params self.gdbserver = gdbserver self.breakpoints = breakpoints self.home = "%f,%f,%u,%u" % (HOME.lat, HOME.lng, HOME.alt, HOME.heading) self.homeloc = None self.speedup = speedup self.log_name = "ArduCopter" self.sitl = None self.hasInit = False def mavproxy_options(self): ret = super(AutoTestCopter, self).mavproxy_options() if self.frame != 'heli': ret.append('--quadcopter') return ret def sitl_streamrate(self): return 5 def vehicleinfo_key(self): return 'ArduCopter' 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) 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, breakpoints=self.breakpoints, vicon=True, wipe=True) self.mavproxy = util.start_MAVProxy_SITL( 'ArduCopter', options=self.mavproxy_options()) self.mavproxy.expect('Telemetry log: (\S+)\r\n') self.logfile = self.mavproxy.match.group(1) self.progress("LOGFILE %s" % self.logfile) self.try_symlink_tlog() self.progress("WAITING FOR PARAMETERS") self.mavproxy.expect('Received [0-9]+ parameters') self.apply_defaultfile_parameters() util.expect_setup_callback(self.mavproxy, self.expect_callback) self.expect_list_clear() self.expect_list_extend([self.sitl, self.mavproxy]) self.progress("Started simulator") self.get_mavlink_connection_going() self.hasInit = True self.progress("Ready to start testing!") def close(self): super(AutoTestCopter, self).close() # [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) def user_takeoff(self, alt_min=30): '''takeoff using mavlink takeoff command''' self.run_cmd(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, # param1 0, # param2 0, # param3 0, # param4 0, # param5 0, # param6 alt_min # param7 ) self.progress("Ran command") self.wait_for_alt(alt_min) def takeoff(self, alt_min=30, takeoff_throttle=1700, require_absolute=True): """Takeoff get to 30m altitude.""" self.progress("TAKEOFF") self.mavproxy.send('switch 6\n') # stabilize mode self.wait_mode('STABILIZE') if not self.armed(): self.progress("Waiting reading for arm") self.wait_ready_to_arm(require_absolute=require_absolute) self.set_rc(3, 1000) self.arm_vehicle() self.set_rc(3, takeoff_throttle) self.wait_for_alt(alt_min=alt_min) self.hover() self.progress("TAKEOFF COMPLETE") def wait_for_alt(self, alt_min=30): """Wait for altitude to be reached.""" m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) alt = m.relative_alt / 1000.0 # mm -> m if alt < alt_min: self.wait_altitude(alt_min - 1, (alt_min + 5), relative=True) def land(self, timeout=60): """Land the quad.""" self.progress("STARTING LANDING") self.mavproxy.send('switch 2\n') # land mode self.wait_mode('LAND', timeout=timeout) self.progress("Entered Landing Mode") self.wait_altitude(-5, 1, relative=True, timeout=timeout) self.progress("LANDING: ok!") def hover(self, hover_throttle=1500): self.set_rc(3, hover_throttle) # loiter - fly south west, then 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') # first aim south east self.progress("turn south east") self.set_rc(4, 1580) self.wait_heading(170) self.set_rc(4, 1500) # fly south east 50m self.set_rc(2, 1100) self.wait_distance(50) self.set_rc(2, 1500) # wait for copter to slow moving self.wait_groundspeed(0, 2) m = self.mav.recv_match(type='VFR_HUD', blocking=True) start_altitude = m.alt start = self.mav.location() tstart = self.get_sim_time() self.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) self.progress("Loiter Dist: %.2fm, alt:%u" % (delta, m.alt)) if alt_delta > maxaltchange: raise NotAchievedException( "Loiter alt shifted %u meters (> limit %u)" % (alt_delta, maxaltchange)) if delta > maxdistchange: raise NotAchievedException( "Loiter shifted %u meters (> limit of %u)" % (delta, maxdistchange)) self.progress("Loiter OK for %u seconds" % holdtime) def change_alt(self, alt_min, climb_throttle=1920, descend_throttle=1080): """Change altitude.""" m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) alt = m.relative_alt / 1000.0 # mm -> m if alt < alt_min: self.progress("Rise to alt:%u from %u" % (alt_min, alt)) self.set_rc(3, climb_throttle) self.wait_altitude(alt_min, (alt_min + 5), relative=True) else: self.progress("Lower to alt:%u from %u" % (alt_min, alt)) self.set_rc(3, descend_throttle) self.wait_altitude((alt_min - 5), alt_min, relative=True) self.hover() ################################################# # TESTS FLY ################################################# # fly a square in alt_hold mode def fly_square(self, side=50, timeout=300): """Fly a square, flying N then E .""" tstart = self.get_sim_time() # 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) # switch to loiter mode temporarily to stop us from rising self.mavproxy.send('switch 5\n') self.wait_mode('LOITER') # first aim north self.progress("turn right towards north") self.set_rc(4, 1580) self.wait_heading(10) 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 self.progress("Save WP 1 & 2") self.save_wp() # switch back to stabilize mode self.mavproxy.send('switch 6\n') self.wait_mode('STABILIZE') # increase throttle a bit because we're about to pitch: self.set_rc(3, 1525) # pitch forward to fly north self.progress("Going north %u meters" % side) self.set_rc(2, 1300) self.wait_distance(side) self.set_rc(2, 1500) # save top left corner of square as waypoint self.progress("Save WP 3") self.save_wp() # roll right to fly east self.progress("Going east %u meters" % side) self.set_rc(1, 1700) self.wait_distance(side) self.set_rc(1, 1500) # save top right corner of square as waypoint self.progress("Save WP 4") self.save_wp() # pitch back to fly south self.progress("Going south %u meters" % side) self.set_rc(2, 1700) self.wait_distance(side) self.set_rc(2, 1500) # save bottom right corner of square as waypoint self.progress("Save WP 5") self.save_wp() # roll left to fly west self.progress("Going west %u meters" % side) self.set_rc(1, 1300) self.wait_distance(side) self.set_rc(1, 1500) # save bottom left corner of square (should be near home) as waypoint self.progress("Save WP 6") self.save_wp() # reduce throttle again self.set_rc(3, 1500) # descend to 10m self.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) self.progress("timeleft = %u" % time_left) if time_left < 20: time_left = 20 self.wait_altitude(-10, 10, time_left, relative=True) self.set_rc(3, 1500) self.save_wp() # enter RTL mode and wait for the vehicle to disarm def fly_RTL(self, timeout=250): """Return, land.""" self.progress("# Enter RTL") self.mavproxy.send('switch 3\n') self.set_rc(3, 1500) tstart = self.get_sim_time() while self.get_sim_time() < tstart + timeout: m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) alt = m.relative_alt / 1000.0 # mm -> m pos = self.mav.location() # requires a GPS fix to function home_distance = self.get_distance(HOME, pos) home = "" if alt <= 1 and home_distance < 10: home = "HOME" self.progress("Alt: %u HomeDist: %.0f %s" % (alt, home_distance, home)) # our post-condition is that we are disarmed: if not self.armed(): if home == "": raise NotAchievedException("Did not get home") # success! return raise AutoTestTimeoutException("Did not get home and disarm") def fly_throttle_failsafe(self, side=60, timeout=180): """Fly east, Failsafe, return, land.""" # switch to loiter mode temporarily to stop us from rising self.mavproxy.send('switch 5\n') self.wait_mode('LOITER') # first aim east self.progress("turn east") self.set_rc(4, 1580) self.wait_heading(135) self.set_rc(4, 1500) # raise throttle slightly to avoid hitting the ground pos = self.mav.location(relative_alt=True) if pos.alt > 25: self.set_rc(3, 1300) self.wait_altitude(20, 25, relative=True) if pos.alt < 20: self.set_rc(3, 1800) self.wait_altitude(20, 25, relative=True) self.hover() # switch to stabilize mode self.mavproxy.send('switch 6\n') self.wait_mode('STABILIZE') self.hover() # fly east 60 meters self.progress("# Going forward %u meters" % side) self.set_rc(2, 1350) self.wait_distance(side, 5, 60) self.set_rc(2, 1500) # pull throttle low self.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='GLOBAL_POSITION_INT', blocking=True) alt = m.relative_alt / 1000.0 # mm -> m pos = self.mav.location() home_distance = self.get_distance(HOME, pos) self.progress("Alt: %u HomeDist: %.0f" % (alt, home_distance)) # check if we've reached home if 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') self.progress("Waiting for disarm") self.mav.motors_disarmed_wait() self.progress("Reached failsafe home OK") self.mavproxy.send('switch 6\n') # stabilize mode self.wait_mode('STABILIZE') self.set_rc(3, 1000) self.arm_vehicle() return # 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') raise AutoTestTimeoutException( ("Failed to land on failsafe RTL - " "timed out after %u seconds" % timeout)) def fly_battery_failsafe(self, timeout=300): # switch to loiter mode so that we hold position self.mavproxy.send('switch 5\n') self.wait_mode('LOITER') self.set_rc(3, 1500) # enable battery failsafe self.set_parameter('BATT_FS_LOW_ACT', 1) # trigger low voltage self.set_parameter('SIM_BATT_VOLTAGE', 10) # wait for LAND mode. If unsuccessful an exception will be raised self.wait_mode('LAND', timeout=timeout) # disable battery failsafe self.set_parameter('BATT_FS_LOW_ACT', 0) self.set_parameter('SIM_BATT_VOLTAGE', 13) self.progress("Successfully entered LAND after battery failsafe") self.reboot_sitl() # 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') # first south self.progress("turn south") self.set_rc(4, 1580) self.wait_heading(180) self.set_rc(4, 1500) # fly west 80m self.set_rc(2, 1100) self.wait_distance(80) self.set_rc(2, 1500) # wait for copter to slow moving self.wait_groundspeed(0, 2) m = self.mav.recv_match(type='VFR_HUD', blocking=True) start_altitude = m.alt start = self.mav.location() tstart = self.get_sim_time() self.progress("Holding loiter at %u meters for %u seconds" % (start_altitude, holdtime)) # cut motor 1 to 55% efficiency self.progress("Cutting motor 1 to 60% efficiency") self.set_parameter("SIM_ENGINE_MUL", 0.60) 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) self.progress("Loiter Dist: %.2fm, alt:%u" % (delta, m.alt)) if alt_delta > maxaltchange: raise NotAchievedException( "Loiter alt shifted %u meters (> limit %u)" % (alt_delta, maxaltchange)) if delta > maxdistchange: raise NotAchievedException( ("Loiter shifted %u meters (> limit of %u)" % (delta, maxdistchange))) # restore motor 1 to 100% efficiency self.set_parameter("SIM_ENGINE_MUL", 1.0) self.progress("Stability patch and Loiter OK for %us" % holdtime) # 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') # enable fence, disable avoidance self.set_parameter("FENCE_ENABLE", 1) self.set_parameter("AVOID_ENABLE", 0) # first east self.progress("turn east") self.set_rc(4, 1580) self.wait_heading(160) self.set_rc(4, 1500) # fly forward (east) at least 20m pitching_forward = True self.set_rc(2, 1100) self.wait_distance(20) # start timer tstart = self.get_sim_time() while self.get_sim_time() < tstart + timeout: m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) alt = m.relative_alt / 1000.0 # mm -> m pos = self.mav.location() home_distance = self.get_distance(HOME, pos) self.progress("Alt: %f HomeDistance: %.0f" % (alt, home_distance)) # recenter pitch sticks once we're home so we don't fly off again if pitching_forward and home_distance < 10: pitching_forward = False self.set_rc(2, 1475) # disable fence self.set_parameter("FENCE_ENABLE", 0) if (alt <= 1 and home_distance < 10) or (not self.armed() and home_distance < 10): # reduce throttle self.set_rc(3, 1000) self.mavproxy.send('switch 2\n') # land mode self.wait_mode('LAND') self.progress("Waiting for disarm") self.mav.motors_disarmed_wait() self.progress("Reached home OK") self.mavproxy.send('switch 6\n') # stabilize mode self.wait_mode('STABILIZE') self.set_rc(3, 1000) # remove if we ever clear battery failsafe flag on disarm: self.mavproxy.send('arm uncheck all\n') self.arm_vehicle() # remove if we ever clear battery failsafe flag on disarm: self.mavproxy.send('arm check all\n') self.progress("Reached home OK") return # disable fence, enable avoidance self.set_parameter("FENCE_ENABLE", 0) self.set_parameter("AVOID_ENABLE", 1) # 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') self.progress() raise AutoTestTimeoutException( ("Fence test failed to reach home - " "timed out after %u seconds" % timeout)) # fly_alt_fence_test - fly up until you hit the fence def fly_alt_max_fence_test(self): """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) self.change_alt(10) # first east self.progress("turn east") self.set_rc(4, 1580) self.wait_heading(160) self.set_rc(4, 1500) # fly forward (east) at least 20m self.set_rc(2, 1100) self.wait_distance(20) # stop flying forward and start flying up: self.set_rc(2, 1500) self.set_rc(3, 1800) # wait for fence to trigger self.wait_mode('RTL', timeout=120) self.progress("Waiting for disarm") self.mav.motors_disarmed_wait() self.set_rc(3, 1000) self.mavproxy.send('switch 6\n') # stabilize mode self.wait_mode('STABILIZE') # remove if we ever clear battery failsafe flag on disarm self.mavproxy.send('arm uncheck all\n') self.arm_vehicle() # remove if we ever clear battery failsafe flag on disarm: self.mavproxy.send('arm check all\n') def fly_gps_glitch_loiter_test(self, timeout=30, max_distance=20): """fly_gps_glitch_loiter_test. Fly south east in loiter and test reaction to gps glitch.""" self.mavproxy.send('switch 5\n') # loiter mode self.wait_mode('LOITER') # turn on simulator display of gps and actual position if self.use_map: self.show_gps_and_sim_positions(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) self.progress("GPS Glitches:") for i in range(1, glitch_num): self.progress("glitch %d %.7f %.7f" % (i, glitch_lat[i], glitch_lon[i])) # turn south east self.progress("turn south east") self.set_rc(4, 1580) try: self.wait_heading(150) self.set_rc(4, 1500) # fly forward (south east) at least 60m self.set_rc(2, 1100) self.wait_distance(60) self.set_rc(2, 1500) # wait for copter to slow down except Exception as e: if self.use_map: self.show_gps_and_sim_positions(False) raise e # record time and position tstart = self.get_sim_time() tnow = tstart start_pos = self.sim_location() # initialise current glitch glitch_current = 0 self.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 glitch list if glitch_current >= glitch_num: glitch_current = -1 self.progress("Completed Glitches") self.set_parameter("SIM_GPS_GLITCH_X", 0) self.set_parameter("SIM_GPS_GLITCH_Y", 0) else: self.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='GLOBAL_POSITION_INT', blocking=True) alt = m.alt/1000.0 # mm -> m curr_pos = self.sim_location() moved_distance = self.get_distance(curr_pos, start_pos) self.progress("Alt: %u Moved: %.0f" % (alt, moved_distance)) if moved_distance > max_distance: self.progress() raise NotAchievedException( "Moved over %u meters, Failed!" % max_distance) # disable gps glitch if glitch_current != -1: self.set_parameter("SIM_GPS_GLITCH_X", 0) self.set_parameter("SIM_GPS_GLITCH_Y", 0) if self.use_map: self.show_gps_and_sim_positions(False) self.progress("GPS glitch test passed!" " stayed within %u meters for %u seconds" % (max_distance, timeout)) # 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) self.progress("GPS Glitches:") for i in range(1, glitch_num): self.progress("glitch %d %.7f %.7f" % (i, glitch_lat[i], glitch_lon[i])) # Fly mission #1 self.progress("# Load copter_glitch_mission") # load the waypoint count global num_wp num_wp = self.load_mission("copter_glitch_mission.txt") if not num_wp: raise NotAchievedException("load copter_glitch_mission failed") # turn on simulator display of gps and actual position if self.use_map: self.show_gps_and_sim_positions(True) self.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 try: self.wait_distance(100, 5, 90) except Exception as e: if self.use_map: self.show_gps_and_sim_positions(False) raise e # record time and position tstart = self.get_sim_time() # initialise current glitch glitch_current = 0 self.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: self.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 self.progress("Completed Glitches") self.set_parameter("SIM_GPS_GLITCH_X", 0) self.set_parameter("SIM_GPS_GLITCH_Y", 0) # continue with the mission self.wait_waypoint(0, num_wp-1, timeout=500) # wait for arrival back home 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): raise AutoTestTimeoutException( ("GPS Glitch testing failed" "- exceeded timeout %u seconds" % timeout)) self.mav.recv_match(type='VFR_HUD', blocking=True) pos = self.mav.location() dist_to_home = self.get_distance(HOME, pos) self.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) self.progress("GPS Glitch test Auto completed: passed!") # 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): # hold position in loiter self.mavproxy.send('switch 5\n') # loiter mode self.wait_mode('LOITER') # set SIMPLE mode for all flight modes self.set_parameter("SIMPLE", 63) # switch to stabilize mode self.mavproxy.send('switch 6\n') self.wait_mode('STABILIZE') self.set_rc(3, 1500) # fly south 50m self.progress("# Flying south %u meters" % side) self.set_rc(1, 1300) self.wait_distance(side, 5, 60) self.set_rc(1, 1500) # fly west 8 seconds self.progress("# Flying west for 8 seconds") self.set_rc(2, 1300) tstart = self.get_sim_time() while self.get_sim_time() < (tstart + 8): self.mav.recv_match(type='VFR_HUD', blocking=True) self.set_rc(2, 1500) # fly north 25 meters self.progress("# Flying north %u meters" % (side/2.0)) self.set_rc(1, 1700) self.wait_distance(side/2, 5, 60) self.set_rc(1, 1500) # fly east 8 seconds self.progress("# Flying east for 8 seconds") self.set_rc(2, 1700) tstart = self.get_sim_time() while self.get_sim_time() < (tstart + 8): self.mav.recv_match(type='VFR_HUD', blocking=True) self.set_rc(2, 1500) # restore to default self.set_parameter("SIMPLE", 0) # hover in place self.hover() # fly_super_simple - flies a circle around home for 45 seconds def fly_super_simple(self, timeout=45): # hold position in loiter self.mavproxy.send('switch 5\n') # loiter mode self.wait_mode('LOITER') # fly forward 20m self.progress("# Flying forward 20 meters") self.set_rc(2, 1300) self.wait_distance(20, 5, 60) self.set_rc(2, 1500) # set SUPER SIMPLE mode for all flight modes self.set_parameter("SUPER_SIMPLE", 63) # 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 self.progress("# rolling left from pilot's POV for %u seconds" % timeout) self.set_rc(1, 1300) tstart = self.get_sim_time() while self.get_sim_time() < (tstart + timeout): self.mav.recv_match(type='VFR_HUD', blocking=True) # stop rolling and yawing self.set_rc(1, 1500) self.set_rc(4, 1500) # restore simple mode parameters to default self.set_parameter("SUPER_SIMPLE", 0) # hover in place self.hover() # fly_circle - flies a circle with 20m radius def fly_circle(self, holdtime=36): # hold position in loiter self.mavproxy.send('switch 5\n') # loiter mode self.wait_mode('LOITER') # face west self.progress("turn west") self.set_rc(4, 1580) self.wait_heading(270) self.set_rc(4, 1500) # set CIRCLE radius self.set_parameter("CIRCLE_RADIUS", 3000) # fly forward (east) at least 100m self.set_rc(2, 1100) self.wait_distance(100) # 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() self.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) self.progress("heading %u" % m.heading) self.progress("CIRCLE OK for %u seconds" % holdtime) # fly_optical_flow_limits - test EKF navigation limiting def fly_optical_flow_limits(self): ex = None self.context_push() try: self.set_parameter("SIM_FLOW_ENABLE", 1) self.set_parameter("FLOW_ENABLE", 1) self.set_parameter("RNGFND_TYPE", 1) self.set_parameter("RNGFND_MIN_CM", 0) self.set_parameter("RNGFND_MAX_CM", 4000) self.set_parameter("RNGFND_PIN", 0) self.set_parameter("RNGFND_SCALING", 12.12, epsilon=0.01) self.set_parameter("SIM_GPS_DISABLE", 1) self.reboot_sitl() self.takeoff(alt_min=2, require_absolute=False) self.mavproxy.send('mode loiter\n') self.wait_mode('LOITER') # speed should be limited to <10m/s self.set_rc(2, 1000) tstart = self.get_sim_time() timeout = 60 while self.get_sim_time_cached() - tstart < timeout: m = self.mav.recv_match(type='VFR_HUD', blocking=True) spd = m.groundspeed self.progress("%0.1f: Low Speed: %f" % (self.get_sim_time_cached() - tstart, spd)) if spd > 8: raise NotAchievedException(("Speed should be limited by" "EKF optical flow limits")) self.progress("Moving higher") self.change_alt(60) self.wait_groundspeed(10, 100, timeout=60) except Exception as e: ex = e self.set_rc(2, 1500) self.context_pop() self.reboot_sitl() if ex is not None: raise ex # fly_autotune - autotune the virtual vehicle def fly_autotune(self): # hold position in loiter self.mavproxy.send('mode autotune\n') self.wait_mode('AUTOTUNE') tstart = self.get_sim_time() sim_time_expected = 5000 deadline = tstart + sim_time_expected while self.get_sim_time_cached() < deadline: now = self.get_sim_time_cached() m = self.mav.recv_match(type='STATUSTEXT', blocking=True, timeout=1) if m is None: continue self.progress("STATUSTEXT (%u<%u): %s" % (now, deadline, m.text)) if "AutoTune: Success" in m.text: self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart)) # near enough for now: return self.progress() raise NotAchievedException("AUTOTUNE failed (%u seconds)" % (self.get_sim_time() - tstart)) # fly_auto_test - fly mission which tests a significant number of commands def fly_auto_test(self): # Fly mission #1 self.progress("# Load copter_mission") # load the waypoint count global num_wp num_wp = self.load_mission("copter_mission.txt") if not num_wp: raise NotAchievedException("load copter_mission failed") self.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 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() self.progress("MOTORS DISARMED OK") self.progress("Auto mission completed: passed!") def load_mission(self, mission): path = os.path.join(testdir, mission) return self.load_mission_from_file(path) # fly_avc_test - fly AVC mission def fly_avc_test(self): # upload mission from file self.progress("# Load copter_AVC2013_mission") # load the waypoint count global num_wp num_wp = self.load_mission("copter_AVC2013_mission.txt") if not num_wp: raise NotAchievedException("load copter_AVC2013_mission failed") self.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 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() self.progress("MOTORS DISARMED OK") self.progress("AVC mission completed: passed!") def fly_motor_fail(self, fail_servo=0, fail_mul=0.0, holdtime=30): """Test flight with reduced motor efficiency""" # we only expect an octocopter to survive ATM: servo_counts = { # 2: 6, # hexa 3: 8, # octa # 5: 6, # Y6 } frame_class = int(self.get_parameter("FRAME_CLASS")) if frame_class not in servo_counts: self.progress("Test not relevant for frame_class %u" % frame_class) return servo_count = servo_counts[frame_class] if fail_servo < 0 or fail_servo > servo_count: raise ValueError('fail_servo outside range for frame class') self.mavproxy.send('switch 5\n') # loiter mode self.wait_mode('LOITER') self.change_alt(alt_min=50) # Get initial values start_hud = self.mav.recv_match(type='VFR_HUD', blocking=True) start_attitude = self.mav.recv_match(type='ATTITUDE', blocking=True) hover_time = 5 try: tstart = self.get_sim_time() int_error_alt = 0 int_error_yaw_rate = 0 int_error_yaw = 0 self.progress("Hovering for %u seconds" % hover_time) failed = False while self.get_sim_time() < tstart + holdtime + hover_time: ti = self.get_sim_time() servo = self.mav.recv_match(type='SERVO_OUTPUT_RAW', blocking=True) hud = self.mav.recv_match(type='VFR_HUD', blocking=True) attitude = self.mav.recv_match(type='ATTITUDE', blocking=True) if not failed and self.get_sim_time() - tstart > hover_time: self.progress("Killing motor %u (%u%%)" % (fail_servo+1, fail_mul)) self.set_parameter("SIM_ENGINE_FAIL", fail_servo) self.set_parameter("SIM_ENGINE_MUL", fail_mul) failed = True if failed: self.progress("Hold Time: %f/%f" % (self.get_sim_time()-tstart, holdtime)) servo_pwm = [servo.servo1_raw, servo.servo2_raw, servo.servo3_raw, servo.servo4_raw, servo.servo5_raw, servo.servo6_raw, servo.servo7_raw, servo.servo8_raw] self.progress("PWM output per motor") for i, pwm in enumerate(servo_pwm[0:servo_count]): if pwm > 1900: state = "oversaturated" elif pwm < 1200: state = "undersaturated" else: state = "OK" if failed and i == fail_servo: state += " (failed)" self.progress("servo %u [pwm=%u] [%s]" % (i+1, pwm, state)) alt_delta = hud.alt - start_hud.alt yawrate_delta = attitude.yawspeed - start_attitude.yawspeed yaw_delta = attitude.yaw - start_attitude.yaw self.progress("Alt=%fm (delta=%fm)" % (hud.alt, alt_delta)) self.progress("Yaw rate=%f (delta=%f) (rad/s)" % (attitude.yawspeed, yawrate_delta)) self.progress("Yaw=%f (delta=%f) (deg)" % (attitude.yaw, yaw_delta)) dt = self.get_sim_time() - ti int_error_alt += abs(alt_delta/dt) int_error_yaw_rate += abs(yawrate_delta/dt) int_error_yaw += abs(yaw_delta/dt) self.progress("## Error Integration ##") self.progress(" Altitude: %fm" % int_error_alt) self.progress(" Yaw rate: %f rad/s" % int_error_yaw_rate) self.progress(" Yaw: %f deg" % int_error_yaw) self.progress("----") if alt_delta < -20: raise NotAchievedException("Vehicle is descending") self.set_parameter("SIM_ENGINE_FAIL", 0) self.set_parameter("SIM_ENGINE_MUL", 1.0) except Exception as e: self.set_parameter("SIM_ENGINE_FAIL", 0) self.set_parameter("SIM_ENGINE_MUL", 1.0) raise e return True def fly_mission(self): """Fly a mission from a file.""" global num_wp self.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') self.wait_waypoint(0, num_wp-1, timeout=500) self.progress("test: MISSION COMPLETE: passed!") # wait here until ready self.mavproxy.send('switch 5\n') # loiter mode self.wait_mode('LOITER') self.set_rc(3, 1500) def fly_vision_position(self): """Disable GPS navigation, enable Vicon input.""" # scribble down a location we can set origin to: self.progress("Waiting for location") self.mavproxy.send('switch 6\n') # stabilize mode self.mav.wait_heartbeat() self.wait_mode('STABILIZE') self.progress("Waiting reading for arm") self.wait_ready_to_arm() old_pos = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) print("old_pos=%s" % str(old_pos)) self.context_push() ex = None try: self.set_parameter("GPS_TYPE", 0) self.set_parameter("EK2_GPS_TYPE", 3) self.set_parameter("SERIAL5_PROTOCOL", 1) self.reboot_sitl() # without a GPS or some sort of external prompting, AP # doesn't send system_time messages. So prompt it: self.mav.mav.system_time_send(time.time() * 1000000, 0) self.mav.mav.set_gps_global_origin_send(1, old_pos.lat, old_pos.lon, old_pos.alt) self.progress("Waiting for non-zero-lat") tstart = self.get_sim_time() while True: gpi = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) # self.progress("gpi=%s" % str(gpi)) if gpi.lat != 0: break if self.get_sim_time() - tstart > 10: raise AutoTestTimeoutException("Did not get non-zero lat") self.takeoff() self.set_rc(1, 1600) tstart = self.get_sim_time() while True: vicon_pos = self.mav.recv_match(type='VICON_POSITION_ESTIMATE', blocking=True) # print("vpe=%s" % str(vicon_pos)) self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) # self.progress("gpi=%s" % str(gpi)) if vicon_pos.x > 40: break if self.get_sim_time() - tstart > 100: raise AutoTestTimeoutException("Vicon showed no movement") # recenter controls: self.set_rc(1, 1500) self.progress("# Enter RTL") self.mavproxy.send('switch 3\n') self.set_rc(3, 1500) tstart = self.get_sim_time() while True: if self.get_sim_time() - tstart > 200: raise NotAchievedException("Did not disarm") self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) # print("gpi=%s" % str(gpi)) self.mav.recv_match(type='SIMSTATE', blocking=True) # print("ss=%s" % str(ss)) # wait for RTL disarm: if not self.armed(): break except Exception as e: self.progress("Exception caught") ex = e self.context_pop() self.set_rc(3, 1000) self.reboot_sitl() if ex is not None: raise ex def fly_nav_delay(self): """Fly a simple mission that has a delay in it.""" self.load_mission("copter_nav_delay.txt") self.mavproxy.send('mode loiter\n') self.mav.wait_heartbeat() self.wait_mode('LOITER') self.progress("Waiting reading for arm") self.wait_ready_to_arm() self.context_push() ex = None try: self.arm_vehicle() self.mavproxy.send('mode auto\n') self.wait_mode('AUTO') self.set_rc(3, 1600) count_start = -1 count_stop = -1 tstart = self.get_sim_time() last_mission_current_msg = 0 last_seq = None while self.armed(): # we RTL at end of mission now = self.get_sim_time_cached() if now - tstart > 120: raise AutoTestTimeoutException("Did not disarm as expected") m = self.mav.recv_match(type='MISSION_CURRENT', blocking=True) if ((now - last_mission_current_msg) > 1 or m.seq != last_seq): dist = None x = self.mav.messages.get("NAV_CONTROLLER_OUTPUT", None) if x is not None: dist = x.wp_dist self.progress("MISSION_CURRENT.seq=%u dist=%s" % (m.seq, dist)) last_mission_current_msg = self.get_sim_time_cached() last_seq = m.seq if m.seq == 3: self.progress("At delay item") if count_start == -1: count_start = now if m.seq > 3: if count_stop == -1: count_stop = now calculated_delay = count_stop - count_start want_delay = 59 # should reflect what's in the mission file self.progress("Stopped for %u seconds (want >=%u seconds)" % (calculated_delay, want_delay)) if calculated_delay < want_delay: raise NotAchievedException("Did not delay for long enough") except Exception as e: self.progress("Exception caught") ex = e self.set_rc(3, 1000) if ex is not None: raise ex def get_system_clock_utc(self, time_seconds): # this is a copy of ArduPilot's AP_RTC function! # separate time into ms, sec, min, hour and days but all expressed # in milliseconds time_ms = time_seconds * 1000 ms = time_ms % 1000 sec_ms = (time_ms % (60 * 1000)) - ms min_ms = (time_ms % (60 * 60 * 1000)) - sec_ms - ms hour_ms = (time_ms % (24 * 60 * 60 * 1000)) - min_ms - sec_ms - ms # convert times as milliseconds into appropriate units secs = sec_ms / 1000 mins = min_ms / (60 * 1000) hours = hour_ms / (60 * 60 * 1000) return (hours, mins, secs, 0) def calc_delay(self, seconds): # delay-for-seconds has to be long enough that we're at the # waypoint before that time. Otherwise we'll try to wait a # day.... (hours, mins, secs, ms) = self.get_system_clock_utc(seconds) self.progress("Now is %uh %um %us" % (hours, mins, secs)) secs += 17 # add seventeen seconds if secs >= 60: secs %= 60 mins += 1 # add sixty seconds mins += 1 if mins >= 60: mins %= 60 hours += 1 if hours >= 24: hours %= 24 return (hours, mins, secs, 0) def reset_delay_item_seventyseven(self, seq): while True: self.progress("Requesting request for seq %u" % (seq,)) self.mav.mav.mission_write_partial_list_send(1, # target system 1, # target component seq, # start index seq) req = self.mav.recv_match(type='MISSION_REQUEST', blocking=True, timeout=1) if req is not None and req.seq == seq: if req.get_srcSystem() == 255: self.progress("Shutup MAVProxy") continue # notionally this might be in the message cache before # we prompt for it... *shrug* break # we have received a request for the item. Supply it: frame = mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT command = mavutil.mavlink.MAV_CMD_NAV_DELAY # retrieve mission item and check it: tried_set = False while True: self.progress("Requesting item") self.mav.mav.mission_request_send(1, 1, seq ) st = self.mav.recv_match(type='MISSION_ITEM', blocking=True, timeout=1) if st is None: continue print("Item: %s" % str(st)) if (tried_set and st.seq == seq and st.command == command and st.param2 == hours and st.param3 == mins and st.param4 == secs): return self.progress("Mission mismatch") m = None tstart = self.get_sim_time() while True: if self.get_sim_time_cached() - tstart > 3: raise NotAchievedException( "Did not receive MISSION_REQUEST") self.mav.mav.mission_write_partial_list_send(1, 1, seq, seq) m = self.mav.recv_match(type='MISSION_REQUEST', blocking=True, timeout=1) if m is None: continue if m.seq != st.seq: continue break self.progress("Sending absolute-time mission item") # we have to change out the delay time... now = self.mav.messages["SYSTEM_TIME"] if now is None: raise PreconditionFailedException("Never got SYSTEM_TIME") if now.time_unix_usec == 0: raise PreconditionFailedException("system time is zero") (hours, mins, secs, ms) = self.calc_delay( now.time_unix_usec/1000000) self.progress("Delay until %uh %um %us" % (hours, mins, secs)) self.mav.mav.mission_item_send( 1, # target system 1, # target component seq, # seq frame, # frame command, # command 0, # current 1, # autocontinue 0, # p1 (relative seconds) hours, # p2 mins, # p3 secs, # p4 0, # p5 0, # p6 0) # p7 tried_set = True ack = self.mav.recv_match(type='MISSION_ACK', blocking=True, timeout=1) self.progress("Received ack: %s" % str(ack)) def fly_nav_delay_abstime(self): """fly a simple mission that has a delay in it""" self.load_mission("copter_nav_delay.txt") self.progress("Starting mission") self.mavproxy.send('mode loiter\n') # stabilize mode self.mav.wait_heartbeat() self.wait_mode('LOITER') self.progress("Waiting reading for arm") self.wait_ready_to_arm() delay_item_seq = 3 self.reset_delay_item_seventyseven(delay_item_seq) delay_for_seconds = 77 reset_at_m = self.mav.recv_match(type='SYSTEM_TIME', blocking=True) reset_at = reset_at_m.time_unix_usec/1000000 self.context_push() ex = None try: self.arm_vehicle() self.mavproxy.send('mode auto\n') # stabilize mode self.wait_mode('AUTO') self.set_rc(3, 1600) count_stop = -1 tstart = self.get_sim_time() while self.armed(): # we RTL at end of mission now = self.get_sim_time() if now - tstart > 120: raise AutoTestTimeoutException("Did not disarm as expected") m = self.mav.recv_match(type='MISSION_CURRENT', blocking=True) self.progress("MISSION_CURRENT.seq=%u" % (m.seq,)) if m.seq == delay_item_seq: self.progress("At delay item") if m.seq > delay_item_seq: if count_stop == -1: count_stop_m = self.mav.recv_match(type='SYSTEM_TIME', blocking=True) count_stop = count_stop_m.time_unix_usec/1000000 calculated_delay = count_stop - reset_at error = abs(calculated_delay - delay_for_seconds) self.progress("Stopped for %u seconds (want >=%u seconds)" % (calculated_delay, delay_for_seconds)) if error > 2: raise NotAchievedException("delay outside expectations") except Exception as e: self.progress("Exception caught") ex = e self.set_rc(3, 1000) if ex is not None: raise ex def fly_nav_takeoff_delay_abstime(self): """make sure taking off at a specific time works""" num_wp = self.load_mission("copter_nav_delay_takeoff.txt") self.progress("Starting mission") self.mavproxy.send('mode loiter\n') # stabilize mode self.mav.wait_heartbeat() self.wait_mode('LOITER') self.progress("Waiting reading for arm") self.wait_ready_to_arm() delay_item_seq = 2 self.reset_delay_item_seventyseven(delay_item_seq) delay_for_seconds = 77 reset_at = self.get_sim_time_cached(); self.context_push(); ex = None try: self.arm_vehicle() self.mavproxy.send('mode auto\n') # stabilize mode self.wait_mode('AUTO') self.set_rc(3, 1600) # should not take off for about least 77 seconds tstart = self.get_sim_time() took_off = False while self.armed(): now = self.get_sim_time_cached() if now - tstart > 200: # timeout break m = self.mav.recv_match(type='MISSION_CURRENT', blocking=True) now = self.get_sim_time_cached() self.progress("%s" % str(m)) if m.seq > delay_item_seq: if not took_off: took_off = True delta_time = now - reset_at if abs(delta_time - delay_for_seconds) > 2: raise NotAchievedException(( "Did not take off on time " "measured=%f want=%f" % (delta_time, delay_for_seconds))) if not took_off: raise NotAchievedException("Did not take off") except Exception as e: self.progress("Exception caught") ex = e self.set_rc(3, 1000) if ex is not None: raise ex def test_setting_modes_via_modeswitch(self): self.context_push() ex = None try: fltmode_ch = 5 self.set_parameter("FLTMODE_CH", fltmode_ch) self.set_rc(fltmode_ch, 1000) # PWM for mode1 testmodes = [("FLTMODE1", 4, "GUIDED", 1165), ("FLTMODE2", 13, "SPORT", 1295), ("FLTMODE3", 6, "RTL", 1425), ("FLTMODE4", 7, "CIRCLE", 1555), ("FLTMODE5", 1, "ACRO", 1685), ("FLTMODE6", 17, "BRAKE", 1815), ] for mode in testmodes: (parm, parm_value, name, pwm) = mode self.set_parameter(parm, parm_value) for mode in reversed(testmodes): (parm, parm_value, name, pwm) = mode self.set_rc(fltmode_ch, pwm) self.wait_mode(name) for mode in testmodes: (parm, parm_value, name, pwm) = mode self.set_rc(fltmode_ch, pwm) self.wait_mode(name) for mode in reversed(testmodes): (parm, parm_value, name, pwm) = mode self.set_rc(fltmode_ch, pwm) self.wait_mode(name) self.mavproxy.send('switch 6\n') self.wait_mode("BRAKE") self.mavproxy.send('switch 5\n') self.wait_mode("ACRO") except Exception as e: self.progress("Exception caught") ex = e self.context_pop() if ex is not None: raise ex def test_setting_modes_via_auxswitch(self): self.context_push() ex = None try: fltmode_ch = int(self.get_parameter("FLTMODE_CH")) self.set_rc(fltmode_ch, 1000) self.wait_mode("CIRCLE") self.set_rc(9, 1000) self.set_rc(10, 1000) self.set_parameter("RC9_OPTION", 18) # land self.set_parameter("RC10_OPTION", 55) # guided self.set_rc(9, 1900) self.wait_mode("LAND") self.set_rc(10, 1900) self.wait_mode("GUIDED") self.set_rc(10, 1000) # this re-polls the mode switch self.wait_mode("CIRCLE") except Exception as e: self.progress("Exception caught") ex = e self.context_pop() if ex is not None: raise ex def fly_guided_change_submode(self): """"Ensure we can move around in guided after a takeoff command.""" self.context_push() ex = None try: '''start by disabling GCS failsafe, otherwise we immediately disarm due to (apparently) not receiving traffic from the GCS for too long. This is probably a function of --speedup''' self.set_parameter("FS_GCS_ENABLE", 0) self.mavproxy.send('mode guided\n') # stabilize mode self.wait_mode('GUIDED') self.wait_ready_to_arm() self.arm_vehicle() self.user_takeoff(alt_min=10) startpos = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) """yaw through absolute angles using MAV_CMD_CONDITION_YAW""" self.guided_achieve_heading(45) self.guided_achieve_heading(135) """move the vehicle using set_position_target_global_int""" tstart = self.get_sim_time() while True: if self.get_sim_time() - tstart > 200: raise NotAchievedException("Did not move far enough") # send a position-control command self.mav.mav.set_position_target_global_int_send( 0, # timestamp 1, # target system_id 1, # target component id mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, 0b1111111111111000, # mask specifying use-only-lat-lon-alt 5, # lat 5, # lon 10, # alt 0, # vx 0, # vy 0, # vz 0, # afx 0, # afy 0, # afz 0, # yaw 0, # yawrate ) pos = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) delta = self.get_distance_int(startpos, pos) self.progress("delta=%f (want >10)" % delta) if delta > 10: break self.progress("Landing") self.mavproxy.send('switch 2\n') # land mode self.wait_mode('LAND') self.mav.motors_disarmed_wait() except Exception as e: self.progress("Exception caught") ex = e self.context_pop() self.set_rc(3, 1000) self.reboot_sitl() if ex is not None: raise ex def test_gripper_mission(self): self.context_push() ex = None try: self.load_mission("copter-gripper-mission.txt") self.mavproxy.send('mode loiter\n') self.wait_ready_to_arm() self.arm_vehicle() self.mavproxy.send('mode auto\n') self.wait_mode('AUTO') self.set_rc(3, 1500) self.mavproxy.expect("Gripper Grabbed") self.mavproxy.expect("Gripper Released") except Exception as e: self.progress("Exception caught") self.mavproxy.send('mode land\n') ex = e self.context_pop() self.mav.motors_disarmed_wait() if ex is not None: raise ex def autotest(self): """Autotest ArduCopter in SITL.""" self.check_test_syntax(test_file=os.path.realpath(__file__)) if not self.hasInit: self.init() self.fail_list = [] try: self.progress("Waiting for a heartbeat with mavlink protocol %s" % self.mav.WIRE_PROTOCOL_VERSION) self.mav.wait_heartbeat() self.progress("Setting up RC parameters") self.set_rc_default() self.set_rc(3, 1000) self.run_test("Fly Nav Delay (takeoff)", self.fly_nav_takeoff_delay_abstime) self.run_test("Fly Nav Delay (AbsTime)", self.fly_nav_delay_abstime) self.run_test("Fly Nav Delay", self.fly_nav_delay) self.run_test("Test submode change", self.fly_guided_change_submode) self.progress("Waiting for location") self.homeloc = self.mav.location() self.progress("Home location: %s" % self.homeloc) self.mavproxy.send('switch 6\n') # stabilize mode self.mav.wait_heartbeat() self.wait_mode('STABILIZE') self.progress("Waiting reading for arm") self.wait_ready_to_arm() self.run_test("Set modes via modeswitch", self.test_setting_modes_via_modeswitch) self.run_test("Set modes via auxswitch", self.test_setting_modes_via_auxswitch) self.mavproxy.send('switch 6\n') # stabilize mode self.wait_mode('STABILIZE') self.mavproxy.send("wp clear\n") # Arm self.run_test("Arm features", self.test_arm_feature) self.arm_vehicle() # Takeoff self.run_test("Takeoff to test fly Square", lambda: self.takeoff(10)) # AutoTune mode self.run_test("Fly AUTOTUNE mode", self.fly_autotune) # Fly a square in Stabilize mode self.run_test("Fly a square and save WPs with CH7", self.fly_square) # save the stored mission to file global num_wp num_wp = self.save_mission_to_file(os.path.join(testdir, "ch7_mission.txt")) if not num_wp: self.fail_list.append("save_mission_to_file") self.progress("save_mission_to_file failed") # fly the stored mission self.run_test("Fly CH7 saved mission", self.fly_mission) # Throttle Failsafe self.run_test("Test Failsafe", self.fly_throttle_failsafe) # Takeoff self.run_test("Takeoff to test battery failsafe", lambda: self.takeoff(10)) # Battery failsafe self.run_test("Fly Battery Failsafe", self.fly_battery_failsafe) # Takeoff self.run_test("Takeoff to test stability patch", lambda: self.takeoff(10)) # Stability patch self.run_test("Fly stability patch", lambda: self.fly_stability_patch(30)) # RTL self.run_test("RTL after stab patch", self.fly_RTL) # Takeoff self.run_test("Takeoff to test horizontal fence", lambda: self.takeoff(10)) # Fence test self.run_test("Test horizontal fence", lambda: self.fly_fence_test(180)) # Takeoff self.run_test("Takeoff to test Max Alt fence", lambda: self.takeoff(10)) # Fence test self.run_test("Test Max Alt Fence", self.fly_alt_max_fence_test) # Takeoff self.run_test("Takeoff to test GPS glitch loiter", lambda: self.takeoff(10)) # Fly GPS Glitch Loiter test self.run_test("GPS Glitch Loiter Test", self.fly_gps_glitch_loiter_test) # RTL after GPS Glitch Loiter test self.run_test("RTL after GPS Glitch Loiter test", self.fly_RTL) # Arm self.mavproxy.send('mode stabilize\n') # stabilize mode self.wait_mode('STABILIZE') self.set_rc(3, 1000) self.run_test("Arm motors", self.arm_vehicle) # Fly GPS Glitch test in auto mode self.run_test("GPS Glitch Auto Test", self.fly_gps_glitch_auto_test) # Takeoff self.run_test("Takeoff to test loiter", lambda: self.takeoff(10)) # Loiter for 10 seconds self.run_test("Test Loiter for 10 seconds", self.loiter) # Loiter Climb self.run_test("Loiter - climb to 30m", lambda: self.change_alt(30)) # Loiter Descend self.run_test("Loiter - descend to 20m", lambda: self.change_alt(20)) # RTL self.run_test("RTL after Loiter climb/descend", self.fly_RTL) # Takeoff self.run_test("Takeoff to test fly SIMPLE mode", lambda: self.takeoff(10)) # Simple mode self.run_test("Fly in SIMPLE mode", self.fly_simple) # RTL self.run_test("RTL after SIMPLE mode", self.fly_RTL) # Takeoff self.run_test("Takeoff to test circle in SUPER SIMPLE mode", lambda: self.takeoff(10)) # Fly a circle in super simple mode self.run_test("Fly a circle in SUPER SIMPLE mode", self.fly_super_simple) # RTL self.run_test("RTL after SUPER SIMPLE mode", self.fly_RTL) # Takeoff self.run_test("Takeoff to test CIRCLE mode", lambda: self.takeoff(10)) # Circle mode self.run_test("Fly CIRCLE mode", self.fly_circle) # Optical flow limits test self.run_test("Fly Optical Flow limits", self.fly_optical_flow_limits) # RTL self.run_test("RTL after CIRCLE mode", self.fly_RTL) # Arm self.set_rc(3, 1000) self.mavproxy.send('mode stabilize\n') # stabilize mode self.wait_mode('STABILIZE') # Takeoff self.run_test("Takeoff to test motor failure", lambda: self.takeoff(10)) self.run_test("Fly motor failure test", self.fly_motor_fail) # RTL self.run_test("RTL after motor failure test", self.fly_RTL) # Arm self.set_rc(3, 1000) self.mavproxy.send('mode stabilize\n') # stabilize mode self.wait_mode('STABILIZE') self.run_test("Arm motors", self.arm_vehicle) # Fly auto test self.run_test("Fly copter mission", self.fly_auto_test) # land self.run_test("Fly copter mission", self.land) # wait for disarm self.mav.motors_disarmed_wait() # Gripper test self.run_test("Test gripper", self.test_gripper) self.run_test("Test gripper mission items", self.test_gripper_mission); '''vision position''' # expects vehicle to be disarmed self.run_test("Fly Vision Position", self.fly_vision_position) # Download logs self.run_test("log download", lambda: self.log_download( self.buildlogs_path("ArduCopter-log.bin"))) except pexpect.TIMEOUT: self.progress("Failed with timeout") self.fail_list.append("Failed with timeout") self.close() if len(self.fail_list): self.progress("FAILED : %s" % self.fail_list) return False return True def autotest_heli(self): """Autotest Helicopter in SITL with AVC2013 mission.""" self.frame = 'heli' if not self.hasInit: self.init() self.fail_list = [] try: self.mav.wait_heartbeat() self.set_rc_default() self.set_rc(3, 1000) self.homeloc = self.mav.location() self.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() self.run_test("Arm features", self.test_arm_feature) # Arm self.run_test("Arm motors", self.arm_vehicle) self.progress("Raising rotor speed") self.set_rc(8, 2000) self.run_test("Fly AVC mission", self.fly_avc_test) self.progress("Lowering rotor speed") self.set_rc(8, 1000) # mission ends with disarm so should be ok to download logs now self.run_test("log download", lambda: self.log_download( self.buildlogs_path("Helicopter-log.bin"))) except pexpect.TIMEOUT: self.fail_list.append("Failed with timeout") self.close() if len(self.fail_list): self.progress("FAILED: %s" % self.fail_list) return False return True