mirror of https://github.com/ArduPilot/ardupilot
778 lines
27 KiB
Python
778 lines
27 KiB
Python
#!/usr/bin/env python
|
|
|
|
# Fly ArduPlane in SITL
|
|
from __future__ import print_function
|
|
import math
|
|
import os
|
|
|
|
import pexpect
|
|
from pymavlink import quaternion
|
|
from pymavlink import mavutil
|
|
|
|
from pysim import util
|
|
|
|
from common import AutoTest
|
|
from common import NotAchievedException
|
|
from common import PreconditionFailedException
|
|
|
|
# get location of scripts
|
|
testdir = os.path.dirname(os.path.realpath(__file__))
|
|
HOME = mavutil.location(-35.362938, 149.165085, 585, 354)
|
|
WIND = "0,180,0.2" # speed,direction,variance
|
|
|
|
|
|
class AutoTestPlane(AutoTest):
|
|
def __init__(self,
|
|
binary,
|
|
valgrind=False,
|
|
gdb=False,
|
|
speedup=10,
|
|
frame=None,
|
|
params=None,
|
|
gdbserver=False,
|
|
breakpoints=[],
|
|
**kwargs):
|
|
super(AutoTestPlane, self).__init__(**kwargs)
|
|
self.binary = binary
|
|
self.valgrind = valgrind
|
|
self.gdb = gdb
|
|
self.frame = frame
|
|
self.params = params
|
|
self.gdbserver = gdbserver
|
|
self.breakpoints = breakpoints
|
|
|
|
self.home = "%f,%f,%u,%u" % (HOME.lat,
|
|
HOME.lng,
|
|
HOME.alt,
|
|
HOME.heading)
|
|
self.homeloc = None
|
|
self.speedup = speedup
|
|
|
|
self.sitl = None
|
|
self.hasInit = False
|
|
|
|
self.log_name = "ArduPlane"
|
|
|
|
def init(self):
|
|
if self.frame is None:
|
|
self.frame = 'plane-elevrev'
|
|
|
|
defaults_file = os.path.join(testdir,
|
|
'default_params/plane-jsbsim.parm')
|
|
self.sitl = util.start_SITL(self.binary,
|
|
wipe=True,
|
|
model=self.frame,
|
|
home=self.home,
|
|
speedup=self.speedup,
|
|
defaults_file=defaults_file,
|
|
valgrind=self.valgrind,
|
|
gdb=self.gdb,
|
|
gdbserver=self.gdbserver,
|
|
breakpoints=self.breakpoints)
|
|
self.mavproxy = util.start_MAVProxy_SITL(
|
|
'ArduPlane', 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.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")
|
|
|
|
self.get_mavlink_connection_going()
|
|
|
|
self.hasInit = True
|
|
self.progress("Ready to start testing!")
|
|
|
|
def takeoff(self):
|
|
"""Takeoff get to 30m altitude."""
|
|
|
|
self.mavproxy.send('switch 4\n')
|
|
self.wait_mode('FBWA')
|
|
|
|
self.wait_ready_to_arm()
|
|
self.arm_vehicle()
|
|
|
|
# some rudder to counteract the prop torque
|
|
self.set_rc(4, 1700)
|
|
|
|
# some up elevator to keep the tail down
|
|
self.set_rc(2, 1200)
|
|
|
|
# get it moving a bit first
|
|
self.set_rc(3, 1300)
|
|
self.mav.recv_match(condition='VFR_HUD.groundspeed>6', blocking=True)
|
|
|
|
# a bit faster again, straighten rudder
|
|
self.set_rc(3, 1600)
|
|
self.set_rc(4, 1500)
|
|
self.mav.recv_match(condition='VFR_HUD.groundspeed>12', blocking=True)
|
|
|
|
# hit the gas harder now, and give it some more elevator
|
|
self.set_rc(2, 1100)
|
|
self.set_rc(3, 2000)
|
|
|
|
# gain a bit of altitude
|
|
self.wait_altitude(self.homeloc.alt+150,
|
|
self.homeloc.alt+180,
|
|
timeout=30)
|
|
|
|
# level off
|
|
self.set_rc(2, 1500)
|
|
|
|
self.progress("TAKEOFF COMPLETE")
|
|
|
|
def fly_left_circuit(self):
|
|
"""Fly a left circuit, 200m on a side."""
|
|
self.mavproxy.send('switch 4\n')
|
|
self.wait_mode('FBWA')
|
|
self.set_rc(3, 2000)
|
|
self.wait_level_flight()
|
|
|
|
self.progress("Flying left circuit")
|
|
# do 4 turns
|
|
for i in range(0, 4):
|
|
# hard left
|
|
self.progress("Starting turn %u" % i)
|
|
self.set_rc(1, 1000)
|
|
self.wait_heading(270 - (90*i), accuracy=10)
|
|
self.set_rc(1, 1500)
|
|
self.progress("Starting leg %u" % i)
|
|
self.wait_distance(100, accuracy=20)
|
|
self.progress("Circuit complete")
|
|
|
|
def fly_RTL(self):
|
|
"""Fly to home."""
|
|
self.progress("Flying home in RTL")
|
|
self.mavproxy.send('switch 2\n')
|
|
self.wait_mode('RTL')
|
|
self.wait_location(self.homeloc,
|
|
accuracy=120,
|
|
target_altitude=self.homeloc.alt+100,
|
|
height_accuracy=20,
|
|
timeout=180)
|
|
self.progress("RTL Complete")
|
|
|
|
def fly_LOITER(self, num_circles=4):
|
|
"""Loiter where we are."""
|
|
self.progress("Testing LOITER for %u turns" % num_circles)
|
|
self.mavproxy.send('loiter\n')
|
|
self.wait_mode('LOITER')
|
|
|
|
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
|
|
initial_alt = m.alt
|
|
self.progress("Initial altitude %u\n" % initial_alt)
|
|
|
|
while num_circles > 0:
|
|
self.wait_heading(0, accuracy=10, timeout=60)
|
|
self.wait_heading(180, accuracy=10, timeout=60)
|
|
num_circles -= 1
|
|
self.progress("Loiter %u circles left" % num_circles)
|
|
|
|
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
|
|
final_alt = m.alt
|
|
self.progress("Final altitude %u initial %u\n" %
|
|
(final_alt, initial_alt))
|
|
|
|
self.mavproxy.send('mode FBWA\n')
|
|
self.wait_mode('FBWA')
|
|
|
|
if abs(final_alt - initial_alt) > 20:
|
|
raise NotAchievedException("Failed to maintain altitude")
|
|
|
|
self.progress("Completed Loiter OK")
|
|
|
|
def fly_CIRCLE(self, num_circles=1):
|
|
"""Circle where we are."""
|
|
self.progress("Testing CIRCLE for %u turns" % num_circles)
|
|
self.mavproxy.send('mode CIRCLE\n')
|
|
self.wait_mode('CIRCLE')
|
|
|
|
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
|
|
initial_alt = m.alt
|
|
self.progress("Initial altitude %u\n" % initial_alt)
|
|
|
|
while num_circles > 0:
|
|
self.wait_heading(0, accuracy=10, timeout=60)
|
|
self.wait_heading(180, accuracy=10, timeout=60)
|
|
num_circles -= 1
|
|
self.progress("CIRCLE %u circles left" % num_circles)
|
|
|
|
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
|
|
final_alt = m.alt
|
|
self.progress("Final altitude %u initial %u\n" %
|
|
(final_alt, initial_alt))
|
|
|
|
self.mavproxy.send('mode FBWA\n')
|
|
self.wait_mode('FBWA')
|
|
|
|
if abs(final_alt - initial_alt) > 20:
|
|
raise NotAchievedException("Failed to maintain altitude")
|
|
|
|
self.progress("Completed CIRCLE OK")
|
|
|
|
def wait_level_flight(self, accuracy=5, timeout=30):
|
|
"""Wait for level flight."""
|
|
tstart = self.get_sim_time()
|
|
self.progress("Waiting for level flight")
|
|
self.set_rc(1, 1500)
|
|
self.set_rc(2, 1500)
|
|
self.set_rc(4, 1500)
|
|
while self.get_sim_time() < tstart + timeout:
|
|
m = self.mav.recv_match(type='ATTITUDE', blocking=True)
|
|
roll = math.degrees(m.roll)
|
|
pitch = math.degrees(m.pitch)
|
|
self.progress("Roll=%.1f Pitch=%.1f" % (roll, pitch))
|
|
if math.fabs(roll) <= accuracy and math.fabs(pitch) <= accuracy:
|
|
self.progress("Attained level flight")
|
|
return
|
|
raise NotAchievedException("Failed to attain level flight")
|
|
|
|
def change_altitude(self, altitude, accuracy=30):
|
|
"""Get to a given altitude."""
|
|
self.mavproxy.send('mode FBWA\n')
|
|
self.wait_mode('FBWA')
|
|
alt_error = self.mav.messages['VFR_HUD'].alt - altitude
|
|
if alt_error > 0:
|
|
self.set_rc(2, 2000)
|
|
else:
|
|
self.set_rc(2, 1000)
|
|
self.wait_altitude(altitude-accuracy/2, altitude+accuracy/2)
|
|
self.set_rc(2, 1500)
|
|
self.progress("Reached target altitude at %u" %
|
|
self.mav.messages['VFR_HUD'].alt)
|
|
return self.wait_level_flight()
|
|
|
|
def axial_left_roll(self, count=1):
|
|
"""Fly a left axial roll."""
|
|
# full throttle!
|
|
self.set_rc(3, 2000)
|
|
self.change_altitude(self.homeloc.alt+300)
|
|
|
|
# fly the roll in manual
|
|
self.mavproxy.send('switch 6\n')
|
|
self.wait_mode('MANUAL')
|
|
|
|
while count > 0:
|
|
self.progress("Starting roll")
|
|
self.set_rc(1, 1000)
|
|
try:
|
|
self.wait_roll(-150, accuracy=90)
|
|
self.wait_roll(150, accuracy=90)
|
|
self.wait_roll(0, accuracy=90)
|
|
except Exception as e:
|
|
self.set_rc(1, 1500)
|
|
raise e
|
|
count -= 1
|
|
|
|
# back to FBWA
|
|
self.set_rc(1, 1500)
|
|
self.mavproxy.send('switch 4\n')
|
|
self.wait_mode('FBWA')
|
|
self.set_rc(3, 1700)
|
|
return self.wait_level_flight()
|
|
|
|
def inside_loop(self, count=1):
|
|
"""Fly a inside loop."""
|
|
# full throttle!
|
|
self.set_rc(3, 2000)
|
|
self.change_altitude(self.homeloc.alt+300)
|
|
# fly the loop in manual
|
|
self.mavproxy.send('switch 6\n')
|
|
self.wait_mode('MANUAL')
|
|
|
|
while count > 0:
|
|
self.progress("Starting loop")
|
|
self.set_rc(2, 1000)
|
|
self.wait_pitch(-60, accuracy=20)
|
|
self.wait_pitch(0, accuracy=20)
|
|
count -= 1
|
|
|
|
# back to FBWA
|
|
self.set_rc(2, 1500)
|
|
self.mavproxy.send('switch 4\n')
|
|
self.wait_mode('FBWA')
|
|
self.set_rc(3, 1700)
|
|
return self.wait_level_flight()
|
|
|
|
def set_attitude_target(self):
|
|
"""Test setting of attitude target in guided mode."""
|
|
# mode guided:
|
|
self.mavproxy.send('mode GUIDED\n')
|
|
self.wait_mode('GUIDED')
|
|
|
|
target_roll_degrees = 70
|
|
state_roll_over = "roll-over"
|
|
state_stabilize_roll = "stabilize-roll"
|
|
state_hold = "hold"
|
|
state_roll_back = "roll-back"
|
|
state_done = "done"
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
try:
|
|
state = state_roll_over
|
|
while state != state_done:
|
|
if self.get_sim_time() - tstart > 20:
|
|
raise AutoTestTimeoutException("Manuevers not completed")
|
|
|
|
m = self.mav.recv_match(type='ATTITUDE',
|
|
blocking=True,
|
|
timeout=0.1)
|
|
if m is None:
|
|
continue
|
|
|
|
r = math.degrees(m.roll)
|
|
if state == state_roll_over:
|
|
target_roll_degrees = 70
|
|
if abs(r - target_roll_degrees) < 10:
|
|
state = state_stabilize_roll
|
|
stabilize_start = self.get_sim_time()
|
|
elif state == state_stabilize_roll:
|
|
# just give it a little time to sort it self out
|
|
if self.get_sim_time() - stabilize_start > 2:
|
|
state = state_hold
|
|
hold_start = self.get_sim_time()
|
|
elif state == state_hold:
|
|
target_roll_degrees = 70
|
|
if self.get_sim_time() - hold_start > 10:
|
|
state = state_roll_back
|
|
if abs(r - target_roll_degrees) > 10:
|
|
raise NotAchievedException("Failed to hold attitude")
|
|
elif state == state_roll_back:
|
|
target_roll_degrees = 0
|
|
if abs(r - target_roll_degrees) < 10:
|
|
state = state_done
|
|
else:
|
|
raise ValueError("Unknown state %s" % str(state))
|
|
|
|
self.progress("%s Roll: %f desired=%f" %
|
|
(state, r, target_roll_degrees))
|
|
|
|
time_boot_millis = 0 # FIXME
|
|
target_system = 1 # FIXME
|
|
target_component = 1 # FIXME
|
|
type_mask = 0b10000001 ^ 0xFF # FIXME
|
|
# attitude in radians:
|
|
q = quaternion.Quaternion([math.radians(target_roll_degrees),
|
|
0 ,
|
|
0])
|
|
roll_rate_radians = 0.5
|
|
pitch_rate_radians = 0
|
|
yaw_rate_radians = 0
|
|
thrust = 1.0
|
|
self.mav.mav.set_attitude_target_send(time_boot_millis,
|
|
target_system,
|
|
target_component,
|
|
type_mask,
|
|
q,
|
|
roll_rate_radians,
|
|
pitch_rate_radians,
|
|
yaw_rate_radians,
|
|
thrust)
|
|
except Exception as e:
|
|
self.mavproxy.send('mode FBWA\n')
|
|
self.wait_mode('FBWA')
|
|
self.set_rc(3, 1700)
|
|
raise e
|
|
|
|
# back to FBWA
|
|
self.mavproxy.send('mode FBWA\n')
|
|
self.wait_mode('FBWA')
|
|
self.set_rc(3, 1700)
|
|
self.wait_level_flight()
|
|
|
|
def test_stabilize(self, count=1):
|
|
"""Fly stabilize mode."""
|
|
# full throttle!
|
|
self.set_rc(3, 2000)
|
|
self.set_rc(2, 1300)
|
|
self.change_altitude(self.homeloc.alt+300)
|
|
self.set_rc(2, 1500)
|
|
|
|
self.mavproxy.send("mode STABILIZE\n")
|
|
self.wait_mode('STABILIZE')
|
|
|
|
while count > 0:
|
|
self.progress("Starting roll")
|
|
self.set_rc(1, 2000)
|
|
self.wait_roll(-150, accuracy=90)
|
|
self.wait_roll(150, accuracy=90)
|
|
self.wait_roll(0, accuracy=90)
|
|
count -= 1
|
|
|
|
self.set_rc(1, 1500)
|
|
self.wait_roll(0, accuracy=5)
|
|
|
|
# back to FBWA
|
|
self.mavproxy.send('mode FBWA\n')
|
|
self.wait_mode('FBWA')
|
|
self.set_rc(3, 1700)
|
|
return self.wait_level_flight()
|
|
|
|
def test_acro(self, count=1):
|
|
"""Fly ACRO mode."""
|
|
# full throttle!
|
|
self.set_rc(3, 2000)
|
|
self.set_rc(2, 1300)
|
|
self.change_altitude(self.homeloc.alt+300)
|
|
self.set_rc(2, 1500)
|
|
|
|
self.mavproxy.send("mode ACRO\n")
|
|
self.wait_mode('ACRO')
|
|
|
|
while count > 0:
|
|
self.progress("Starting roll")
|
|
self.set_rc(1, 1000)
|
|
self.wait_roll(-150, accuracy=90)
|
|
self.wait_roll(150, accuracy=90)
|
|
self.wait_roll(0, accuracy=90)
|
|
count -= 1
|
|
self.set_rc(1, 1500)
|
|
|
|
# back to FBWA
|
|
self.mavproxy.send('mode FBWA\n')
|
|
self.wait_mode('FBWA')
|
|
|
|
self.wait_level_flight()
|
|
|
|
self.mavproxy.send("mode ACRO\n")
|
|
self.wait_mode('ACRO')
|
|
|
|
count = 2
|
|
while count > 0:
|
|
self.progress("Starting loop")
|
|
self.set_rc(2, 1000)
|
|
self.wait_pitch(-60, accuracy=20)
|
|
self.wait_pitch(0, accuracy=20)
|
|
count -= 1
|
|
|
|
self.set_rc(2, 1500)
|
|
|
|
# back to FBWA
|
|
self.mavproxy.send('mode FBWA\n')
|
|
self.wait_mode('FBWA')
|
|
self.set_rc(3, 1700)
|
|
return self.wait_level_flight()
|
|
|
|
def test_FBWB(self, mode='FBWB'):
|
|
"""Fly FBWB or CRUISE mode."""
|
|
self.mavproxy.send("mode %s\n" % mode)
|
|
self.wait_mode(mode)
|
|
self.set_rc(3, 1700)
|
|
self.set_rc(2, 1500)
|
|
|
|
# lock in the altitude by asking for an altitude change then releasing
|
|
self.set_rc(2, 1000)
|
|
self.wait_distance(50, accuracy=20)
|
|
self.set_rc(2, 1500)
|
|
self.wait_distance(50, accuracy=20)
|
|
|
|
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
|
|
initial_alt = m.alt
|
|
self.progress("Initial altitude %u\n" % initial_alt)
|
|
|
|
self.progress("Flying right circuit")
|
|
# do 4 turns
|
|
for i in range(0, 4):
|
|
# hard left
|
|
self.progress("Starting turn %u" % i)
|
|
self.set_rc(1, 1800)
|
|
try:
|
|
self.wait_heading(0 + (90*i), accuracy=20, timeout=60)
|
|
except Exception as e:
|
|
self.set_rc(1, 1500)
|
|
raise e
|
|
self.set_rc(1, 1500)
|
|
self.progress("Starting leg %u" % i)
|
|
self.wait_distance(100, accuracy=20)
|
|
self.progress("Circuit complete")
|
|
|
|
self.progress("Flying rudder left circuit")
|
|
# do 4 turns
|
|
for i in range(0, 4):
|
|
# hard left
|
|
self.progress("Starting turn %u" % i)
|
|
self.set_rc(4, 1900)
|
|
try:
|
|
self.wait_heading(360 - (90*i), accuracy=20, timeout=60)
|
|
except Exception as e:
|
|
self.set_rc(4, 1500)
|
|
raise e
|
|
self.set_rc(4, 1500)
|
|
self.progress("Starting leg %u" % i)
|
|
self.wait_distance(100, accuracy=20)
|
|
self.progress("Circuit complete")
|
|
|
|
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
|
|
final_alt = m.alt
|
|
self.progress("Final altitude %u initial %u\n" %
|
|
(final_alt, initial_alt))
|
|
|
|
# back to FBWA
|
|
self.mavproxy.send('mode FBWA\n')
|
|
self.wait_mode('FBWA')
|
|
|
|
if abs(final_alt - initial_alt) > 20:
|
|
raise NotAchievedException("Failed to maintain altitude")
|
|
|
|
return self.wait_level_flight()
|
|
|
|
def wp_load(self, 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')
|
|
|
|
def fly_mission(self, filename):
|
|
"""Fly a mission from a file."""
|
|
self.progress("Flying mission %s" % filename)
|
|
self.wp_load(filename)
|
|
self.mavproxy.send('switch 1\n') # auto mode
|
|
self.wait_mode('AUTO')
|
|
self.wait_waypoint(1, 7, max_dist=60)
|
|
self.wait_groundspeed(0, 0.5, timeout=60)
|
|
self.mavproxy.expect("Auto disarmed")
|
|
self.progress("Mission OK")
|
|
|
|
def fly_flaps(self):
|
|
"""Test flaps functionality."""
|
|
filename = os.path.join(testdir, "flaps.txt")
|
|
self.context_push()
|
|
ex = None
|
|
try:
|
|
flaps_ch = 5
|
|
servo_ch = 5
|
|
self.set_parameter("SERVO%u_FUNCTION" % servo_ch, 3) # flapsauto
|
|
self.set_parameter("FLAP_IN_CHANNEL", flaps_ch)
|
|
self.set_parameter("LAND_FLAP_PERCNT", 50)
|
|
self.set_parameter("LOG_DISARMED", 1)
|
|
flaps_ch_min = 1000
|
|
flaps_ch_trim = 1500
|
|
flaps_ch_max = 2000
|
|
self.set_parameter("RC%u_MIN" % flaps_ch, flaps_ch_min)
|
|
self.set_parameter("RC%u_MAX" % flaps_ch, flaps_ch_max)
|
|
self.set_parameter("RC%u_TRIM" % flaps_ch, flaps_ch_trim)
|
|
|
|
servo_ch_min = 1200
|
|
servo_ch_trim = 1300
|
|
servo_ch_max = 1800
|
|
self.set_parameter("SERVO%u_MIN" % servo_ch, servo_ch_min)
|
|
self.set_parameter("SERVO%u_MAX" % servo_ch, servo_ch_max)
|
|
self.set_parameter("SERVO%u_TRIM" % servo_ch, servo_ch_trim)
|
|
|
|
# check flaps are not deployed:
|
|
self.set_rc(flaps_ch, flaps_ch_min)
|
|
self.wait_servo_channel_value(servo_ch, servo_ch_min)
|
|
# deploy the flaps:
|
|
self.set_rc(flaps_ch, flaps_ch_max)
|
|
tstart = self.get_sim_time()
|
|
self.wait_servo_channel_value(servo_ch, servo_ch_max)
|
|
tstop = self.get_sim_time_cached()
|
|
delta_time = tstop - tstart
|
|
delta_time_min = 0.5
|
|
delta_time_max = 1.5
|
|
if delta_time < delta_time_min or delta_time > delta_time_max:
|
|
raise NotAchievedException((
|
|
"Flaps Slew not working (%f seconds)" % (delta_time,)))
|
|
# undeploy flaps:
|
|
self.set_rc(flaps_ch, flaps_ch_min)
|
|
self.wait_servo_channel_value(servo_ch, servo_ch_min)
|
|
|
|
self.progress("Flying mission %s" % filename)
|
|
self.wp_load(filename)
|
|
self.mavproxy.send('wp set 1\n')
|
|
self.mavproxy.send('switch 1\n') # auto mode
|
|
self.wait_mode('AUTO')
|
|
self.wait_ready_to_arm()
|
|
self.arm_vehicle()
|
|
tstart = self.get_sim_time_cached()
|
|
last_mission_current_msg = 0
|
|
last_seq = None
|
|
while self.armed():
|
|
m = self.mav.recv_match(type='MISSION_CURRENT', blocking=True)
|
|
time_delta = (self.get_sim_time_cached() -
|
|
last_mission_current_msg)
|
|
if (time_delta >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,str(dist)))
|
|
last_mission_current_msg = self.get_sim_time_cached()
|
|
last_seq = m.seq
|
|
# flaps should undeploy at the end
|
|
self.wait_servo_channel_value(servo_ch, servo_ch_min, timeout=30)
|
|
|
|
# do a short flight in FBWA, watching for flaps
|
|
# self.mavproxy.send('switch 4\n')
|
|
# self.wait_mode('FBWA')
|
|
# self.wait_seconds(10)
|
|
# self.mavproxy.send('switch 6\n')
|
|
# self.wait_mode('MANUAL')
|
|
# self.wait_seconds(10)
|
|
|
|
self.progress("Flaps OK")
|
|
except Exception as e:
|
|
ex = e
|
|
self.context_pop()
|
|
if ex:
|
|
if self.armed():
|
|
self.disarm_vehicle()
|
|
raise ex
|
|
|
|
def test_rc_relay(self):
|
|
'''test toggling channel 12 toggles relay'''
|
|
off = self.get_parameter("SIM_PIN_MASK")
|
|
if off:
|
|
raise PreconditionFailedException("SIM_MASK_PIN off")
|
|
self.set_rc(12, 2000)
|
|
self.mav.wait_heartbeat()
|
|
self.mav.wait_heartbeat()
|
|
on = self.get_parameter("SIM_PIN_MASK")
|
|
if not on:
|
|
raise NotAchievedException("SIM_PIN_MASK doesn't reflect ON")
|
|
self.set_rc(12, 1000)
|
|
self.mav.wait_heartbeat()
|
|
self.mav.wait_heartbeat()
|
|
off = self.get_parameter("SIM_PIN_MASK")
|
|
if off:
|
|
raise NotAchievedException("SIM_PIN_MASK doesn't reflect OFF")
|
|
|
|
def test_rc_option_camera_trigger(self):
|
|
'''test toggling channel 12 takes picture'''
|
|
x = self.mav.messages.get("CAMERA_FEEDBACK", None)
|
|
if x is not None:
|
|
raise PreconditionFailedException("Receiving CAMERA_FEEDBACK?!")
|
|
self.set_rc(12, 2000)
|
|
tstart = self.get_sim_time()
|
|
while self.get_sim_time() - tstart < 10:
|
|
x = self.mav.messages.get("CAMERA_FEEDBACK", None)
|
|
if x is not None:
|
|
break
|
|
self.mav.wait_heartbeat()
|
|
self.set_rc(12, 1000)
|
|
if x is None:
|
|
raise NotAchievedException("No CAMERA_FEEDBACK message received")
|
|
|
|
def test_gripper_mission(self):
|
|
self.context_push()
|
|
ex = None
|
|
try:
|
|
self.wp_load(os.path.join(testdir, "plane-gripper-mission.txt"))
|
|
self.mavproxy.send("wp set 1\n")
|
|
self.mavproxy.send('switch 1\n') # auto mode
|
|
self.wait_mode('AUTO')
|
|
self.wait_ready_to_arm()
|
|
self.arm_vehicle()
|
|
self.mavproxy.expect("Gripper Grabbed")
|
|
self.mavproxy.expect("Gripper Released")
|
|
self.mavproxy.expect("Auto disarmed")
|
|
except Exception as e:
|
|
self.progress("Exception caught")
|
|
ex = e
|
|
self.context_pop()
|
|
if ex is not None:
|
|
raise ex
|
|
|
|
def autotest(self):
|
|
"""Autotest ArduPlane 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.set_rc(8, 1800)
|
|
|
|
self.set_parameter("RC12_OPTION", 9)
|
|
self.reboot_sitl() # needed for RC12_OPTION to take effect
|
|
|
|
self.run_test("Test RC Option - Camera Trigger",
|
|
self.test_rc_option_camera_trigger)
|
|
|
|
self.set_parameter("RC12_OPTION", 28)
|
|
self.reboot_sitl() # needed for RC12_OPTION to take effect
|
|
|
|
self.run_test("Test Relay RC Channel Option",
|
|
self.test_rc_relay)
|
|
|
|
self.progress("Waiting for GPS fix")
|
|
self.mav.recv_match(condition='VFR_HUD.alt>10', blocking=True)
|
|
self.mav.wait_gps_fix()
|
|
while self.mav.location().alt < 10:
|
|
self.mav.wait_gps_fix()
|
|
self.homeloc = self.mav.location()
|
|
self.progress("Home location: %s" % self.homeloc)
|
|
self.wait_ready_to_arm()
|
|
self.run_test("Arm features", self.test_arm_feature)
|
|
|
|
self.run_test("Flaps", self.fly_flaps)
|
|
|
|
self.mavproxy.send('switch 6\n')
|
|
self.wait_mode('MANUAL')
|
|
|
|
self.run_test("Takeoff", self.takeoff)
|
|
|
|
self.run_test("Set Attitude Target", self.set_attitude_target)
|
|
|
|
self.run_test("Fly left circuit", self.fly_left_circuit)
|
|
|
|
self.run_test("Left roll", lambda: self.axial_left_roll(1))
|
|
|
|
self.run_test("Inside loop", self.inside_loop)
|
|
|
|
self.run_test("Stablize test", self.test_stabilize)
|
|
|
|
self.run_test("ACRO test", self.test_acro)
|
|
|
|
self.run_test("FBWB test", self.test_FBWB)
|
|
|
|
self.run_test("CRUISE test", lambda: self.test_FBWB(mode='CRUISE'))
|
|
|
|
self.run_test("RTL test", self.fly_RTL)
|
|
|
|
self.run_test("LOITER test", self.fly_LOITER)
|
|
|
|
self.run_test("CIRCLE test", self.fly_CIRCLE)
|
|
|
|
self.run_test("Mission test",
|
|
lambda: self.fly_mission(
|
|
os.path.join(testdir, "ap1.txt")))
|
|
|
|
self.run_test("Test Gripper mission items",
|
|
self.test_gripper_mission)
|
|
|
|
self.run_test("Log download",
|
|
lambda: self.log_download(
|
|
self.buildlogs_path("ArduPlane-log.bin"),
|
|
upload_logs=True))
|
|
|
|
except pexpect.TIMEOUT:
|
|
self.progress("Failed with timeout")
|
|
self.fail_list.append("timeout")
|
|
|
|
self.close()
|
|
|
|
if len(self.fail_list):
|
|
self.progress("FAILED: %s" % self.fail_list)
|
|
return False
|
|
|
|
self.progress("Max set_rc_timeout=%s" % self.max_set_rc_timeout);
|
|
|
|
return True
|