From 7bfd5d1bb71ce6e4881f1b6ad2eee2863e33ba94 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Mon, 18 Mar 2024 22:23:11 -0400 Subject: [PATCH] ArgenTech: FireEye RealFlight autotest class Most of this is dirty hackery, and can't actually run in CI or anything, but being able to create tests with the autotest framework is beneficial, particularly to thoroughly test the new engine-out script. --- .../EngineOutScript/mission.waypoints | 11 + Tools/autotest/autotest.py | 3 + .../default_params/fireeye-engout.parm | 11 + Tools/autotest/default_params/fireeye.parm | 175 ++++++++++ Tools/autotest/fireeye.py | 330 ++++++++++++++++++ 5 files changed, 530 insertions(+) create mode 100644 Tools/autotest/ArduPlane_Tests/EngineOutScript/mission.waypoints create mode 100644 Tools/autotest/default_params/fireeye-engout.parm create mode 100644 Tools/autotest/default_params/fireeye.parm create mode 100644 Tools/autotest/fireeye.py diff --git a/Tools/autotest/ArduPlane_Tests/EngineOutScript/mission.waypoints b/Tools/autotest/ArduPlane_Tests/EngineOutScript/mission.waypoints new file mode 100644 index 00000000000000..9e784405e6f3bf --- /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 970dc3bde03372..25102ac37ee482 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 00000000000000..8f00676916a5c2 --- /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 00000000000000..2ac06814204a46 --- /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 00000000000000..a301018e4abc8a --- /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, + ]