diff --git a/Tools/autotest/ArduPlane_Tests/EngineOutScript/mission.waypoints b/Tools/autotest/ArduPlane_Tests/EngineOutScript/mission.waypoints new file mode 100644 index 0000000000000..9e784405e6f3b --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/EngineOutScript/mission.waypoints @@ -0,0 +1,11 @@ +QGC WPL 110 +0 1 0 16 0 0 0 0 36.8325080 -2.8512098 224.024950 1 +1 0 3 84 0.00000000 0.00000000 0.00000000 0.00000000 0.00000000 0.00000000 6.096000 1 +2 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 36.82814570 -2.85303350 6.096000 1 +3 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 36.82386890 -2.85902020 6.096000 1 +4 0 3 17 0.00000000 0.00000000 0.00000000 0.00000000 36.82598160 -2.86116600 6.096000 1 +5 0 3 189 0.00000000 0.00000000 0.00000000 0.00000000 36.83294860 -2.85484150 60.960000 1 +6 0 3 18 0.00000000 0.00000000 152.40000000 0.00000000 36.83294860 -2.85484150 60.960000 1 +7 0 3 31 0.00000000 152.40000000 0.00000000 0.00000000 36.83294860 -2.85484150 30.480000 1 +8 0 3 18 1.00000000 0.00000000 152.40000000 1.00000000 36.83294860 -2.85484150 30.480000 1 +9 0 3 85 0.00000000 0.00000000 0.00000000 0.00000000 36.83250810 -2.85121000 0.000000 1 diff --git a/Tools/autotest/autotest.py b/Tools/autotest/autotest.py index 970dc3bde0337..25102ac37ee48 100755 --- a/Tools/autotest/autotest.py +++ b/Tools/autotest/autotest.py @@ -31,6 +31,7 @@ import balancebot import sailboat import helicopter +import fireeye import examples from pysim import util @@ -288,6 +289,7 @@ def should_run_step(step): "Sailboat": "ardurover", "SITLPeriphUniversal": "sitl_periph_universal.AP_Periph", "CAN": "arducopter", + "FireEye": "arduplane", } @@ -359,6 +361,7 @@ def find_specific_test_to_run(step): "test.Sub": ardusub.AutoTestSub, "test.Tracker": antennatracker.AutoTestTracker, "test.CAN": arducopter.AutoTestCAN, + "test.FireEye": fireeye.AutoTestFireEye, } supplementary_test_binary_map = { diff --git a/Tools/autotest/default_params/fireeye-engout.parm b/Tools/autotest/default_params/fireeye-engout.parm new file mode 100644 index 0000000000000..8f00676916a5c --- /dev/null +++ b/Tools/autotest/default_params/fireeye-engout.parm @@ -0,0 +1,11 @@ +ENGOUT_AUX_FUNC,307 +ENGOUT_DELAY,2 +ENGOUT_FS_ENABLE,1 +ENGOUT_GLIDE_SPD,22 +ENGOUT_QAST_GSPD,5 +ENGOUT_QAST_TIME,75 +ENGOUT_QRTL_TIME,45 +ENGOUT_RPM_CHAN,2 +ENGOUT_RPM_THRSH,500 +ENGOUT_VIB_THRSH,10000 +Q_RTL_ALT, 60 diff --git a/Tools/autotest/default_params/fireeye.parm b/Tools/autotest/default_params/fireeye.parm new file mode 100644 index 0000000000000..2ac06814204a4 --- /dev/null +++ b/Tools/autotest/default_params/fireeye.parm @@ -0,0 +1,175 @@ +AHRS_EKF_TYPE,10 +AIRSPEED_CRUISE,22 +AIRSPEED_MAX,27 +AIRSPEED_MIN,20 +ARMING_RUDDER,2 +ARSPD_TYPE,100 +ARSPD_USE,1 +BATT_MONITOR,3 +BATT_VOLT_MULT,5.61 +EFI_FUEL_DENS,1 +EFI_TYPE,7 +EK3_IMU_MASK,1 +EK3_OGN_HGT_MASK,1 +FBWB_CLIMB_RATE,1 +FENCE_ALT_MIN,30 +FENCE_ENABLE,1 +FENCE_OPTIONS,0 +FLIGHT_OPTIONS,2 +FLTMODE_CH,0 +FS_GCS_ENABL,1 +FS_LONG_ACTN,1 +ICE_ENABLE,1 +ICE_OPTIONS,4 +ICE_START_PCT,0 +ICE_STARTER_TIME,0.1 +LAND_DISARMDELAY,3 +LEVEL_ROLL_LIMIT,7 +MIXING_GAIN,0.75 +NAVL1_LIM_BANK,25 +NAVL1_PERIOD,20 +PTCH_LIM_MAX_DEG,10 +PTCH_LIM_MIN_DEG,-10 +PTCH_RATE_D,0.0793846 +PTCH_RATE_FF,1.744243 +PTCH_RATE_FLTD,10 +PTCH_RATE_FLTT,1.515761 +PTCH_RATE_I,2.642293 +PTCH_RATE_IMAX,0.67 +PTCH_RATE_P,3.523058 +PTCH_TRIM_DEG,2.5 +PTCH2SRV_RMAX_DN,75 +PTCH2SRV_RMAX_UP,75 +PTCH2SRV_TCONST,0.75 +Q_A_ANG_LIM_TC,2 +Q_A_INPUT_TC,0.15 +Q_A_RAT_PIT_D,0.0115453 +Q_A_RAT_PIT_FLTT,10 +Q_A_RAT_PIT_I,0.509538 +Q_A_RAT_PIT_P,0.509538 +Q_A_RAT_RLL_D,0.0138788 +Q_A_RAT_RLL_FLTT,10 +Q_A_RAT_RLL_I,0.5261019 +Q_A_RAT_RLL_P,0.5261019 +Q_A_RAT_YAW_D,0.01 +Q_A_RAT_YAW_FLTD,10 +Q_A_RAT_YAW_FLTE,2 +Q_A_RAT_YAW_FLTT,10 +Q_A_RAT_YAW_I,0.05 +Q_A_RAT_YAW_P,0.4999999 +Q_A_RATE_P_MAX,30 +Q_A_RATE_R_MAX,30 +Q_A_THR_MIX_MIN,0.2 +Q_ACRO_PIT_RATE,40 +Q_ACRO_RLL_RATE,40 +Q_ACRO_YAW_RATE,40 +Q_ANGLE_MAX,1000 +Q_ASSIST_ANGLE,20 +Q_ASSIST_SPEED,18 +Q_ENABLE,1 +Q_FRAME_CLASS,4 +Q_FRAME_TYPE,3 +Q_FW_LND_APR_RAD,200 +Q_FWD_MANTHR_MAX,100 +Q_FWD_PIT_LIM,2.5 +Q_FWD_THR_GAIN,3.9 +Q_LAND_FINAL_ALT,9 +Q_LAND_FINAL_SPD,0.9 +Q_LAND_ICE_CUT,0 +Q_LOIT_ACC_MAX,100 +Q_LOIT_ANG_MAX,20 +Q_LOIT_BRK_DELAY,0 +Q_LOIT_BRK_JERK,500 +Q_M_HOVER_LEARN,1 +Q_M_SPIN_ARM,0.095 +Q_M_SPIN_MAX,1 +Q_M_SPIN_MIN,0.1 +Q_M_THST_HOVER,0.5 +Q_M_YAW_HEADROOM,100 +Q_OPTIONS,1 +Q_P_ACCZ_I,0.15 +Q_P_ACCZ_IMAX,200 +Q_P_VELXY_D,0 +Q_P_VELXY_I,0.17 +Q_PILOT_SPD_DN,1 +Q_PILOT_SPD_UP,2 +Q_PLT_Y_RATE,25 +Q_RTL_ALT,30 +Q_TRAN_PIT_MAX,1 +Q_TRANS_DECEL,1 +Q_TRANSITION_MS,1000 +Q_VFWD_ALT,2 +Q_WP_SPEED_DN,225 +Q_WP_SPEED_UP,200 +Q_WVANE_ANG_MIN,0.5 +Q_WVANE_GAIN,2 +RALLY_INCL_HOME,0 +RALLY_LIMIT_KM,50 +RC_OPTIONS,288 +RC3_TRIM,1000 +RC7_OPTION,96 +RC8_OPTION,4 +RELAY2_FUNCTION,2 +RELAY2_PIN,1 +RLL_RATE_D,0.0481776 +RLL_RATE_FF,0.8620883 +RLL_RATE_FLTD,10 +RLL_RATE_FLTT,2.273642 +RLL_RATE_I,0.8620883 +RLL_RATE_IMAX,0.67 +RLL_RATE_P,0.9492083 +RLL2SRV_RMAX,75 +ROLL_LIMIT_DEG,25 +RPM1_TYPE,0 +RPM2_TYPE,10 +RTL_ALTITUDE,-1 +RTL_AUTOLAND,2 +RTL_RADIUS,152 +SCALING_SPEED,20 +SCR_ENABLE,1 +SERIAL3_BAUD,38 +SERIAL4_BAUD,38 +SERVO_AUTO_TRIM,1 +SERVO1_TRIM,1508 +SERVO10_FUNCTION,38 +SERVO11_FUNCTION,39 +SERVO12_FUNCTION,40 +SERVO2_FUNCTION,80 +SERVO2_REVERSED,1 +SERVO2_TRIM,1400 +SERVO3_MAX,2000 +SERVO3_MIN,1000 +SERVO3_TRIM,1000 +SERVO4_FUNCTION,79 +SERVO4_TRIM,1600 +SERVO5_FUNCTION,33 +SERVO6_FUNCTION,34 +SERVO7_FUNCTION,35 +SERVO8_FUNCTION,36 +SERVO9_FUNCTION,37 +SIM_GPS_NUMSATS,69 +SIM_MAG1_OFS_X,5 +SIM_MAG1_OFS_Y,13 +SIM_MAG1_OFS_Z,-18 +SIM_MAG2_OFS_X,5 +SIM_MAG2_OFS_Y,13 +SIM_MAG2_OFS_Z,-18 +SIM_MAG3_OFS_X,5 +SIM_MAG3_OFS_Y,13 +SIM_MAG3_OFS_Z,-18 +STAB_PITCH_DOWN,5 +TECS_CLMB_MAX,1.6 +TECS_PITCH_MAX,3 +TECS_PITCH_MIN,-8 +TECS_PTCH_DAMP,0.1 +TECS_RLL2THR,13 +TECS_SINK_MAX,4 +TECS_SINK_MIN,2.5 +TECS_SPDWEIGHT,1.1 +TECS_TIME_CONST,7 +THR_SLEWRATE,50 +THROTTLE_NUDGE,0 +TRIM_THROTTLE,52 +USE_REV_THRUST,0 +WP_LOITER_RAD,304 +WP_RADIUS,118 diff --git a/Tools/autotest/fireeye.py b/Tools/autotest/fireeye.py new file mode 100644 index 0000000000000..a301018e4abc8 --- /dev/null +++ b/Tools/autotest/fireeye.py @@ -0,0 +1,330 @@ +''' +Fly FireEye in RealFlight +''' + +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 +import shutil +import random + + +# 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(36.8325082, -2.8512096, 735, 0) # AutoTest Hill + + +class AutoTestFireEye(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 "flightaxis_autotest:" + os.environ['wslhost'] + + def test_filepath(self): + return os.path.realpath(__file__) + + def sitl_start_location(self): + return SITL_START_LOCATION + + def default_speedup(self): + '''RealFlight doesn't support speedup''' + return 1 + + def log_name(self): + return "FireEye" + + 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 os.path.join(testdir, "default_params/fireeye.parm") + + def is_plane(self): + return True + + def default_mode(self): + return "QHOVER" + + 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 reset_aircraft(self): + '''Reset aircraft and wait for it to be ready to arm''' + self.disarm_vehicle(force=True) + self.reboot_sitl() + self.repeatedly_apply_parameter_file( + os.path.join(testdir, 'default_params/fireeye-engout.parm')) + self.set_rc(3, 1000) + self.change_mode('QHOVER') + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_ENGINE_CONTROL, p1=1) + self.wait_rpm(2, 1000, 3000) + self.wait_ready_to_arm() + + def do_guided(self, location): + '''Fly to a location in GUIDED mode''' + self.change_mode('GUIDED') + self.mav.mav.mission_item_int_send( + 1, + 1, + 0, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, + mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, + 2, # current + 0, # autocontinue + 0, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + int(location.lat * 1e7), # latitude + int(location.lng * 1e7), # longitude + location.alt) # altitude + + def EngineOutScript(self): + '''Test engine out script in RealFlight on an AgTS FireEye''' + def basic_auto_mission(heading, target, is_guided=False, + min_distance = 0, max_distance = 50, + qassist_timeout=0, qrtl_timeout=0): + ''' + Run the basic auto mission, and kill the engine when we reach + the desired altitude and heading. By testing many different + headings, we confirm that the landing behavior works regardless of + which angle we happen to be facing relative to the wind when we + reach the desired landing altitude. + + Specify the target landing point, which is a rally point by default + or a guided point if is_guided is True. + + You can optionally specify a minimum and maximum distance to the + target for the test to pass, and you can override the Q_ASSIST and + QRTL timeouts to test that they work as expected. + ''' + + subtest_message = \ + f"Basic mission test with heading {heading:.0f}" + \ + " and " + ("guided" if is_guided else "rally") + \ + f" {target.lat:.6f}, {target.lng:.6f}" + + if qassist_timeout: + subtest_message = "Testing that the Q_ASSIST timeout works" + if qrtl_timeout: + subtest_message = "Testing that the QRTL timeout works" + + self.start_subtest(subtest_message) + + self.reset_aircraft() + if not is_guided: + self.upload_rally_points_from_locations([target]) + if qassist_timeout: + self.set_parameter("ENGOUT_QAST_TIME", qassist_timeout) + if qrtl_timeout: + self.set_parameter("ENGOUT_QRTL_TIME", qrtl_timeout) + self.change_mode('AUTO') + self.arm_vehicle() + self.set_rc(3, 1500) + + self.wait_current_waypoint(4, timeout=600) + + # Wait for the aircraft to reach the desired heading + self.wait_heading(heading, 5, timeout=300) + + # Kill the engine + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_ENGINE_CONTROL, p1=0) + + if is_guided: + self.wait_mode('RTL') + self.delay_sim_time(10) + self.do_guided(target) + + if qassist_timeout: + self.wait_text("Q_ASSIST for too long") + elif qrtl_timeout: + self.wait_text("QRTL for too long") + + # Wait for the aircraft to land + self.wait_disarmed(timeout=600) + + # Confirm the expected distance to the target + self.assert_distance(target, self.mav.location(), + min_distance=min_distance, + max_distance=max_distance) + + self.end_subtest(subtest_message) + + # Install the scripts + source = os.path.join(self.rootdir(), "libraries", "AP_Scripting", "modules", "utilities.lua") # noqa: E501 + destdir = os.path.join(self.rootdir(),"scripts", "modules") + dest = "utilities.lua" + if not os.path.exists(destdir): + os.mkdir(destdir) + self.progress("Copying (%s) to (%s)" % (source, dest)) + shutil.copy(source, os.path.join(destdir, dest)) + + source = os.path.join(self.rootdir(), "libraries", "AP_Scripting", "drivers", "EFI_SITL.lua") # noqa: E501 + self.install_script(source, "EFI_SITL.lua") + self.install_applet_script("engine_out.lua") + + # Reboot + self.reboot_sitl() + + # Disable fence + self.set_parameter('FENCE_ENABLE', 0) + + # Set the SYSID_MYGCS to the source system (prevent GCS failsafe) + self.set_parameter("SYSID_MYGCS", self.mav.source_system) + + # Upload mission + self.load_mission("mission.waypoints") + # Load rally point + # (we set a stupid altitude on purpose; it should not be used) + rally_loc = mavutil.location(36.8164241, -2.868918, 5000, 0) + guided_loc = mavutil.location(36.8192676, -2.8719136, 5000, 0) + + # ============================================= + # Test guided override + # ============================================= + self.upload_rally_points_from_locations([rally_loc]) + basic_auto_mission(270, guided_loc, is_guided=True) + + # ============================================= + # Test regular landings from many random angles + # ============================================= + + # Killing the engine at random headings to make sure the landing always + # goes smoothly no matter which orientation compared to the wind we are + # at when we reach the landing altitude. + headings = list(range(0, 360, 90)) + random.sample(range(360), 4) + for heading in headings: + basic_auto_mission(heading, rally_loc) + + # ============================================= + # Test the Q_ASSIST timeout + # ============================================= + basic_auto_mission(270, rally_loc, qassist_timeout=1, min_distance=0, max_distance=300) + + # ============================================= + # Test the QRTL timeout + # ============================================= + basic_auto_mission(270, rally_loc, qrtl_timeout=5, min_distance=50, max_distance=300) + + # ===================================================================== + # Test detection messages, and the backup and restore of all parameters + # ===================================================================== + self.start_subtest("Testing detections and parameters backup/restore") + self.reset_aircraft() + params_before, _ = self.download_parameters(self.sysid_thismav(), 1) + self.change_mode('AUTO') + self.arm_vehicle() + self.set_rc(3, 1500) + self.wait_current_waypoint(4, timeout=600) + # Kill the engine + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_ENGINE_CONTROL, p1=0) + self.wait_text("Engine out") + self.delay_sim_time(1) + # Read all parameters + params_after, _ = self.download_parameters(self.sysid_thismav(), 1) + # I don't know why Q_P_ACCZ_IMAX changes randomly, but it's unrelated + param_ignore_filter = ["STAT", "BARO", "SERVO", "Q_P_ACCZ_IMAX"] + # Print differences + for p in params_before: + if any(p.startswith(s) for s in param_ignore_filter): + continue + if params_before[p] != params_after[p]: + self.progress(f"{p} changed from {params_before[p]} to {params_after[p]}") + + # Restore the engine + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_ENGINE_CONTROL, p1=1) + self.wait_text("Engine running") + self.delay_sim_time(1) + # Read the parameters again + params_after, _ = self.download_parameters(self.sysid_thismav(), 1) + # Assert that all parameters are the same + for p in params_before: + if any(p.startswith(s) for s in param_ignore_filter): + continue + if params_before[p] != params_after[p]: + raise ValueError(f"{p} changed from {params_before[p]} to {params_after[p]}") + + # Force disarm and end the subtest + self.disarm_vehicle(force=True) + self.wait_disarmed() + self.end_subtest("Testing detections and parameters backup/restore") + + # ============================================= + # Test prearm checks + # ============================================= + self.start_subtest("Testing prearm checks") + self.reset_aircraft() + + # Deliberately set too low of a value for GLIDE_SPD + self.set_parameter("GLIDE_SPD", 1) + self.wait_not_ready_to_arm() + self.set_parameter("GLIDE_SPD", 22) + self.wait_ready_to_arm() + + # Deliberately set too high of a value for GLIDE_SPD + self.set_parameter("GLIDE_SPD", 100) + self.wait_not_ready_to_arm() + self.set_parameter("GLIDE_SPD", 22) + self.wait_ready_to_arm() + + # Turn off the engine + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_ENGINE_CONTROL, p1=0) + self.wait_not_ready_to_arm() + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_ENGINE_CONTROL, p1=1) + self.wait_ready_to_arm() + + self.end_subtest("Testing prearm checks") + + self.remove_installed_modules("utilities.lua") + self.remove_installed_script("EFI_SITL.lua") + self.remove_installed_script("engine_out.lua") + + def tests(self): + '''return list of all tests''' + return [ + self.EngineOutScript, + ]