''' 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 import vehicle_test_suite from vehicle_test_suite import Test from vehicle_test_suite 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(vehicle_test_suite.TestSuite): @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''' 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("ROLL_LIMIT_DEG") 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) # Test angle assist 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) # Test alt assist, climb to 60m and set assist alt to 50m self.context_push() guided_loc = self.home_relative_loc_ne(0, 0) guided_loc.alt = 60 self.change_mode("GUIDED") self.send_do_reposition(guided_loc) self.wait_altitude(58, 62, relative=True) self.set_parameter("Q_ASSIST_ALT", 50) # Try and descent to 40m guided_loc.alt = 40 self.send_do_reposition(guided_loc) # Expect alt assist to kick in, eg "Alt assist 48.9m" self.wait_statustext(r"Alt assist \d*.\d*m", regex=True, timeout=100) # Test transition timeout, should switch to QRTL self.set_parameter("Q_TRANS_FAIL_ACT", 1) self.set_parameter("Q_TRANS_FAIL", 10) self.wait_mode("QRTL") self.context_pop() self.wait_disarmed(timeout=200) 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 PrecisionLanding(self): '''VTOL precision landing''' applet_script = "plane_precland.lua" self.install_applet_script(applet_script) here = self.mav.location() target = self.offset_location_ne(here, 20, 0) self.set_parameters({ "SCR_ENABLE": 1, "PLND_ENABLED": 1, "PLND_TYPE": 4, "SIM_PLD_ENABLE": 1, "SIM_PLD_LAT" : target.lat, "SIM_PLD_LON" : target.lng, "SIM_PLD_HEIGHT" : 0, "SIM_PLD_ALT_LMT" : 50, "SIM_PLD_DIST_LMT" : 30, "RNGFND1_TYPE": 100, "RNGFND1_PIN" : 0, "RNGFND1_SCALING" : 12.2, "RNGFND1_MAX_CM" : 5000, "RNGFND_LANDING" : 1, }) self.reboot_sitl() self.context_push() self.context_collect('STATUSTEXT') self.set_parameters({ "PLND_ALT_CUTOFF" : 5, "SIM_SPEEDUP" : 10, }) self.scripting_restart() self.wait_text("PLND: Loaded", check_context=True) self.wait_ready_to_arm() self.change_mode("GUIDED") self.arm_vehicle() self.takeoff(60, 'GUIDED') self.wait_altitude(58, 62, relative=True) self.drain_mav() self.change_mode("QRTL") self.wait_text("PLND: Target Acquired", check_context=True, timeout=60) self.wait_disarmed(timeout=180) loc2 = self.mav.location() error = self.get_distance(target, loc2) self.progress("Target error %.1fm" % error) if error > 2: raise NotAchievedException("too far from target %.1fm" % error) self.context_pop() self.remove_installed_script(applet_script) self.reboot_sitl() def ShipLanding(self): '''ship landing test''' applet_script = "plane_ship_landing.lua" self.install_applet_script(applet_script) self.set_parameters({ "SCR_ENABLE": 1, "SIM_SHIP_ENABLE": 1, "SIM_SHIP_SPEED": 5, "SIM_SHIP_DSIZE": 10, "FOLL_ENABLE": 1, "FOLL_SYSID": 17, "FOLL_OFS_TYPE": 1, "SIM_TERRAIN" : 0, "TERRAIN_ENABLE" : 0, }) self.load_mission("takeoff100.txt") self.reboot_sitl(check_position=False) self.context_push() self.context_collect('STATUSTEXT') self.set_parameters({ "SHIP_ENABLE" : 1, "SIM_SPEEDUP" : 10, }) self.scripting_restart() self.wait_text("ShipLanding: loaded", check_context=True) self.wait_ready_to_arm() self.change_mode("AUTO") self.arm_vehicle() self.wait_altitude(95, 105, relative=True, timeout=90) self.drain_mav() self.wait_text("Mission complete, changing mode to RTL", check_context=True, timeout=60) self.wait_text("Descending for approach", check_context=True, timeout=60) self.wait_text("Reached target altitude", check_context=True, timeout=120) self.wait_text("Starting approach", check_context=True, timeout=120) self.wait_text("Land complete", check_context=True, timeout=120) self.wait_disarmed(timeout=180) # we confirm successful landing on the ship from our ground speed. The # deck is just 10m in size, so we must be within 10m if we are moving # with the deck self.wait_groundspeed(4.8, 5.2) self.context_pop() self.remove_installed_script(applet_script) self.reboot_sitl(check_position=False) 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 Q_GUIDED_MODE(self): '''test moving in VTOL mode with SET_POSITION_TARGET_GLOBAL_INT''' self.set_parameter('Q_GUIDED_MODE', 1) self.change_mode('GUIDED') self.wait_ready_to_arm() self.arm_vehicle() self.run_cmd(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, p7=15) self.wait_altitude(14, 16, relative=True) loc = self.mav.location() self.location_offset_ne(loc, 50, 50) # set position target 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), 30, # alt frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, ) self.wait_location(loc, timeout=120) self.fly_home_land_and_disarm() def DCMClimbRate(self): '''Test the climb rate measurement in DCM with and without GPS''' self.wait_ready_to_arm() self.change_mode('QHOVER') self.arm_vehicle() self.set_rc(3, 2000) self.wait_altitude(30, 50, relative=True) # Start Descending self.set_rc(3, 1000) self.wait_climbrate(-5, -0.5, timeout=10) # Switch to DCM self.set_parameter('AHRS_EKF_TYPE', 0) self.delay_sim_time(5) # Start Climbing self.set_rc(3, 2000) self.wait_climbrate(0.5, 5, timeout=10) # Kill any GPSs self.set_parameters({ 'SIM_GPS_DISABLE': 1, 'SIM_GPS2_DISABLE': 1, }) self.delay_sim_time(5) # Start Descending self.set_rc(3, 1000) self.wait_climbrate(-5, -0.5, timeout=10) # Force disarm self.disarm_vehicle(force=True) def RTL_AUTOLAND_1(self): '''test behaviour when RTL_AUTOLAND==1''' self.set_parameters({ "RTL_AUTOLAND": 1, }) # when RTL is entered and RTL_AUTOLAND is 1 we should fly home # then to the landing sequence. This mission puts the landing # sequence well to the West of home so if we go directly there # we won't come within 200m of home wps = self.create_simple_relhome_mission([ (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 30), # fly North (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 500, 0, 30), # add a waypoint 1km North (which we will look for and trigger RTL (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 1000, 0, 30), # *exciting* landing sequence is ~1km West and points away from Home. self.create_MISSION_ITEM_INT( mavutil.mavlink.MAV_CMD_DO_LAND_START, ), (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, -1000, 30), (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, -1300, 15), (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, -1600, 5), (mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND, 0, -1750, 0), ]) self.check_mission_upload_download(wps) self.change_mode('AUTO') self.wait_ready_to_arm() self.arm_vehicle() self.wait_current_waypoint(3) # will be 2km North here self.change_mode('RTL') self.wait_distance_to_home(100, 200, timeout=120) self.wait_current_waypoint(7) self.fly_home_land_and_disarm() def send_reposition_to_loc(self, loc): 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), 20, # alt frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, ) def reposition_to_loc(self, loc, accuracy=100): self.send_reposition_to_loc(loc) self.wait_location( loc, accuracy=accuracy, minimum_duration=20, timeout=120, ) def RTL_AUTOLAND_1_FROM_GUIDED(self): '''test behaviour when RTL_AUTOLAND==1 and entering from guided''' self.set_parameters({ "RTL_AUTOLAND": 1, }) # when RTL is entered and RTL_AUTOLAND is 1 we should fly home # then to the landing sequence. This mission puts the landing # sequence well to the West of home so if we go directly there # we won't come within 200m of home wps = self.create_simple_relhome_mission([ (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 30), # fly North (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 500, 0, 30), # add a waypoint 1km North (which we will look for and trigger RTL (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 1000, 0, 30), # *exciting* landing sequence is ~1km West and points away from Home. self.create_MISSION_ITEM_INT( mavutil.mavlink.MAV_CMD_DO_LAND_START, ), (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, -1000, 30), (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, -1300, 15), (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, -1600, 5), (mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND, 0, -1750, 0), ]) self.check_mission_upload_download(wps) self.set_current_waypoint(0, check_afterwards=False) self.change_mode('AUTO') self.wait_ready_to_arm() here = self.mav.location() guided_loc = self.offset_location_ne(here, 500, -500) self.arm_vehicle() self.wait_current_waypoint(3) # will be 2km North here self.reposition_to_loc(guided_loc) self.send_cmd_do_set_mode('RTL') self.wait_distance_to_home(100, 200, timeout=120) self.wait_current_waypoint(7) self.fly_home_land_and_disarm() 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.TestLogDownload, self.TestLogDownloadWrap, 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, self.PrecisionLanding, self.ShipLanding, 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, self.Q_GUIDED_MODE, self.DCMClimbRate, self.RTL_AUTOLAND_1, # as in fly-home then go to landing sequence self.RTL_AUTOLAND_1_FROM_GUIDED, # as in fly-home then go to landing sequence ]) return ret