2017-02-21 13:32:26 -04:00
|
|
|
#!/usr/bin/env python
|
|
|
|
|
|
|
|
# Fly ArduPlane QuadPlane in SITL
|
2016-11-08 07:06:05 -04:00
|
|
|
from __future__ import print_function
|
2016-07-31 07:22:06 -03:00
|
|
|
import os
|
2020-01-11 13:59:37 -04:00
|
|
|
import numpy
|
|
|
|
import math
|
|
|
|
|
2016-01-09 01:27:03 -04:00
|
|
|
from pymavlink import mavutil
|
2016-07-31 07:22:06 -03:00
|
|
|
|
2018-03-14 08:08:53 -03:00
|
|
|
from common import AutoTest
|
2020-08-03 23:26:39 -03:00
|
|
|
from common import AutoTestTimeoutException, NotAchievedException, PreconditionFailedException
|
2019-03-31 18:42:06 -03:00
|
|
|
|
2019-02-09 15:43:07 -04:00
|
|
|
from pysim import vehicleinfo
|
2019-02-11 13:55:18 -04:00
|
|
|
import operator
|
2016-01-09 01:27:03 -04:00
|
|
|
|
2019-03-31 18:42:06 -03:00
|
|
|
|
2016-01-09 01:27:03 -04:00
|
|
|
# get location of scripts
|
2016-07-31 07:22:06 -03:00
|
|
|
testdir = os.path.dirname(os.path.realpath(__file__))
|
|
|
|
WIND = "0,180,0.2" # speed,direction,variance
|
2020-01-11 13:59:37 -04:00
|
|
|
SITL_START_LOCATION = mavutil.location(-27.274439, 151.290064, 343, 8.7)
|
2018-03-05 11:14:34 -04:00
|
|
|
|
2020-03-06 23:45:22 -04:00
|
|
|
|
2018-03-14 08:08:53 -03:00
|
|
|
class AutoTestQuadPlane(AutoTest):
|
2020-01-11 13:59:37 -04:00
|
|
|
|
2018-10-10 10:06:01 -03:00
|
|
|
@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 []
|
2018-12-17 17:52:09 -04:00
|
|
|
|
2019-03-09 00:20:36 -04:00
|
|
|
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 log_name(self):
|
|
|
|
return "QuadPlane"
|
|
|
|
|
2020-03-10 09:18:59 -03:00
|
|
|
def set_current_test_name(self, name):
|
|
|
|
self.current_test_name_directory = "ArduPlane_Tests/" + name + "/"
|
|
|
|
|
2019-03-09 00:20:36 -04:00
|
|
|
def apply_defaultfile_parameters(self):
|
2020-04-13 23:08:19 -03:00
|
|
|
# plane passes in a defaults_filepath in place of applying
|
2019-03-09 00:20:36 -04:00
|
|
|
# parameters afterwards.
|
|
|
|
pass
|
|
|
|
|
|
|
|
def defaults_filepath(self):
|
2020-04-21 04:30:17 -03:00
|
|
|
return self.model_defaults_filepath("ArduPlane",self.frame)
|
2018-03-05 11:14:34 -04:00
|
|
|
|
2018-10-10 10:07:21 -03:00
|
|
|
def is_plane(self):
|
|
|
|
return True
|
|
|
|
|
2019-03-26 09:17:11 -03:00
|
|
|
def get_stick_arming_channel(self):
|
2018-10-10 10:07:21 -03:00
|
|
|
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)
|
2018-03-05 11:14:34 -04:00
|
|
|
|
2020-08-03 23:26:39 -03:00
|
|
|
def test_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_THR_MIN_PWM")
|
|
|
|
|
2020-08-17 10:44:46 -03:00
|
|
|
self.progress("Verify Motor1 is at min_pwm when disarmed")
|
2020-08-03 23:26:39 -03:00
|
|
|
self.wait_servo_channel_value(5, min_pwm, comparator=operator.eq)
|
|
|
|
|
|
|
|
"""set Q_OPTIONS bit AIRMODE"""
|
|
|
|
airmode_option_bit = (1<<9)
|
|
|
|
self.set_parameter("Q_OPTIONS", airmode_option_bit)
|
|
|
|
|
|
|
|
armdisarm_option = 41
|
|
|
|
arm_ch = 8
|
|
|
|
self.set_parameter("RC%d_OPTION" % arm_ch, armdisarm_option)
|
2020-08-17 10:44:46 -03:00
|
|
|
self.progress("Configured RC%d as ARMDISARM switch" % arm_ch)
|
2020-08-03 23:26:39 -03:00
|
|
|
|
|
|
|
"""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 Q_OPTIONS=AirMode")
|
|
|
|
for mode in ('QSTABILIZE', 'QACRO'):
|
2020-08-04 10:36:16 -03:00
|
|
|
"""verify that arming with switch results in higher PWM output"""
|
2020-08-03 23:26:39 -03:00
|
|
|
self.progress("Testing %s mode" % mode)
|
|
|
|
self.change_mode(mode)
|
|
|
|
self.zero_throttle()
|
2020-08-17 10:44:46 -03:00
|
|
|
self.progress("Arming with switch at zero throttle")
|
2020-08-03 23:26:39 -03:00
|
|
|
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)
|
|
|
|
|
2020-08-17 10:44:46 -03:00
|
|
|
self.progress("Verify that rudder disarm is disabled")
|
|
|
|
if self.disarm_motors_with_rc_input():
|
|
|
|
raise NotAchievedException("Rudder disarm not disabled")
|
|
|
|
|
|
|
|
self.progress("Disarming with switch")
|
2020-08-03 23:26:39 -03:00
|
|
|
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()
|
|
|
|
|
2020-08-17 10:44:46 -03:00
|
|
|
self.start_subtest("Verify that arming with switch does not spin motors in other modes")
|
2020-08-04 10:36:16 -03:00
|
|
|
# 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
|
|
|
|
for mode in ('QLOITER', 'QHOVER', 'CIRCLE', 'STABILIZE', 'TRAINING', 'ACRO', 'FBWA', 'FBWB', 'CRUISE', 'AUTOTUNE', 'RTL', 'LOITER', 'AVOID_ADSB', 'GUIDED', 'QLAND', 'QRTL'):
|
2020-08-03 23:26:39 -03:00
|
|
|
self.progress("Testing %s mode" % mode)
|
|
|
|
self.change_mode(mode)
|
|
|
|
self.zero_throttle()
|
2020-08-17 10:44:46 -03:00
|
|
|
self.progress("Arming with switch at zero throttle")
|
2020-08-03 23:26:39 -03:00
|
|
|
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)
|
2020-08-04 10:36:16 -03:00
|
|
|
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)
|
2020-08-03 23:26:39 -03:00
|
|
|
|
2020-08-17 10:44:46 -03:00
|
|
|
self.progress("Disarming with switch")
|
2020-08-03 23:26:39 -03:00
|
|
|
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()
|
2020-08-04 10:36:16 -03:00
|
|
|
# remove attitude error
|
|
|
|
self.set_parameter("AHRS_TRIM_X", ahrs_trim_x)
|
2020-08-03 23:26:39 -03:00
|
|
|
|
|
|
|
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()
|
2020-08-17 10:44:46 -03:00
|
|
|
self.progress("Arming with GCS at zero throttle")
|
2020-08-03 23:26:39 -03:00
|
|
|
self.arm_vehicle()
|
|
|
|
|
2020-08-17 10:44:46 -03:00
|
|
|
self.progress("Turn airmode on with auxswitch")
|
2020-08-03 23:26:39 -03:00
|
|
|
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)
|
|
|
|
|
2020-08-17 10:44:46 -03:00
|
|
|
self.progress("Turn airmode off with auxswitch")
|
2020-08-03 23:26:39 -03:00
|
|
|
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()
|
2020-08-17 10:44:46 -03:00
|
|
|
self.progress("Arming with GCS at zero throttle")
|
2020-08-03 23:26:39 -03:00
|
|
|
self.arm_vehicle()
|
|
|
|
|
2020-08-17 10:44:46 -03:00
|
|
|
self.progress("Turn airmode on with auxswitch")
|
2020-08-03 23:26:39 -03:00
|
|
|
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)
|
|
|
|
|
2020-08-17 10:44:46 -03:00
|
|
|
self.progress("Disarm/rearm with GCS")
|
2020-08-03 23:26:39 -03:00
|
|
|
self.disarm_vehicle()
|
|
|
|
self.arm_vehicle()
|
|
|
|
|
2020-08-17 10:44:46 -03:00
|
|
|
self.progress("Verify that airmode is still on")
|
2020-08-03 23:26:39 -03:00
|
|
|
self.wait_servo_channel_value(5, spin_min_pwm, comparator=operator.ge)
|
|
|
|
self.disarm_vehicle()
|
|
|
|
self.wait_ready_to_arm()
|
|
|
|
|
|
|
|
|
2019-02-11 13:55:18 -04:00
|
|
|
def test_motor_mask(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()
|
|
|
|
|
2020-01-11 13:59:37 -04:00
|
|
|
def fly_mission(self, filename, fence=None, height_accuracy=-1):
|
2018-03-05 11:14:34 -04:00
|
|
|
"""Fly a mission from a file."""
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("Flying mission %s" % filename)
|
2018-11-09 08:32:02 -04:00
|
|
|
self.load_mission(filename)
|
2020-01-11 13:59:37 -04:00
|
|
|
if fence is not None:
|
|
|
|
self.load_fence(fence)
|
2018-03-05 11:14:34 -04:00
|
|
|
self.mavproxy.send('wp list\n')
|
|
|
|
self.mavproxy.expect('Requesting [0-9]+ waypoints')
|
2018-12-17 17:52:09 -04:00
|
|
|
self.wait_ready_to_arm()
|
|
|
|
self.arm_vehicle()
|
2018-03-05 11:14:34 -04:00
|
|
|
self.mavproxy.send('mode AUTO\n')
|
|
|
|
self.wait_mode('AUTO')
|
2018-04-27 15:21:53 -03:00
|
|
|
self.wait_waypoint(1, 19, max_dist=60, timeout=1200)
|
|
|
|
|
2020-03-12 20:51:17 -03:00
|
|
|
self.wait_disarmed(timeout=120) # give quadplane a long time to land
|
2018-03-05 11:14:34 -04:00
|
|
|
# wait for blood sample here
|
|
|
|
self.mavproxy.send('wp set 20\n')
|
2018-09-21 06:46:55 -03:00
|
|
|
self.wait_ready_to_arm()
|
2018-03-05 11:14:34 -04:00
|
|
|
self.arm_vehicle()
|
2018-04-27 15:21:53 -03:00
|
|
|
self.wait_waypoint(20, 34, max_dist=60, timeout=1200)
|
|
|
|
|
2020-03-12 20:51:17 -03:00
|
|
|
self.wait_disarmed(timeout=120) # give quadplane a long time to land
|
2018-03-14 08:08:53 -03:00
|
|
|
self.progress("Mission OK")
|
2018-03-05 11:14:34 -04:00
|
|
|
|
2019-01-17 20:58:18 -04:00
|
|
|
def fly_qautotune(self):
|
|
|
|
self.change_mode("QHOVER")
|
|
|
|
self.wait_ready_to_arm()
|
|
|
|
self.arm_vehicle()
|
|
|
|
self.set_rc(3, 1800)
|
|
|
|
self.wait_altitude(30,
|
|
|
|
40,
|
|
|
|
relative=True,
|
|
|
|
timeout=30)
|
|
|
|
self.set_rc(3, 1500)
|
|
|
|
self.change_mode("QAUTOTUNE")
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
sim_time_expected = 5000
|
|
|
|
deadline = tstart + sim_time_expected
|
|
|
|
while self.get_sim_time_cached() < deadline:
|
|
|
|
now = self.get_sim_time_cached()
|
|
|
|
m = self.mav.recv_match(type='STATUSTEXT',
|
|
|
|
blocking=True,
|
|
|
|
timeout=1)
|
|
|
|
if m is None:
|
|
|
|
continue
|
|
|
|
self.progress("STATUSTEXT (%u<%u): %s" % (now, deadline, m.text))
|
|
|
|
if "AutoTune: Success" in m.text:
|
2019-03-31 18:42:06 -03:00
|
|
|
break
|
|
|
|
self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart))
|
|
|
|
self.set_rc(3, 1200)
|
|
|
|
self.wait_altitude(-5, 1, relative=True, timeout=30)
|
|
|
|
while self.get_sim_time_cached() < deadline:
|
|
|
|
self.mavproxy.send('disarm\n')
|
|
|
|
try:
|
|
|
|
self.wait_text("AutoTune: Saved gains for Roll Pitch Yaw", timeout=0.5)
|
|
|
|
except AutoTestTimeoutException as e:
|
|
|
|
continue
|
|
|
|
break
|
2020-03-12 20:51:17 -03:00
|
|
|
self.wait_disarmed()
|
2019-01-17 20:58:18 -04:00
|
|
|
|
2020-01-11 13:59:37 -04:00
|
|
|
def takeoff(self, height, mode):
|
2020-07-23 18:57:30 -03:00
|
|
|
"""climb to specified height and set throttle to 1500"""
|
2020-01-11 13:59:37 -04:00
|
|
|
self.change_mode(mode)
|
|
|
|
self.wait_ready_to_arm()
|
|
|
|
self.arm_vehicle()
|
|
|
|
self.set_rc(3, 1800)
|
|
|
|
self.wait_altitude(height,
|
|
|
|
height+5,
|
|
|
|
relative=True,
|
|
|
|
timeout=30)
|
|
|
|
self.set_rc(3, 1500)
|
|
|
|
|
|
|
|
def do_RTL(self):
|
|
|
|
self.change_mode("QRTL")
|
|
|
|
self.wait_altitude(-5, 1, relative=True, timeout=60)
|
2020-03-12 20:51:17 -03:00
|
|
|
self.wait_disarmed()
|
2020-07-23 18:57:30 -03:00
|
|
|
self.zero_throttle()
|
2020-01-11 13:59:37 -04:00
|
|
|
|
2020-05-19 01:52:22 -03:00
|
|
|
def fly_home_land_and_disarm(self, timeout=30):
|
2020-01-11 13:59:37 -04:00
|
|
|
self.set_parameter("LAND_TYPE", 0)
|
2020-03-10 09:18:59 -03:00
|
|
|
filename = "flaps.txt"
|
2020-01-11 13:59:37 -04:00
|
|
|
self.progress("Using %s to fly home" % filename)
|
|
|
|
self.load_mission(filename)
|
|
|
|
self.change_mode("AUTO")
|
|
|
|
self.mavproxy.send('wp set 7\n')
|
2020-05-19 01:52:22 -03:00
|
|
|
self.wait_disarmed(timeout=timeout)
|
2020-01-11 13:59:37 -04:00
|
|
|
|
|
|
|
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)
|
|
|
|
|
2020-03-08 05:10:00 -03:00
|
|
|
def hover_and_check_matched_frequency(self, dblevel=-15, minhz=200, maxhz=300, fftLength=32, peakhz=None):
|
2020-01-11 13:59:37 -04:00
|
|
|
|
|
|
|
# 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:
|
2020-03-06 23:45:22 -04:00
|
|
|
self.mav.recv_match(type='ATTITUDE', blocking=True)
|
2020-01-11 13:59:37 -04:00
|
|
|
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
|
2020-03-13 13:13:54 -03:00
|
|
|
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])
|
2020-01-11 13:59:37 -04:00
|
|
|
if peakdb < dblevel or (peakhz is not None and abs(freq - peakhz) / peakhz > 0.05):
|
|
|
|
raise NotAchievedException("Did not detect a motor peak, found %fHz at %fdB" % (freq, peakdb))
|
|
|
|
else:
|
|
|
|
self.progress("Detected motor peak at %fHz, throttle %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()
|
2020-03-08 05:10:00 -03:00
|
|
|
# accuracy is determined by sample rate and fft length, given our use of quinn we could probably use half of this
|
|
|
|
freqDelta = 1000. / fftLength
|
2020-01-11 13:59:37 -04:00
|
|
|
pkAvg = freq
|
2020-03-13 13:13:54 -03:00
|
|
|
freqs = []
|
2020-01-11 13:59:37 -04:00
|
|
|
|
2020-03-06 21:13:17 -04:00
|
|
|
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
|
2020-03-13 13:13:54 -03:00
|
|
|
freqs.append(m.PkAvg)
|
|
|
|
|
2020-03-08 05:10:00 -03:00
|
|
|
# peak within resolution of FFT length
|
2020-03-13 13:13:54 -03:00
|
|
|
pkAvg = numpy.median(numpy.asarray(freqs))
|
2020-03-08 05:10:00 -03:00
|
|
|
if abs(pkAvg - freq) > freqDelta:
|
2020-01-11 13:59:37 -04:00
|
|
|
raise NotAchievedException("FFT did not detect a motor peak at %f, found %f, wanted %f" % (dblevel, pkAvg, freq))
|
|
|
|
|
|
|
|
return freq
|
|
|
|
|
|
|
|
def fly_gyro_fft(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_parameter("AHRS_EKF_TYPE", 10)
|
|
|
|
|
|
|
|
self.set_parameter("INS_LOG_BAT_MASK", 3)
|
|
|
|
self.set_parameter("INS_LOG_BAT_OPT", 0)
|
|
|
|
self.set_parameter("INS_GYRO_FILTER", 100)
|
|
|
|
self.set_parameter("LOG_BITMASK", 45054)
|
|
|
|
self.set_parameter("LOG_DISARMED", 0)
|
|
|
|
self.set_parameter("SIM_DRIFT_SPEED", 0)
|
|
|
|
self.set_parameter("SIM_DRIFT_TIME", 0)
|
|
|
|
# enable a noisy motor peak
|
|
|
|
self.set_parameter("SIM_GYR_RND", 20)
|
|
|
|
# enabling FFT will also enable the arming check, self-testing the functionality
|
|
|
|
self.set_parameter("FFT_ENABLE", 1)
|
|
|
|
self.set_parameter("FFT_MINHZ", 80)
|
|
|
|
self.set_parameter("FFT_MAXHZ", 350)
|
|
|
|
self.set_parameter("FFT_SNR_REF", 10)
|
|
|
|
self.set_parameter("FFT_WINDOW_SIZE", 128)
|
|
|
|
self.set_parameter("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_parameter("SIM_VIB_FREQ_X", 250)
|
|
|
|
self.set_parameter("SIM_VIB_FREQ_Y", 250)
|
|
|
|
self.set_parameter("SIM_VIB_FREQ_Z", 250)
|
|
|
|
|
|
|
|
self.reboot_sitl()
|
|
|
|
|
|
|
|
# find a motor peak
|
2020-03-08 05:10:00 -03:00
|
|
|
self.hover_and_check_matched_frequency(-15, 100, 350, 128, 250)
|
2020-01-11 13:59:37 -04:00
|
|
|
|
|
|
|
# Step 2: inject actual motor noise and use the standard length FFT to track it
|
|
|
|
self.set_parameter("SIM_VIB_MOT_MAX", 350)
|
|
|
|
self.set_parameter("FFT_WINDOW_SIZE", 32)
|
|
|
|
self.set_parameter("FFT_WINDOW_OLAP", 0.5)
|
|
|
|
|
|
|
|
self.reboot_sitl()
|
|
|
|
# find a motor peak
|
2020-03-08 05:10:00 -03:00
|
|
|
freq = self.hover_and_check_matched_frequency(-15, 200, 300, 32)
|
2020-01-11 13:59:37 -04:00
|
|
|
|
|
|
|
# Step 3: add a FFT dynamic notch and check that the peak is squashed
|
|
|
|
self.set_parameter("INS_LOG_BAT_OPT", 2)
|
|
|
|
self.set_parameter("INS_HNTCH_ENABLE", 1)
|
|
|
|
self.set_parameter("INS_HNTCH_FREQ", freq)
|
|
|
|
self.set_parameter("INS_HNTCH_REF", 1.0)
|
|
|
|
self.set_parameter("INS_HNTCH_ATT", 50)
|
|
|
|
self.set_parameter("INS_HNTCH_BW", freq/2)
|
|
|
|
self.set_parameter("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:
|
2020-03-06 23:45:22 -04:00
|
|
|
self.mav.recv_match(type='ATTITUDE', blocking=True)
|
2020-01-11 13:59:37 -04:00
|
|
|
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]
|
2020-03-13 13:13:54 -03:00
|
|
|
if numpy.amax(psd["X"][ignore_bins:]) < -10:
|
2020-01-11 13:59:37 -04:00
|
|
|
self.progress("Did not detect a motor peak, found %f at %f dB" % (freq, numpy.amax(psd["X"][ignore_bins:])))
|
|
|
|
else:
|
|
|
|
raise NotAchievedException("Detected motor peak at %f Hz" % (freq))
|
|
|
|
|
|
|
|
# Step 4: take off as a copter land as a plane, make sure we track
|
|
|
|
self.progress("Flying with gyro FFT - vtol to plane")
|
2020-03-10 09:18:59 -03:00
|
|
|
self.load_mission("quadplane-gyro-mission.txt")
|
2020-01-11 13:59:37 -04:00
|
|
|
self.mavproxy.send('wp list\n')
|
|
|
|
self.mavproxy.expect('Requesting [0-9]+ waypoints')
|
|
|
|
self.wait_ready_to_arm()
|
|
|
|
self.arm_vehicle()
|
|
|
|
self.mavproxy.send('mode AUTO\n')
|
|
|
|
self.wait_mode('AUTO')
|
|
|
|
self.wait_waypoint(1, 7, max_dist=60, timeout=1200)
|
2020-03-12 20:51:17 -03:00
|
|
|
self.wait_disarmed(timeout=120) # give quadplane a long time to land
|
2020-01-11 13:59:37 -04:00
|
|
|
|
|
|
|
# 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:
|
2020-03-06 21:13:17 -04:00
|
|
|
self.progress("Exception caught: %s" % (
|
|
|
|
self.get_exception_stacktrace(e)))
|
2020-01-11 13:59:37 -04:00
|
|
|
ex = e
|
|
|
|
|
|
|
|
self.context_pop()
|
|
|
|
|
2020-03-06 21:13:17 -04:00
|
|
|
self.reboot_sitl()
|
|
|
|
|
2020-01-11 13:59:37 -04:00
|
|
|
if ex is not None:
|
|
|
|
raise ex
|
|
|
|
|
2019-02-28 19:04:23 -04:00
|
|
|
def test_pid_tuning(self):
|
|
|
|
self.change_mode("FBWA") # we don't update PIDs in MANUAL
|
|
|
|
super(AutoTestQuadPlane, self).test_pid_tuning()
|
|
|
|
|
2019-03-04 23:52:24 -04:00
|
|
|
def test_parameter_checks(self):
|
|
|
|
self.test_parameter_checks_poscontrol("Q_P")
|
|
|
|
|
2020-07-21 18:48:55 -03:00
|
|
|
def rc_defaults(self):
|
|
|
|
ret = super(AutoTestQuadPlane, self).rc_defaults()
|
|
|
|
ret[3] = 1000
|
|
|
|
return ret
|
|
|
|
|
2018-12-17 17:52:09 -04:00
|
|
|
def default_mode(self):
|
2019-02-11 13:55:18 -04:00
|
|
|
return "MANUAL"
|
2018-12-17 17:52:09 -04:00
|
|
|
|
2019-02-04 05:45:41 -04:00
|
|
|
def disabled_tests(self):
|
|
|
|
return {
|
|
|
|
"QAutoTune": "See https://github.com/ArduPilot/ardupilot/issues/10411",
|
2020-01-11 13:59:37 -04:00
|
|
|
"FRSkyPassThrough": "Currently failing",
|
2020-02-28 20:14:11 -04:00
|
|
|
"CPUFailsafe": "servo channel values not scaled like ArduPlane",
|
2019-02-04 05:45:41 -04:00
|
|
|
}
|
|
|
|
|
2020-05-07 20:58:39 -03:00
|
|
|
def test_pilot_yaw(self):
|
|
|
|
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()
|
|
|
|
|
2020-02-28 20:14:11 -04:00
|
|
|
def CPUFailsafe(self):
|
|
|
|
'''In lockup Plane should copy RC inputs to RC outputs'''
|
|
|
|
self.plane_CPUFailsafe()
|
|
|
|
|
2020-05-19 01:52:22 -03:00
|
|
|
def test_qassist(self):
|
|
|
|
# find a motor peak
|
|
|
|
self.takeoff(10, mode="QHOVER")
|
|
|
|
self.set_rc(3, 1800)
|
|
|
|
self.change_mode("FBWA")
|
|
|
|
thr_min_pwm = self.get_parameter("Q_THR_MIN_PWM")
|
|
|
|
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, 1500)
|
2020-06-29 19:06:17 -03:00
|
|
|
|
|
|
|
self.context_push()
|
|
|
|
self.progress("Rolling over hard")
|
|
|
|
self.set_rc(1, 1000)
|
|
|
|
self.wait_roll(-65, 5)
|
|
|
|
self.progress("Killing servo outputs to force qassist to help")
|
2020-07-12 23:00:59 -03:00
|
|
|
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")
|
2020-06-29 19:06:17 -03:00
|
|
|
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(85, 5)
|
|
|
|
self.context_pop()
|
|
|
|
|
2020-05-19 01:52:22 -03:00
|
|
|
self.change_mode("RTL")
|
|
|
|
self.delay_sim_time(20)
|
|
|
|
self.change_mode("QRTL")
|
2020-06-29 19:06:17 -03:00
|
|
|
self.wait_disarmed(timeout=300)
|
2020-05-19 01:52:22 -03:00
|
|
|
|
2018-12-17 17:52:09 -04:00
|
|
|
def tests(self):
|
|
|
|
'''return list of all tests'''
|
|
|
|
|
|
|
|
ret = super(AutoTestQuadPlane, self).tests()
|
|
|
|
ret.extend([
|
2020-08-03 23:26:39 -03:00
|
|
|
("TestAirMode", "Test airmode", self.test_airmode),
|
|
|
|
|
2019-02-11 13:55:18 -04:00
|
|
|
("TestMotorMask", "Test output_motor_mask", self.test_motor_mask),
|
|
|
|
|
2020-05-07 20:58:39 -03:00
|
|
|
("PilotYaw",
|
|
|
|
"Test pilot yaw in various modes",
|
|
|
|
self.test_pilot_yaw),
|
|
|
|
|
2019-03-04 23:52:24 -04:00
|
|
|
("ParameterChecks",
|
|
|
|
"Test Arming Parameter Checks",
|
|
|
|
self.test_parameter_checks),
|
|
|
|
|
2020-05-11 20:29:39 -03:00
|
|
|
("TestLogDownload",
|
|
|
|
"Test Onboard Log Download",
|
|
|
|
self.test_log_download),
|
|
|
|
|
2018-12-17 17:52:09 -04:00
|
|
|
("Mission", "Dalby Mission",
|
2020-03-10 09:18:59 -03:00
|
|
|
lambda: self.fly_mission("Dalby-OBC2016.txt", "Dalby-OBC2016-fence.txt")),
|
2020-01-11 13:59:37 -04:00
|
|
|
|
2020-05-19 01:52:22 -03:00
|
|
|
("QAssist",
|
|
|
|
"QuadPlane Assist tests",
|
|
|
|
self.test_qassist),
|
|
|
|
|
2020-01-11 13:59:37 -04:00
|
|
|
("GyroFFT", "Fly Gyro FFT",
|
2020-07-21 21:17:02 -03:00
|
|
|
self.fly_gyro_fft),
|
|
|
|
|
|
|
|
("LogUpload",
|
|
|
|
"Log upload",
|
|
|
|
self.log_upload),
|
2018-12-17 17:52:09 -04:00
|
|
|
])
|
|
|
|
return ret
|