Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/Tools/autotest/quadplane.py

1529 lines
58 KiB
Python
Raw Blame History

'''
Fly ArduPlane QuadPlane in SITL
AP_FLAKE8_CLEAN
'''
from __future__ import print_function
import os
import numpy
import math
from pymavlink import mavutil
from pymavlink.rotmat import Vector3
from common import AutoTest
from common import Test
from common import AutoTestTimeoutException, NotAchievedException, PreconditionFailedException
import operator
# get location of scripts
testdir = os.path.dirname(os.path.realpath(__file__))
WIND = "0,180,0.2" # speed,direction,variance
SITL_START_LOCATION = mavutil.location(-27.274439, 151.290064, 343, 8.7)
class AutoTestQuadPlane(AutoTest):
@staticmethod
def get_not_armable_mode_list():
return []
@staticmethod
def get_not_disarmed_settable_modes_list():
return []
@staticmethod
def get_no_position_not_settable_modes_list():
return []
@staticmethod
def get_position_armable_modes_list():
return []
@staticmethod
def get_normal_armable_modes_list():
return []
def vehicleinfo_key(self):
return 'ArduPlane'
def default_frame(self):
return "quadplane"
def test_filepath(self):
return os.path.realpath(__file__)
def sitl_start_location(self):
return SITL_START_LOCATION
def default_speedup(self):
'''QuadPlane seems to be race-free'''
return 100
def log_name(self):
return "QuadPlane"
def set_current_test_name(self, name):
self.current_test_name_directory = "ArduPlane_Tests/" + name + "/"
def apply_defaultfile_parameters(self):
# plane passes in a defaults_filepath in place of applying
# parameters afterwards.
pass
def defaults_filepath(self):
return self.model_defaults_filepath(self.frame)
def is_plane(self):
return True
def get_stick_arming_channel(self):
return int(self.get_parameter("RCMAP_YAW"))
def get_disarm_delay(self):
return int(self.get_parameter("LAND_DISARMDELAY"))
def set_autodisarm_delay(self, delay):
self.set_parameter("LAND_DISARMDELAY", delay)
def AirMode(self):
"""Check that plane.air_mode turns on and off as required"""
self.progress("########## Testing AirMode operation")
self.set_parameter("AHRS_EKF_TYPE", 10)
self.change_mode('QSTABILIZE')
self.wait_ready_to_arm()
"""
SPIN_ARM and SPIN_MIN default to 0.10 and 0.15
when armed with zero throttle in AirMode, motor PWM should be at SPIN_MIN
If AirMode is off, motor PWM will drop to SPIN_ARM
"""
self.progress("Verify that SERVO5 is Motor1 (default)")
motor1_servo_function_lp = 33
if (self.get_parameter('SERVO5_FUNCTION') != motor1_servo_function_lp):
raise PreconditionFailedException("SERVO5_FUNCTION not %d" % motor1_servo_function_lp)
self.progress("Verify that flightmode channel is 5 (default)")
default_fltmode_ch = 5
if (self.get_parameter("FLTMODE_CH") != default_fltmode_ch):
raise PreconditionFailedException("FLTMODE_CH not %d" % default_fltmode_ch)
"""When disarmed, motor PWM will drop to min_pwm"""
min_pwm = self.get_parameter("Q_M_PWM_MIN")
self.progress("Verify Motor1 is at min_pwm when disarmed")
self.wait_servo_channel_value(5, min_pwm, comparator=operator.eq)
armdisarm_option = 154
arm_ch = 8
self.set_parameter("RC%d_OPTION" % arm_ch, armdisarm_option)
self.progress("Configured RC%d as ARMDISARM switch" % arm_ch)
"""arm with GCS, record Motor1 SPIN_ARM PWM output and disarm"""
spool_delay = self.get_parameter("Q_M_SPOOL_TIME") + 0.25
self.zero_throttle()
self.arm_vehicle()
self.progress("Waiting for Motor1 to spool up to SPIN_ARM")
self.delay_sim_time(spool_delay)
spin_arm_pwm = self.wait_servo_channel_value(5, min_pwm, comparator=operator.gt)
self.progress("spin_arm_pwm: %d" % spin_arm_pwm)
self.disarm_vehicle()
"""arm with switch, record Motor1 SPIN_MIN PWM output and disarm"""
self.set_rc(8, 2000)
self.delay_sim_time(spool_delay)
self.progress("Waiting for Motor1 to spool up to SPIN_MIN")
spin_min_pwm = self.wait_servo_channel_value(5, spin_arm_pwm, comparator=operator.gt)
self.progress("spin_min_pwm: %d" % spin_min_pwm)
self.set_rc(8, 1000)
if (spin_arm_pwm >= spin_min_pwm):
raise PreconditionFailedException("SPIN_MIN pwm not greater than SPIN_ARM pwm")
self.start_subtest("Test auxswitch arming with AirMode Switch")
for mode in ('QSTABILIZE', 'QACRO'):
"""verify that arming with switch results in higher PWM output"""
self.progress("Testing %s mode" % mode)
self.change_mode(mode)
self.zero_throttle()
self.progress("Arming with switch at zero throttle")
self.arm_motors_with_switch(arm_ch)
self.progress("Waiting for Motor1 to speed up")
self.wait_servo_channel_value(5, spin_min_pwm, comparator=operator.ge)
self.progress("Verify that rudder disarm is disabled")
try:
self.disarm_motors_with_rc_input()
except NotAchievedException:
pass
if not self.armed():
raise NotAchievedException("Rudder disarm not disabled")
self.progress("Disarming with switch")
self.disarm_motors_with_switch(arm_ch)
self.progress("Waiting for Motor1 to stop")
self.wait_servo_channel_value(5, min_pwm, comparator=operator.le)
self.wait_ready_to_arm()
self.start_subtest("Verify that arming with switch does not spin motors in other modes")
# disable compass magnetic field arming check that is triggered by the simulated lean of vehicle
# this is required because adjusting the AHRS_TRIM values only affects the IMU and not external compasses
arming_magthresh = self.get_parameter("ARMING_MAGTHRESH")
self.set_parameter("ARMING_MAGTHRESH", 0)
# introduce a large attitude error to verify that stabilization is not active
ahrs_trim_x = self.get_parameter("AHRS_TRIM_X")
self.set_parameter("AHRS_TRIM_X", math.radians(-60))
self.wait_roll(60, 1)
# test all modes except QSTABILIZE, QACRO, AUTO and QAUTOTUNE and QLAND and QRTL
# QRTL and QLAND aren't tested because we can't arm in that mode
for mode in (
'ACRO',
'AUTOTUNE',
'AVOID_ADSB',
'CIRCLE',
'CRUISE',
'FBWA',
'FBWB',
'GUIDED',
'LOITER',
'QHOVER',
'QLOITER',
'STABILIZE',
'TRAINING',
):
self.progress("Testing %s mode" % mode)
self.change_mode(mode)
self.zero_throttle()
self.progress("Arming with switch at zero throttle")
self.arm_motors_with_switch(arm_ch)
self.progress("Waiting for Motor1 to (not) speed up")
self.delay_sim_time(spool_delay)
self.wait_servo_channel_value(5, spin_arm_pwm, comparator=operator.le)
self.wait_servo_channel_value(6, spin_arm_pwm, comparator=operator.le)
self.wait_servo_channel_value(7, spin_arm_pwm, comparator=operator.le)
self.wait_servo_channel_value(8, spin_arm_pwm, comparator=operator.le)
self.progress("Disarming with switch")
self.disarm_motors_with_switch(arm_ch)
self.progress("Waiting for Motor1 to stop")
self.wait_servo_channel_value(5, min_pwm, comparator=operator.le)
self.wait_ready_to_arm()
# remove attitude error and reinstance compass arming check
self.set_parameter("AHRS_TRIM_X", ahrs_trim_x)
self.set_parameter("ARMING_MAGTHRESH", arming_magthresh)
self.start_subtest("verify that AIRMODE auxswitch turns airmode on/off while armed")
"""set RC7_OPTION to AIRMODE"""
option_airmode = 84
self.set_parameter("RC7_OPTION", option_airmode)
for mode in ('QSTABILIZE', 'QACRO'):
self.progress("Testing %s mode" % mode)
self.change_mode(mode)
self.zero_throttle()
self.progress("Arming with GCS at zero throttle")
self.arm_vehicle()
self.progress("Turn airmode on with auxswitch")
self.set_rc(7, 2000)
self.progress("Waiting for Motor1 to speed up")
self.wait_servo_channel_value(5, spin_min_pwm, comparator=operator.ge)
self.progress("Turn airmode off with auxswitch")
self.set_rc(7, 1000)
self.progress("Waiting for Motor1 to slow down")
self.wait_servo_channel_value(5, spin_arm_pwm, comparator=operator.le)
self.disarm_vehicle()
self.wait_ready_to_arm()
self.start_subtest("Test GCS arming")
for mode in ('QSTABILIZE', 'QACRO'):
self.progress("Testing %s mode" % mode)
self.change_mode(mode)
self.zero_throttle()
self.progress("Arming with GCS at zero throttle")
self.arm_vehicle()
self.progress("Turn airmode on with auxswitch")
self.set_rc(7, 2000)
self.progress("Waiting for Motor1 to speed up")
self.wait_servo_channel_value(5, spin_min_pwm, comparator=operator.ge)
self.disarm_vehicle_expect_fail()
self.arm_vehicle()
self.progress("Verify that airmode is still on")
self.wait_servo_channel_value(5, spin_min_pwm, comparator=operator.ge)
self.disarm_vehicle(force=True)
self.wait_ready_to_arm()
def TestMotorMask(self):
"""Check operation of output_motor_mask"""
"""copter tailsitters will add condition: or (int(self.get_parameter('Q_TAILSIT_MOTMX')) & 1)"""
if not (int(self.get_parameter('Q_TILT_MASK')) & 1):
self.progress("output_motor_mask not in use")
return
self.progress("Testing output_motor_mask")
self.wait_ready_to_arm()
"""Default channel for Motor1 is 5"""
self.progress('Assert that SERVO5 is Motor1')
assert 33 == self.get_parameter('SERVO5_FUNCTION')
modes = ('MANUAL', 'FBWA', 'QHOVER')
for mode in modes:
self.progress("Testing %s mode" % mode)
self.change_mode(mode)
self.arm_vehicle()
self.progress("Raising throttle")
self.set_rc(3, 1800)
self.progress("Waiting for Motor1 to start")
self.wait_servo_channel_value(5, 1100, comparator=operator.gt)
self.set_rc(3, 1000)
self.disarm_vehicle()
self.wait_ready_to_arm()
def fly_mission(self, filename, fence=None, height_accuracy=-1):
"""Fly a mission from a file."""
self.progress("Flying mission %s" % filename)
num_wp = self.load_mission(filename)
if self.mavproxy is not None:
self.mavproxy.send('wp list\n')
if fence is not None:
self.load_fence(fence)
if self.mavproxy is not None:
self.mavproxy.send('fence list\n')
# self.install_terrain_handlers_context()
self.change_mode('AUTO')
self.wait_ready_to_arm()
self.arm_vehicle()
self.wait_waypoint(1, num_wp-1)
self.wait_disarmed(timeout=120) # give quadplane a long time to land
def EXTENDED_SYS_STATE_SLT(self):
self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_EXTENDED_SYS_STATE, 10)
self.change_mode("QHOVER")
self.assert_extended_sys_state(mavutil.mavlink.MAV_VTOL_STATE_MC,
mavutil.mavlink.MAV_LANDED_STATE_ON_GROUND)
self.change_mode("FBWA")
self.assert_extended_sys_state(mavutil.mavlink.MAV_VTOL_STATE_FW,
mavutil.mavlink.MAV_LANDED_STATE_ON_GROUND)
self.change_mode("QHOVER")
self.wait_ready_to_arm()
self.arm_vehicle()
# should not change just because we arm:
self.assert_extended_sys_state(mavutil.mavlink.MAV_VTOL_STATE_MC,
mavutil.mavlink.MAV_LANDED_STATE_ON_GROUND)
self.change_mode("MANUAL")
self.assert_extended_sys_state(mavutil.mavlink.MAV_VTOL_STATE_FW,
mavutil.mavlink.MAV_LANDED_STATE_ON_GROUND)
self.change_mode("QHOVER")
self.progress("Taking off")
self.set_rc(3, 1750)
self.wait_altitude(1, 5, relative=True)
self.assert_extended_sys_state(mavutil.mavlink.MAV_VTOL_STATE_MC,
mavutil.mavlink.MAV_LANDED_STATE_IN_AIR)
self.wait_altitude(10, 15, relative=True)
self.progress("Transitioning to fixed wing")
self.change_mode("FBWA")
self.set_rc(3, 1900) # apply spurs
self.wait_extended_sys_state(mavutil.mavlink.MAV_VTOL_STATE_TRANSITION_TO_FW,
mavutil.mavlink.MAV_LANDED_STATE_IN_AIR)
self.wait_extended_sys_state(mavutil.mavlink.MAV_VTOL_STATE_FW,
mavutil.mavlink.MAV_LANDED_STATE_IN_AIR)
self.progress("Transitioning to multicopter")
self.set_rc(3, 1500) # apply reins
self.change_mode("QHOVER")
# for a standard quadplane there is no transition-to-mc stage.
# tailsitters do have such a state.
self.wait_extended_sys_state(mavutil.mavlink.MAV_VTOL_STATE_MC,
mavutil.mavlink.MAV_LANDED_STATE_IN_AIR)
self.change_mode("QLAND")
self.wait_altitude(0, 2, relative=True, timeout=60)
self.wait_extended_sys_state(mavutil.mavlink.MAV_VTOL_STATE_MC,
mavutil.mavlink.MAV_LANDED_STATE_ON_GROUND,
timeout=30)
self.mav.motors_disarmed_wait()
def EXTENDED_SYS_STATE(self):
'''Check extended sys state works'''
self.EXTENDED_SYS_STATE_SLT()
def QAUTOTUNE(self):
'''test Plane QAutoTune mode'''
# this is a list of all parameters modified by QAUTOTUNE. Set
# them so that when the context is popped we get the original
# values back:
parameter_values = self.get_parameters([
"Q_A_RAT_RLL_P",
"Q_A_RAT_RLL_I",
"Q_A_RAT_RLL_D",
"Q_A_ANG_RLL_P",
"Q_A_ACCEL_R_MAX",
"Q_A_RAT_PIT_P",
"Q_A_RAT_PIT_I",
"Q_A_RAT_PIT_D",
"Q_A_ANG_PIT_P",
"Q_A_ACCEL_P_MAX",
"Q_A_RAT_YAW_P",
"Q_A_RAT_YAW_I",
"Q_A_RAT_YAW_FLTE",
"Q_A_ANG_YAW_P",
"Q_A_ACCEL_Y_MAX",
])
self.set_parameters(parameter_values)
self.takeoff(15, mode='GUIDED')
self.set_rc(3, 1500)
self.change_mode("QLOITER")
self.change_mode("QAUTOTUNE")
tstart = self.get_sim_time()
self.context_collect('STATUSTEXT')
while True:
now = self.get_sim_time_cached()
if now - tstart > 5000:
raise NotAchievedException("Did not get success message")
try:
self.wait_text("AutoTune: Success", timeout=1, check_context=True)
except AutoTestTimeoutException:
continue
# got success message
break
self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart))
self.context_clear_collection('STATUSTEXT')
self.progress("Landing to save gains")
self.set_rc(3, 1200)
self.wait_speed_vector(
Vector3(float('nan'), float('nan'), 1.4),
timeout=5,
)
self.wait_speed_vector(
Vector3(0.0, 0.0, 0.0),
timeout=20,
)
distance = self.distance_to_home()
if distance > 20:
raise NotAchievedException("wandered from home (distance=%f)" %
(distance,))
self.set_rc(3, 1000)
tstart = self.get_sim_time()
while True:
now = self.get_sim_time_cached()
if now - tstart > 500:
raise NotAchievedException("Did not get success message")
self.send_mavlink_disarm_command()
try:
self.wait_text(
"AutoTune: Saved gains for Roll Pitch Yaw.*",
timeout=0.5,
check_context=True,
regex=True,
)
except AutoTestTimeoutException:
continue
break
self.wait_disarmed()
self.reboot_sitl() # far from home
def takeoff(self, height, mode, timeout=30):
"""climb to specified height and set throttle to 1500"""
self.set_current_waypoint(0, check_afterwards=False)
self.change_mode(mode)
self.wait_ready_to_arm()
self.arm_vehicle()
if mode == 'GUIDED':
self.user_takeoff(alt_min=height, timeout=timeout)
return
self.set_rc(3, 1800)
self.wait_altitude(height,
height+5,
relative=True,
timeout=timeout)
self.set_rc(3, 1500)
def do_RTL(self):
self.change_mode("QRTL")
self.wait_altitude(-5, 1, relative=True, timeout=60)
self.wait_disarmed()
self.zero_throttle()
def fly_home_land_and_disarm(self, timeout=30):
self.context_push()
self.change_mode('LOITER')
self.set_parameter('RTL_AUTOLAND', 2)
filename = "QuadPlaneDalbyRTL.txt"
self.progress("Using %s to fly home" % filename)
self.load_generic_mission(filename)
self.send_cmd_do_set_mode("RTL")
self.wait_mode('AUTO')
self.wait_current_waypoint(4)
self.wait_statustext('Land descend started')
self.wait_statustext('Land final started', timeout=60)
self.wait_disarmed(timeout=timeout)
self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_MISSION)
# the following command is accepted, but doesn't actually
# work! Should be able to remove check_afterwards!
self.set_current_waypoint(0, check_afterwards=False)
self.change_mode('MANUAL')
self.context_pop()
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_cached() < 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 fly_left_circuit(self):
"""Fly a left circuit, 200m on a side."""
self.mavproxy.send('switch 4\n')
self.change_mode('FBWA')
self.set_rc(3, 1700)
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")
self.change_mode('QHOVER')
self.set_rc(3, 1100)
self.wait_altitude(10, 15,
relative=True,
timeout=60)
self.set_rc(3, 1500)
def hover_and_check_matched_frequency(self, dblevel=-15, minhz=200, maxhz=300, fftLength=32, peakhz=None):
# find a motor peak
self.takeoff(10, mode="QHOVER")
hover_time = 15
tstart = self.get_sim_time()
self.progress("Hovering for %u seconds" % hover_time)
while self.get_sim_time_cached() < tstart + hover_time:
self.mav.recv_match(type='ATTITUDE', blocking=True)
vfr_hud = self.mav.recv_match(type='VFR_HUD', blocking=True)
tend = self.get_sim_time()
self.do_RTL()
psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6)
# batch sampler defaults give 1024 fft and sample rate of 1kz so roughly 1hz/bin
scale = 1000. / 1024.
sminhz = int(minhz * scale)
smaxhz = int(maxhz * scale)
freq = psd["F"][numpy.argmax(psd["X"][sminhz:smaxhz]) + sminhz]
peakdb = numpy.amax(psd["X"][sminhz:smaxhz])
if peakdb < dblevel or (peakhz is not None and abs(freq - peakhz) / peakhz > 0.05):
raise NotAchievedException("No motor peak, found %fHz at %fdB" % (freq, peakdb))
else:
self.progress("motor peak %fHz, thr %f%%, %fdB" % (freq, vfr_hud.throttle, peakdb))
# we have a peak make sure that the FFT detected something close
# logging is at 10Hz
mlog = self.dfreader_for_current_onboard_log()
# accuracy is determined by sample rate and fft length, given our use of quinn we could probably use half of this
freqDelta = 1000. / fftLength
pkAvg = freq
freqs = []
while True:
m = mlog.recv_match(
type='FTN1',
blocking=True,
condition="FTN1.TimeUS>%u and FTN1.TimeUS<%u" % (tstart * 1.0e6, tend * 1.0e6))
if m is None:
break
freqs.append(m.PkAvg)
# peak within resolution of FFT length
pkAvg = numpy.median(numpy.asarray(freqs))
if abs(pkAvg - freq) > freqDelta:
raise NotAchievedException("FFT did not detect a motor peak at %f, found %f, wanted %f" % (dblevel, pkAvg, freq))
return freq
def GyroFFT(self):
"""Use dynamic harmonic notch to control motor noise."""
# basic gyro sample rate test
self.progress("Flying with gyro FFT - Gyro sample rate")
self.context_push()
ex = None
try:
self.set_rc_default()
# magic tridge EKF type that dramatically speeds up the test
self.set_parameters({
"AHRS_EKF_TYPE": 10,
"INS_LOG_BAT_MASK": 3,
"INS_LOG_BAT_OPT": 0,
"INS_GYRO_FILTER": 100,
"LOG_BITMASK": 45054,
"LOG_DISARMED": 0,
"SIM_DRIFT_SPEED": 0,
"SIM_DRIFT_TIME": 0,
# enable a noisy motor peak
"SIM_GYR1_RND": 20,
# enabling FFT will also enable the arming check: self-testing the functionality
"FFT_ENABLE": 1,
"FFT_MINHZ": 80,
"FFT_MAXHZ": 350,
"FFT_SNR_REF": 10,
"FFT_WINDOW_SIZE": 128,
"FFT_WINDOW_OLAP": 0.75,
})
# Step 1: inject a very precise noise peak at 250hz and make sure the in-flight fft
# can detect it really accurately. For a 128 FFT the frequency resolution is 8Hz so
# a 250Hz peak should be detectable within 5%
self.set_parameters({
"SIM_VIB_FREQ_X": 250,
"SIM_VIB_FREQ_Y": 250,
"SIM_VIB_FREQ_Z": 250,
})
self.reboot_sitl()
# find a motor peak
self.hover_and_check_matched_frequency(-15, 100, 350, 128, 250)
# Step 2: inject actual motor noise and use the standard length FFT to track it
self.set_parameters({
"SIM_VIB_MOT_MAX": 350,
"FFT_WINDOW_SIZE": 32,
"FFT_WINDOW_OLAP": 0.5,
})
self.reboot_sitl()
# find a motor peak
freq = self.hover_and_check_matched_frequency(-15, 200, 300, 32)
# Step 3: add a FFT dynamic notch and check that the peak is squashed
self.set_parameters({
"INS_LOG_BAT_OPT": 2,
"INS_HNTCH_ENABLE": 1,
"INS_HNTCH_FREQ": freq,
"INS_HNTCH_REF": 1.0,
"INS_HNTCH_ATT": 50,
"INS_HNTCH_BW": freq/2,
"INS_HNTCH_MODE": 4,
})
self.reboot_sitl()
self.takeoff(10, mode="QHOVER")
hover_time = 15
ignore_bins = 20
self.progress("Hovering for %u seconds" % hover_time)
tstart = self.get_sim_time()
while self.get_sim_time_cached() < tstart + hover_time:
self.mav.recv_match(type='ATTITUDE', blocking=True)
tend = self.get_sim_time()
self.do_RTL()
psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6)
freq = psd["F"][numpy.argmax(psd["X"][ignore_bins:]) + ignore_bins]
peakdB = numpy.amax(psd["X"][ignore_bins:])
if peakdB < -10:
self.progress("No motor peak, %f at %f dB" % (freq, peakdB))
else:
raise NotAchievedException("Detected peak at %f Hz of %.2f dB" % (freq, peakdB))
# Step 4: take off as a copter land as a plane, make sure we track
self.progress("Flying with gyro FFT - vtol to plane")
self.load_mission("quadplane-gyro-mission.txt")
if self.mavproxy is not None:
self.mavproxy.send('wp list\n')
self.change_mode('AUTO')
self.wait_ready_to_arm()
self.arm_vehicle()
self.wait_waypoint(1, 7, max_dist=60, timeout=1200)
self.wait_disarmed(timeout=120) # give quadplane a long time to land
# prevent update parameters from messing with the settings when we pop the context
self.set_parameter("FFT_ENABLE", 0)
self.reboot_sitl()
except Exception as e:
self.progress("Exception caught: %s" % (
self.get_exception_stacktrace(e)))
ex = e
self.context_pop()
self.reboot_sitl()
if ex is not None:
raise ex
def PIDTuning(self):
'''Test PID Tuning'''
self.change_mode("FBWA") # we don't update PIDs in MANUAL
super(AutoTestQuadPlane, self).PIDTuning()
def ParameterChecks(self):
'''basic parameter checks'''
self.test_parameter_checks_poscontrol("Q_P")
def rc_defaults(self):
ret = super(AutoTestQuadPlane, self).rc_defaults()
ret[3] = 1000
return ret
def default_mode(self):
return "MANUAL"
def disabled_tests(self):
return {
"FRSkyPassThrough": "Currently failing",
"CPUFailsafe": "servo channel values not scaled like ArduPlane",
"GyroFFT": "flapping test",
"ConfigErrorLoop": "failing because RC values not settable",
}
def BootInAUTO(self):
'''Test behaviour when booting in auto'''
self.load_mission("mission.txt")
self.set_parameters({
})
self.set_rc(5, 1000)
self.wait_mode('AUTO')
self.reboot_sitl()
self.wait_ready_to_arm()
self.delay_sim_time(20)
self.assert_current_waypoint(1)
self.arm_vehicle()
self.wait_altitude(9, 11, relative=True) # value from mission file is 10
distance = self.distance_to_home()
# this distance check is very, very loose. At time of writing
# the vehicle actually pitches ~6 degrees on trakeoff,
# wandering over 1m.
if distance > 2:
raise NotAchievedException("wandered from home (distance=%f)" %
(distance,))
self.change_mode('QLAND')
self.wait_disarmed(timeout=60)
def PilotYaw(self):
'''Test pilot yaw in various modes'''
self.takeoff(10, mode="QLOITER")
self.set_parameter("STICK_MIXING", 0)
self.set_rc(4, 1700)
for mode in "QLOITER", "QHOVER":
self.wait_heading(45)
self.wait_heading(90)
self.wait_heading(180)
self.wait_heading(275)
self.set_rc(4, 1500)
self.do_RTL()
def FwdThrInVTOL(self):
'''test use of fwd motor throttle into wind'''
self.set_parameters({"SIM_WIND_SPD": 25, # need very strong wind for this test
"SIM_WIND_DIR": 360,
"Q_WVANE_ENABLE": 1,
"Q_WVANE_GAIN": 1,
"STICK_MIXING": 0,
"Q_FWD_THR_USE": 2,
"SIM_ENGINE_FAIL": 2}) # we want to fail the forward thrust motor only
self.takeoff(10, mode="QLOITER")
self.set_rc(2, 1000)
self.delay_sim_time(10)
# Check that it is using some forward throttle
fwd_thr_pwm = self.get_servo_channel_value(3)
if fwd_thr_pwm < 1150 :
raise NotAchievedException("fwd motor pwm command low, want >= 1150 got %f" % (fwd_thr_pwm))
# check that pitch is on limit
m = self.mav.recv_match(type='ATTITUDE', blocking=True)
pitch = math.degrees(m.pitch)
if abs(pitch + 3.0) > 0.5 :
raise NotAchievedException("pitch should be -3.0 +- 0.5 deg, got %f" % (pitch))
self.set_rc(2, 1500)
self.delay_sim_time(5)
loc1 = self.mav.location()
self.set_parameter("SIM_ENGINE_MUL", 0) # simulate a complete loss of forward motor thrust
self.delay_sim_time(20)
self.change_mode('QLAND')
self.wait_disarmed(timeout=60)
loc2 = self.mav.location()
position_drift = self.get_distance(loc1, loc2)
if position_drift > 5.0 :
raise NotAchievedException("position drift high, want < 5.0 m got %f m" % (position_drift))
def Weathervane(self):
'''test nose-into-wind functionality'''
# We test nose into wind code paths and yaw direction in copter autotest,
# so we shall test the side into wind yaw direction and plane code paths here.
self.set_parameters({"SIM_WIND_SPD": 10,
"SIM_WIND_DIR": 240,
"Q_WVANE_ENABLE": 3, # WVANE_ENABLE = 3 gives direction of side into wind
"Q_WVANE_GAIN": 3,
"STICK_MIXING": 0})
self.takeoff(10, mode="QLOITER")
# Turn aircraft to heading 90 deg
self.set_rc(4, 1700)
self.wait_heading(90)
self.set_rc(4, 1500)
# Now wait for weathervaning to activate and turn side-on to wind at 240 deg therefore heading 150 deg
self.wait_heading(150, accuracy=5, timeout=180)
self.do_RTL()
def CPUFailsafe(self):
'''In lockup Plane should copy RC inputs to RC outputs'''
self.plane_CPUFailsafe()
def QAssist(self):
'''QuadPlane Assist tests'''
# find a motor peak
self.takeoff(10, mode="QHOVER")
self.set_rc(3, 1800)
self.change_mode("FBWA")
# disable stall prevention so roll angle is not limited
self.set_parameter("STALL_PREVENTION", 0)
thr_min_pwm = self.get_parameter("Q_M_PWM_MIN")
lim_roll_deg = self.get_parameter("LIM_ROLL_CD") * 0.01
self.progress("Waiting for motors to stop (transition completion)")
self.wait_servo_channel_value(5,
thr_min_pwm,
timeout=30,
comparator=operator.eq)
self.delay_sim_time(5)
self.wait_servo_channel_value(5,
thr_min_pwm,
timeout=30,
comparator=operator.eq)
self.progress("Stopping forward motor to kill airspeed below limit")
self.set_rc(3, 1000)
self.progress("Waiting for qassist to kick in")
self.wait_servo_channel_value(5, 1400, timeout=30, comparator=operator.gt)
self.progress("Move forward again, check qassist stops")
self.set_rc(3, 1800)
self.progress("Checking qassist stops")
self.wait_servo_channel_value(5,
thr_min_pwm,
timeout=30,
comparator=operator.eq)
self.set_rc(3, 1300)
self.context_push()
self.progress("Rolling over to %.0f degrees" % -lim_roll_deg)
self.set_rc(1, 1000)
self.wait_roll(-lim_roll_deg, 5)
self.progress("Killing servo outputs to force qassist to help")
self.set_parameter("SERVO1_MIN", 1480)
self.set_parameter("SERVO1_MAX", 1480)
self.set_parameter("SERVO1_TRIM", 1480)
self.progress("Trying to roll over hard the other way")
self.set_rc(1, 2000)
self.progress("Waiting for qassist (angle) to kick in")
self.wait_servo_channel_value(5, 1100, timeout=30, comparator=operator.gt)
self.wait_roll(lim_roll_deg, 5)
self.context_pop()
self.set_rc(1, 1500)
self.set_parameter("Q_RTL_MODE", 1)
self.change_mode("RTL")
self.wait_disarmed(timeout=300)
def LoiterAltQLand(self):
'''test loitering and qland with terrain involved'''
self.LoiterAltQLand_Terrain(
home="LakeGeorgeLookout",
ofs_n=0,
ofs_e=300,
)
# self.LoiterAltQLand_Terrain(
# home="KalaupapaCliffs",
# ofs_n=500,
# ofs_e=500,
# )
self.LoiterAltQLand_Relative()
def LoiterAltQLand_Relative(self):
'''test failsafe where vehicle loiters in fixed-wing mode to a
specific altitude then changes mode to QLAND'''
self.set_parameters({
'BATT_MONITOR': 4, # LoiterAltQLand
'BATT_FS_LOW_ACT': 6, # LoiterAltQLand
})
self.reboot_sitl()
takeoff_alt = 5
self.takeoff(takeoff_alt, mode='QLOITER')
loc = self.mav.location()
self.location_offset_ne(loc, 500, 500)
new_alt = 100
initial_altitude = self.get_altitude(relative=False, timeout=2)
self.run_cmd_int(
mavutil.mavlink.MAV_CMD_DO_REPOSITION,
0,
1, # reposition flags; 1 means "change to guided"
0,
0,
int(loc.lat * 1e7),
int(loc.lng * 1e7),
new_alt, # alt
frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
)
self.wait_altitude(
new_alt-1,
new_alt+1,
timeout=60,
relative=True,
minimum_duration=10)
self.wait_location(loc, timeout=120, accuracy=100)
self.progress("Triggering failsafe")
self.set_parameter('BATT_LOW_VOLT', 50)
self.wait_mode(25) # LoiterAltQLand
self.drain_mav()
m = self.assert_receive_message('POSITION_TARGET_GLOBAL_INT', very_verbose=True)
q_rtl_alt = self.get_parameter('Q_RTL_ALT')
expected_alt = initial_altitude - takeoff_alt + q_rtl_alt
if abs(m.alt - expected_alt) > 20:
raise NotAchievedException("Unexpected altitude; expected=%f got=%f" %
(expected_alt, m.alt))
self.assert_mode('LOITERALTQLAND')
self.wait_mode('QLAND')
alt = self.get_altitude(relative=True)
if abs(alt - q_rtl_alt) > 2:
raise NotAchievedException("qland too late; want=%f got=%f" %
(alt, q_rtl_alt))
self.wait_disarmed(timeout=300)
def LoiterAltQLand_Terrain(self,
home=None,
ofs_n=None,
ofs_e=None,
reposition_alt=100):
'''test failsafe where vehicle loiters in fixed-wing mode to a
specific altitude then changes mode to QLAND'''
self.context_push()
self.install_terrain_handlers_context()
self.set_parameters({
'BATT_MONITOR': 4, # LoiterAltQLand
'BATT_FS_LOW_ACT': 6, # LoiterAltQLand
'TERRAIN_FOLLOW': 1, # enabled in all modes
})
self.customise_SITL_commandline(
["--home", home]
)
takeoff_alt = 5
self.takeoff(takeoff_alt, mode='QLOITER')
loc = self.mav.location()
self.location_offset_ne(loc, ofs_n, ofs_e)
initial_altitude = self.get_altitude(relative=False, timeout=2)
self.run_cmd_int(
mavutil.mavlink.MAV_CMD_DO_REPOSITION,
0,
1, # reposition flags; 1 means "change to guided"
0,
0,
int(loc.lat * 1e7),
int(loc.lng * 1e7),
reposition_alt, # alt
frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
)
self.wait_altitude(
reposition_alt-1,
reposition_alt+1,
timeout=60,
relative=True,
minimum_duration=10)
self.wait_location(loc, timeout=500, accuracy=100)
self.progress("Triggering failsafe")
self.set_parameter('BATT_LOW_VOLT', 50)
self.wait_mode(25) # LoiterAltQLand
terrain_alt = self.get_terrain_height(verbose=True)
self.drain_mav()
m = self.assert_receive_message('POSITION_TARGET_GLOBAL_INT', very_verbose=True)
q_rtl_alt = self.get_parameter('Q_RTL_ALT')
expected_alt = terrain_alt + q_rtl_alt
if abs(m.alt - expected_alt) > 20:
raise NotAchievedException("Unexpected altitude; expected=%f got=%f" %
(expected_alt, m.alt))
self.assert_mode('LOITERALTQLAND')
self.wait_mode('QLAND')
alt = initial_altitude + self.get_altitude(relative=True)
if abs(alt - expected_alt) > 10:
raise NotAchievedException("qland too late; want=%f got=%f" %
(expected_alt, alt))
self.wait_disarmed(timeout=300)
self.zero_throttle()
self.reset_SITL_commandline()
self.context_pop()
def GUIDEDToAUTO(self):
'''Test using GUIDED mode for takeoff before shifting to auto'''
self.load_mission("mission.txt")
self.takeoff(30, mode='GUIDED')
# extra checks would go here
self.assert_not_receiving_message('CAMERA_FEEDBACK')
self.change_mode('AUTO')
self.wait_current_waypoint(3)
self.change_mode('QRTL')
self.wait_disarmed(timeout=240)
def Tailsitter(self):
'''tailsitter test'''
self.set_parameter('Q_FRAME_CLASS', 10)
self.set_parameter('Q_ENABLE', 1)
self.set_parameter('Q_TAILSIT_ENABLE', 1)
self.reboot_sitl()
self.wait_ready_to_arm()
value_before = self.get_servo_channel_value(3)
self.progress("Before: %u" % value_before)
self.change_mode('QHOVER')
tstart = self.get_sim_time()
while True:
now = self.get_sim_time_cached()
if now - tstart > 60:
break
value_after = self.get_servo_channel_value(3)
self.progress("After: t=%f output=%u" % ((now - tstart), value_after))
if value_before != value_after:
raise NotAchievedException("Changed throttle output on mode change to QHOVER")
self.disarm_vehicle()
def setup_ICEngine_vehicle(self, start_chan):
'''restarts SITL with an IC Engine setup'''
self.set_parameters({
'ICE_START_CHAN': start_chan,
})
model = "quadplane-ice"
self.customise_SITL_commandline(
[],
model=model,
defaults_filepath=self.model_defaults_filepath(model),
wipe=False,
)
def ICEngine(self):
'''Test ICE Engine support'''
rc_engine_start_chan = 11
self.setup_ICEngine_vehicle(start_chan=rc_engine_start_chan)
self.wait_ready_to_arm()
self.wait_rpm(1, 0, 0, minimum_duration=1)
self.arm_vehicle()
self.wait_rpm(1, 0, 0, minimum_duration=1)
self.context_collect("STATUSTEXT")
self.progress("Setting engine-start RC switch to HIGH")
self.set_rc(rc_engine_start_chan, 2000)
self.wait_statustext("Starting engine", check_context=True)
self.wait_rpm(1, 300, 400, minimum_duration=1)
self.progress("Setting engine-start RC switch to MID")
self.set_rc(rc_engine_start_chan, 1500)
self.progress("Setting full throttle")
self.set_rc(3, 2000)
self.wait_rpm(1, 6500, 7500, minimum_duration=30, timeout=40)
self.progress("Setting min-throttle")
self.set_rc(3, 1000)
self.wait_rpm(1, 65, 75, minimum_duration=1)
self.progress("Setting engine-start RC switch to LOW")
self.set_rc(rc_engine_start_chan, 1000)
self.wait_rpm(1, 0, 0, minimum_duration=1)
# ICE provides forward thrust, which can make us think we're flying:
self.disarm_vehicle(force=True)
self.reboot_sitl()
self.start_subtest("Testing throttle out in manual mode")
self.change_mode('MANUAL')
self.set_rc(3, 1700)
self.wait_servo_channel_value(3, 2000)
self.set_parameter("ICE_OPTIONS", 4)
# remember that throttle is reversed!
self.wait_servo_channel_value(3, 1300)
self.change_mode('FBWA')
self.wait_servo_channel_value(3, 2000)
def ICEngineMission(self):
'''Test ICE Engine Mission support'''
rc_engine_start_chan = 11
self.setup_ICEngine_vehicle(start_chan=rc_engine_start_chan)
self.load_mission("mission.txt")
self.wait_ready_to_arm()
self.set_rc(rc_engine_start_chan, 2000)
self.arm_vehicle()
self.change_mode('AUTO')
self.wait_disarmed(timeout=300)
def MAV_CMD_DO_ENGINE_CONTROL(self):
'''test MAV_CMD_DO_ENGINE_CONTROL mavlink command'''
expected_idle_rpm_min = 65
expected_idle_rpm_max = 75
expected_starter_rpm_min = 345
expected_starter_rpm_max = 355
rc_engine_start_chan = 11
self.setup_ICEngine_vehicle(start_chan=rc_engine_start_chan)
self.wait_ready_to_arm()
for method in self.run_cmd, self.run_cmd_int:
self.change_mode('MANUAL')
self.set_rc(rc_engine_start_chan, 1500) # allow motor to run
self.wait_rpm(1, 0, 0, minimum_duration=1)
self.arm_vehicle()
self.wait_rpm(1, 0, 0, minimum_duration=1)
self.start_subtest("Start motor")
method(mavutil.mavlink.MAV_CMD_DO_ENGINE_CONTROL, p1=1)
self.wait_rpm(1, expected_starter_rpm_min, expected_starter_rpm_max)
self.wait_rpm(1, expected_idle_rpm_min, expected_idle_rpm_max, minimum_duration=10)
# starting the motor while it is running is failure
# (probably wrong, but that's how this works):
self.start_subtest("try start motor again")
self.context_collect('STATUSTEXT')
method(mavutil.mavlink.MAV_CMD_DO_ENGINE_CONTROL, p1=1, want_result=mavutil.mavlink.MAV_RESULT_FAILED)
self.wait_statustext("already running", check_context=True)
self.context_stop_collecting('STATUSTEXT')
# shouldn't affect run state:
self.wait_rpm(1, expected_idle_rpm_min, expected_idle_rpm_max, minimum_duration=1)
self.start_subtest("Stop motor")
method(mavutil.mavlink.MAV_CMD_DO_ENGINE_CONTROL, p1=0)
self.wait_rpm(1, 0, 0, minimum_duration=1)
self.start_subtest("Stop motor (again)")
method(mavutil.mavlink.MAV_CMD_DO_ENGINE_CONTROL, p1=0)
self.wait_rpm(1, 0, 0, minimum_duration=1)
self.start_subtest("Check start chan control disable")
old_start_channel_value = self.get_rc_channel_value(rc_engine_start_chan)
self.set_rc(rc_engine_start_chan, 1000)
self.context_collect('STATUSTEXT')
method(mavutil.mavlink.MAV_CMD_DO_ENGINE_CONTROL, p1=1, want_result=mavutil.mavlink.MAV_RESULT_FAILED)
self.wait_statustext("start control disabled", check_context=True)
self.context_stop_collecting('STATUSTEXT')
self.set_rc(rc_engine_start_chan, old_start_channel_value)
self.wait_rpm(1, 0, 0, minimum_duration=1)
self.start_subtest("test start-at-height")
self.wait_rpm(1, 0, 0, minimum_duration=1)
self.context_collect('STATUSTEXT')
method(
mavutil.mavlink.MAV_CMD_DO_ENGINE_CONTROL,
p1=1, # start
p3=15.5, # ... at 15.5 metres
)
self.wait_statustext("height set to 15.5m", check_context=True)
self.wait_rpm(1, 0, 0, minimum_duration=2)
self.takeoff(20, mode='GUIDED')
self.wait_rpm(1, expected_starter_rpm_min, expected_starter_rpm_max, minimum_duration=1)
self.wait_statustext("Engine running", check_context=True)
self.context_stop_collecting('STATUSTEXT')
# stop the motor again:
method(mavutil.mavlink.MAV_CMD_DO_ENGINE_CONTROL, p1=0)
self.wait_rpm(1, 0, 0, minimum_duration=1)
self.change_mode('QLAND')
self.wait_disarmed()
def Ship(self):
'''Ensure we can take off from simulated ship'''
self.context_push()
self.set_parameters({
'SIM_SHIP_ENABLE': 1,
'SIM_SHIP_SPEED': 1, # the default of 3 will break this test
})
self.change_mode('QLOITER')
self.wait_ready_to_arm()
self.arm_vehicle()
self.set_rc(3, 1700)
# self.delay_sim_time(1)
# self.send_debug_trap()
# output here is a bit weird as we also receive altitude from
# the simulated ship....
self.wait_altitude(20, 30, relative=True)
self.disarm_vehicle(force=True)
self.context_pop()
self.reboot_sitl()
def MidAirDisarmDisallowed(self):
'''Check disarm behaviour in Q-mode'''
self.start_subtest("Basic arm in qloiter")
self.set_parameter("FLIGHT_OPTIONS", 0)
self.change_mode('QLOITER')
self.wait_ready_to_arm()
self.arm_vehicle()
self.disarm_vehicle()
self.context_push()
self.start_subtest("Ensure disarming in q-modes on ground works")
self.set_parameter("FLIGHT_OPTIONS", 1 << 11)
self.arm_vehicle()
self.disarm_vehicle() # should be OK as we're not flying yet
self.context_pop()
self.start_subtest("Ensure no disarming mid-air")
self.arm_vehicle()
self.set_rc(3, 2000)
self.wait_altitude(5, 50, relative=True)
self.set_rc(3, 1000)
disarmed = False
try:
self.disarm_vehicle()
disarmed = True
except ValueError as e:
self.progress("Got %s" % repr(e))
if "Expected MAV_RESULT_ACCEPTED got MAV_RESULT_FAILED" not in str(e):
raise e
if disarmed:
raise NotAchievedException("Disarmed when we shouldn't have")
self.change_mode('QLAND')
self.wait_disarmed()
self.start_subtest("Check we can disarm after a short period on the ground")
self.takeoff(5, 'QHOVER')
self.change_mode('QLAND')
try:
self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_EXTENDED_SYS_STATE, 10)
self.wait_extended_sys_state(
landed_state=mavutil.mavlink.MAV_LANDED_STATE_ON_GROUND,
vtol_state=mavutil.mavlink.MAV_VTOL_STATE_MC,
timeout=60
)
except Exception:
self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_EXTENDED_SYS_STATE, 0)
raise
self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_EXTENDED_SYS_STATE, -1)
self.disarm_vehicle()
def MAV_CMD_NAV_LOITER_TO_ALT(self, target_system=1, target_component=1):
'''ensure consecutive loiter to alts work'''
self.load_mission('mission.txt')
self.change_mode('AUTO')
self.wait_ready_to_arm()
self.arm_vehicle()
self.wait_current_waypoint(4, timeout=240)
self.assert_altitude(120, accuracy=5, relative=True)
self.delay_sim_time(30)
self.assert_altitude(120, accuracy=5, relative=True)
self.set_current_waypoint(5)
self.wait_altitude(altitude_min=65, altitude_max=75, relative=True)
if self.current_waypoint() != 5:
raise NotAchievedException("Should pass 90m before passing waypoint 5")
self.wait_disarmed(timeout=300)
def Mission(self):
'''fly the OBC 2016 mission in Dalby'''
self.load_mission("Dalby-OBC2016.txt")
self.load_fence("Dalby-OBC2016-fence.txt")
if self.mavproxy is not None:
self.mavproxy.send('wp list\n')
self.install_terrain_handlers_context()
self.wait_ready_to_arm()
self.arm_vehicle()
self.change_mode('AUTO')
self.wait_waypoint(1, 19, max_dist=60, timeout=1200)
self.wait_disarmed(timeout=120) # give quadplane a long time to land
# wait for blood sample here
self.set_current_waypoint(20)
self.wait_ready_to_arm()
self.arm_vehicle()
self.wait_waypoint(20, 34, max_dist=60, timeout=1200)
self.wait_disarmed(timeout=120) # give quadplane a long time to land
self.progress("Mission OK")
def VTOLLandSpiral(self):
'''check spiral-to-alt option for landing'''
self.fly_mission('mission.txt')
self.set_parameter('WP_LOITER_RAD', -self.get_parameter('WP_LOITER_RAD'))
self.set_current_waypoint(0, check_afterwards=False)
self.fly_mission('mission.txt')
def VTOLQuicktune(self):
'''VTOL Quicktune'''
applet_script = "VTOL-quicktune.lua"
self.install_applet_script(applet_script)
self.set_parameters({
"SCR_ENABLE": 1,
"SIM_SPEEDUP": 20, # need to give some cycles to lua
"RC7_OPTION": 300,
})
self.reboot_sitl()
self.context_push()
self.context_collect('STATUSTEXT')
self.set_parameters({
"QUIK_ENABLE" : 1,
"QUIK_DOUBLE_TIME" : 5, # run faster for autotest
})
self.scripting_restart()
self.wait_text("Quicktune for quadplane loaded", check_context=True)
self.wait_ready_to_arm()
self.change_mode("QLOITER")
self.arm_vehicle()
self.takeoff(20, 'QLOITER')
# use rc switch to start tune
self.set_rc(7, 1500)
self.wait_text("Tuning: starting tune", check_context=True)
for axis in ['RLL', 'PIT', 'YAW']:
self.wait_text("Starting %s tune" % axis, check_context=True)
self.wait_text("Tuning: %s_D done" % axis, check_context=True, timeout=120)
self.wait_text("Tuning: %s_P done" % axis, check_context=True, timeout=120)
self.wait_text("Tuning: %s done" % axis, check_context=True, timeout=120)
self.wait_text("Tuning: YAW done", check_context=True, timeout=120)
# to test aux function method, use aux fn for save
self.run_auxfunc(300, 2)
self.wait_text("Tuning: saved", check_context=True)
self.change_mode("QLAND")
self.wait_disarmed(timeout=120)
self.set_parameter("QUIK_ENABLE", 0)
self.context_pop()
self.remove_installed_script(applet_script)
self.reboot_sitl()
def RCDisableAirspeedUse(self):
'''check disabling airspeed using RC switch'''
self.set_parameter("RC9_OPTION", 106)
self.delay_sim_time(5)
self.set_rc(9, 1000)
self.wait_sensor_state(
mavutil.mavlink.MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE,
True,
True,
True)
self.set_rc(9, 2000)
self.wait_sensor_state(
mavutil.mavlink.MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE,
True,
False,
True)
self.set_rc(9, 1000)
self.wait_sensor_state(
mavutil.mavlink.MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE,
True,
True,
True)
self.progress("Disabling airspeed sensor")
self.context_push()
self.set_rc(9, 2000)
self.set_parameters({
"COMPASS_ENABLE": 0,
"EK2_ENABLE": 0,
"AHRS_EKF_TYPE": 3,
"COMPASS_USE": 0,
"COMPASS_USE2": 0,
"COMPASS_USE3": 0,
"ARMING_CHECK": 589818, # from a logfile, disables compass
})
self.reboot_sitl()
self.context_collect('STATUSTEXT')
self.wait_prearm_sys_status_healthy(timeout=120)
self.change_mode('QLOITER')
self.arm_vehicle()
self.set_rc(3, 2000)
self.wait_altitude(10, 30, relative=True)
self.change_mode('FBWA')
self.wait_statustext('Transition done')
# the vehicle stays in DCM until there's velocity - make sure
# we did go to EK3 evenutally, 'though:
self.wait_statustext('EKF3 active', check_context=True)
self.disarm_vehicle(force=True)
self.context_pop()
self.reboot_sitl()
def mission_MAV_CMD_DO_VTOL_TRANSITION(self):
'''mission item forces transition'''
wps = self.create_simple_relhome_mission([
(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 30),
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 200, 0, 30),
self.create_MISSION_ITEM_INT(
mavutil.mavlink.MAV_CMD_DO_VTOL_TRANSITION,
p1=mavutil.mavlink.MAV_VTOL_STATE_MC
),
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 300, 200, 30),
self.create_MISSION_ITEM_INT(
mavutil.mavlink.MAV_CMD_DO_VTOL_TRANSITION,
p1=mavutil.mavlink.MAV_VTOL_STATE_FW
),
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 100, 200, 30),
(mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0),
])
self.check_mission_upload_download(wps)
self.change_mode('AUTO')
self.wait_ready_to_arm()
self.arm_vehicle()
self.wait_current_waypoint(4)
self.wait_servo_channel_value(5, 1200, comparator=operator.gt)
self.wait_current_waypoint(6)
self.wait_servo_channel_value(5, 1000, comparator=operator.eq, timeout=90)
self.fly_home_land_and_disarm()
def mavlink_MAV_CMD_DO_VTOL_TRANSITION(self):
'''mavlink command forces transition during mission'''
wps = self.create_simple_relhome_mission([
(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 30),
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 2000, 0, 30),
(mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0),
])
self.check_mission_upload_download(wps)
self.change_mode('AUTO')
self.wait_ready_to_arm()
self.arm_vehicle()
self.wait_current_waypoint(2)
self.wait_servo_channel_value(5, 1000, comparator=operator.eq, timeout=90)
for command in self.run_cmd, self.run_cmd_int:
command(mavutil.mavlink.MAV_CMD_DO_VTOL_TRANSITION, p1=mavutil.mavlink.MAV_VTOL_STATE_MC)
self.wait_servo_channel_value(5, 1200, comparator=operator.gt, timeout=300)
command(mavutil.mavlink.MAV_CMD_DO_VTOL_TRANSITION, p1=mavutil.mavlink.MAV_VTOL_STATE_FW)
self.wait_servo_channel_value(5, 1000, comparator=operator.eq, timeout=90)
self.fly_home_land_and_disarm()
def MAV_CMD_NAV_TAKEOFF(self):
'''test issuing takeoff command via mavlink'''
self.change_mode('GUIDED')
self.wait_ready_to_arm()
self.arm_vehicle()
self.run_cmd(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, p7=5)
self.wait_altitude(4.5, 5.5, minimum_duration=5, relative=True)
self.change_mode('QLAND')
self.wait_disarmed()
self.start_subtest("Check NAV_TAKEOFF is above current location, not home location")
self.change_mode('GUIDED')
self.wait_ready_to_arm()
# reset home 20 metres above current location
current_alt_abs = self.get_altitude(relative=False)
loc = self.mav.location()
home_z_ofs = 20
self.run_cmd(
mavutil.mavlink.MAV_CMD_DO_SET_HOME,
p5=loc.lat,
p6=loc.lng,
p7=current_alt_abs + home_z_ofs,
)
self.arm_vehicle()
takeoff_alt = 5
self.run_cmd(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, p7=takeoff_alt)
self.wait_altitude(
current_alt_abs + takeoff_alt - 0.5,
current_alt_abs + takeoff_alt + 0.5,
minimum_duration=5,
relative=False,
)
self.change_mode('QLAND')
self.wait_disarmed()
self.reboot_sitl() # unlock home position
def tests(self):
'''return list of all tests'''
ret = super(AutoTestQuadPlane, self).tests()
ret.extend([
self.FwdThrInVTOL,
self.AirMode,
self.TestMotorMask,
self.PilotYaw,
self.ParameterChecks,
self.QAUTOTUNE,
self.LogDownload,
self.EXTENDED_SYS_STATE,
self.Mission,
self.Weathervane,
self.QAssist,
self.GyroFFT,
self.Tailsitter,
self.ICEngine,
self.ICEngineMission,
self.MAV_CMD_DO_ENGINE_CONTROL,
self.MidAirDisarmDisallowed,
self.GUIDEDToAUTO,
self.BootInAUTO,
self.Ship,
self.MAV_CMD_NAV_LOITER_TO_ALT,
self.LoiterAltQLand,
self.VTOLLandSpiral,
self.VTOLQuicktune,
Test(self.MotorTest, kwargs={ # tests motors 4 and 2
"mot1_servo_chan": 8, # quad-x second motor cw from f-r
"mot4_servo_chan": 6, # quad-x third motor cw from f-r
"wait_finish_text": False,
"quadplane": True,
}),
self.RCDisableAirspeedUse,
self.mission_MAV_CMD_DO_VTOL_TRANSITION,
self.mavlink_MAV_CMD_DO_VTOL_TRANSITION,
self.MAV_CMD_NAV_TAKEOFF,
])
return ret
Reference in New Issue View Git Blame Copy Permalink