40cd0cd17f
If any of the tests throws an exception, it will be caught by the generic test function that collects all errors and displays them before passing the result to the vehicle testcode. Hence the name of the test and the exception that got raised get printed in a single location. This nicely reduces code duplication.
1183 lines
42 KiB
Python
1183 lines
42 KiB
Python
#!/usr/bin/env python
|
|
|
|
# Fly ArduCopter in SITL
|
|
from __future__ import print_function
|
|
import math
|
|
import os
|
|
import shutil
|
|
|
|
import pexpect
|
|
from pymavlink import mavutil
|
|
|
|
from pysim import util
|
|
|
|
from common import AutoTest
|
|
from common import NotAchievedException, AutoTestTimeoutException
|
|
|
|
# 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,
|
|
**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.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
|
|
|
|
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.apply_parameters_using_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.mavproxy_options())
|
|
self.mavproxy.expect('Telemetry log: (\S+)\r\n')
|
|
self.logfile = self.mavproxy.match.group(1)
|
|
self.progress("LOGFILE %s" % self.logfile)
|
|
|
|
self.buildlog = self.buildlogs_path(self.log_name + "-test.tlog")
|
|
self.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:
|
|
self.progress("WARN: Failed to create symlink: %s => %s, "
|
|
"will copy tlog manually to target location" %
|
|
(self.logfile, self.buildlog))
|
|
self.copy_tlog = True
|
|
|
|
self.mavproxy.expect('Received [0-9]+ parameters')
|
|
|
|
util.expect_setup_callback(self.mavproxy, self.expect_callback)
|
|
|
|
self.expect_list_clear()
|
|
self.expect_list_extend([self.sitl, self.mavproxy])
|
|
|
|
self.progress("Started simulator")
|
|
|
|
# get a mavlink connection going
|
|
connection_string = '127.0.0.1:19550'
|
|
try:
|
|
self.mav = mavutil.mavlink_connection(connection_string,
|
|
robust_parsing=True)
|
|
except Exception as msg:
|
|
self.progress("Failed to start mavlink connection on %s: %s" %
|
|
(connection_string, msg,))
|
|
raise
|
|
self.mav.message_hooks.append(self.message_hook)
|
|
self.mav.idle_hooks.append(self.idle_hook)
|
|
self.hasInit = True
|
|
self.progress("Ready to start testing!")
|
|
|
|
def 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 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()
|
|
self.progress("TAKEOFF COMPLETE")
|
|
|
|
def land(self, timeout=60):
|
|
"""Land the quad."""
|
|
self.progress("STARTING LANDING")
|
|
self.mavproxy.send('switch 2\n') # land mode
|
|
self.wait_mode('LAND')
|
|
self.progress("Entered Landing Mode")
|
|
self.wait_altitude(-5, 1)
|
|
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:
|
|
self.progress("Loiter alt shifted %u meters (> limit of %u)" %
|
|
(alt_delta, maxaltchange))
|
|
raise NotAchievedException()
|
|
if delta > maxdistchange:
|
|
self.progress("Loiter shifted %u meters (> limit of %u)" %
|
|
(delta, maxdistchange))
|
|
raise NotAchievedException()
|
|
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='VFR_HUD', blocking=True)
|
|
if m.alt < alt_min:
|
|
self.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:
|
|
self.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()
|
|
|
|
#################################################
|
|
# 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.set_rc(3, 1500)
|
|
self.mavproxy.send('switch 6\n')
|
|
self.wait_mode('STABILIZE')
|
|
|
|
# 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()
|
|
|
|
# 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)
|
|
self.save_wp()
|
|
|
|
def fly_RTL(self, side=60, timeout=250):
|
|
"""Return, land."""
|
|
self.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)
|
|
self.progress("Alt: %u HomeDist: %.0f" % (m.alt, home_distance))
|
|
if m.alt <= 1 and home_distance < 10:
|
|
return
|
|
raise AutoTestTimeoutException()
|
|
|
|
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
|
|
self.set_rc(3, 1600)
|
|
|
|
# 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='VFR_HUD', blocking=True)
|
|
pos = self.mav.location()
|
|
home_distance = self.get_distance(HOME, pos)
|
|
self.progress("Alt: %u HomeDist: %.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')
|
|
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
|
|
self.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')
|
|
raise AutoTestTimeoutException()
|
|
|
|
def fly_battery_failsafe(self, timeout=30):
|
|
# 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', 300)
|
|
|
|
# disable battery failsafe
|
|
self.set_parameter('BATT_FS_LOW_ACT', 0)
|
|
|
|
self.progress("Successfully entered LAND after battery failsafe")
|
|
|
|
# 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.mavproxy.send('param set SIM_ENGINE_MUL 0.60\n')
|
|
|
|
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:
|
|
self.progress("Loiter alt shifted %u meters (> limit of %u)" %
|
|
(alt_delta, maxaltchange))
|
|
raise NotAchievedException()
|
|
if delta > maxdistchange:
|
|
self.progress("Loiter shifted %u meters (> limit of %u)" %
|
|
(delta, maxdistchange))
|
|
raise NotAchievedException()
|
|
|
|
# restore motor 1 to 100% efficiency
|
|
self.mavproxy.send('param set SIM_ENGINE_MUL 1.0\n')
|
|
|
|
self.progress("Stability patch and Loiter OK for %u seconds" % 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.mavproxy.send('param set FENCE_ENABLE 1\n')
|
|
self.mavproxy.send('param set AVOID_ENABLE 0\n')
|
|
|
|
# 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='VFR_HUD', blocking=True)
|
|
pos = self.mav.location()
|
|
home_distance = self.get_distance(HOME, pos)
|
|
self.progress("Alt: %u HomeDistance: %.0f" %
|
|
(m.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, 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')
|
|
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.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')
|
|
self.progress("Fence test failed to reach home - "
|
|
"timed out after %u seconds" % timeout)
|
|
raise AutoTestTimeoutException()
|
|
|
|
# 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)
|
|
|
|
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')
|
|
|
|
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()
|
|
success = True
|
|
|
|
# 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.mavproxy.send('param set SIM_GPS_GLITCH_X 0\n')
|
|
self.mavproxy.send('param set SIM_GPS_GLITCH_Y 0\n')
|
|
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='VFR_HUD', blocking=True)
|
|
curr_pos = self.sim_location()
|
|
moved_distance = self.get_distance(curr_pos, start_pos)
|
|
self.progress("Alt: %u Moved: %.0f" % (m.alt, moved_distance))
|
|
if moved_distance > max_distance:
|
|
self.progress("Moved over %u meters, Failed!" %
|
|
max_distance)
|
|
raise NotAchievedException()
|
|
|
|
# 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)
|
|
|
|
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:
|
|
self.progress("load copter_glitch_mission failed")
|
|
raise NotAchievedException()
|
|
|
|
# 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, 60)
|
|
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.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
|
|
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):
|
|
self.progress("GPS Glitch testing failed"
|
|
"- exceeded timeout %u seconds" % timeout)
|
|
raise AutoTestTimeoutException()
|
|
|
|
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, timeout=120):
|
|
|
|
# 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
|
|
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.mavproxy.send('param set SIMPLE 0\n')
|
|
|
|
# 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.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
|
|
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.mavproxy.send('param set SUPER_SIMPLE 0\n')
|
|
|
|
# hover in place
|
|
self.hover()
|
|
|
|
# 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
|
|
self.progress("turn west")
|
|
self.set_rc(4, 1580)
|
|
self.wait_heading(270)
|
|
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)
|
|
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_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:
|
|
self.progress("load copter_mission failed")
|
|
raise NotAchievedException()
|
|
|
|
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
|
|
if not self.wait_waypoint(0, num_wp-1, timeout=500):
|
|
self.land()
|
|
|
|
# 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:
|
|
self.progress("load copter_AVC2013_mission failed")
|
|
raise NotAchievedException()
|
|
|
|
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_mission(self, height_accuracy=-1.0, target_altitude=None):
|
|
"""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')
|
|
|
|
def autotest(self):
|
|
"""Autotest ArduCopter in SITL."""
|
|
if not self.hasInit:
|
|
self.init()
|
|
|
|
self.fail_list = []
|
|
|
|
try:
|
|
self.progress("Waiting for a heartbeat with mavlink protocol %s"
|
|
% self.mav.WIRE_PROTOCOL_VERSION)
|
|
self.mav.wait_heartbeat()
|
|
self.progress("Setting up RC parameters")
|
|
self.set_rc_default()
|
|
self.set_rc(3, 1000)
|
|
self.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()
|
|
|
|
# Arm
|
|
self.run_test("Arm motors", self.arm_vehicle)
|
|
|
|
# Takeoff
|
|
self.run_test("Takeoff to test fly Square", lambda: self.takeoff(10))
|
|
|
|
# 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", lambda: self.fly_mission(height_accuracy=0.5, target_altitude=10))
|
|
|
|
# Throttle Failsafe
|
|
self.run_test("Test Failsafe", lambda: 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", lambda: 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", lambda: 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))
|
|
|
|
# Fence test
|
|
self.run_test("Test Max Alt Fence", lambda: self.fly_alt_max_fence_test(180))
|
|
|
|
# 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)
|
|
|
|
# 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)
|
|
|
|
# RTL
|
|
self.run_test("RTL after CIRCLE mode", self.fly_RTL)
|
|
|
|
# Fly auto test
|
|
self.run_test("Fly copter mission", self.fly_auto_test)
|
|
|
|
# wait for disarm
|
|
self.mav.motors_disarmed_wait()
|
|
|
|
# Download logs
|
|
self.run_test("log download", lambda: self.log_download(self.buildlogs_path("ArduCopter-log.bin")))
|
|
|
|
except pexpect.TIMEOUT as e:
|
|
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()
|
|
|
|
# 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 as e:
|
|
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
|