From 1e6e291b527a6186e0548be6133642a0722a7a2a Mon Sep 17 00:00:00 2001 From: George Zogopoulos Date: Tue, 21 May 2024 19:34:43 +0200 Subject: [PATCH] autotest: New tests Autotests for takeoffs have been added for Plane, covering AUTO and TAKEOFF mode takeoffs. An auxiliary `set_servo` method has been added to `vehicle_test_suite.py`. --- .../ArduPlane_Tests/TakeoffAuto1/catapult.txt | 13 + .../ArduPlane_Tests/TakeoffAuto2/catapult.txt | 13 + .../ArduPlane_Tests/TakeoffAuto3/catapult.txt | 13 + .../ArduPlane_Tests/TakeoffAuto4/catapult.txt | 13 + Tools/autotest/arduplane.py | 350 ++++++++++++++++++ Tools/autotest/vehicle_test_suite.py | 4 + 6 files changed, 406 insertions(+) create mode 100644 Tools/autotest/ArduPlane_Tests/TakeoffAuto1/catapult.txt create mode 100644 Tools/autotest/ArduPlane_Tests/TakeoffAuto2/catapult.txt create mode 100644 Tools/autotest/ArduPlane_Tests/TakeoffAuto3/catapult.txt create mode 100644 Tools/autotest/ArduPlane_Tests/TakeoffAuto4/catapult.txt diff --git a/Tools/autotest/ArduPlane_Tests/TakeoffAuto1/catapult.txt b/Tools/autotest/ArduPlane_Tests/TakeoffAuto1/catapult.txt new file mode 100644 index 0000000000..0aa9cd624d --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/TakeoffAuto1/catapult.txt @@ -0,0 +1,13 @@ +QGC WPL 110 +0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.363262 149.165237 584.390015 1 +1 0 3 22 15.000000 0.000000 0.000000 0.000000 -35.361279 149.164230 100.000000 1 +2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361229 149.163025 100.000000 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364563 149.163773 100.000000 1 +4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364384 149.164795 80.000000 1 +5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361027 149.164093 80.000000 1 +6 0 0 177 2.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 +7 0 3 189 0.000000 0.000000 0.000000 0.000000 -35.362915 149.162613 60.000000 1 +8 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363136 149.162750 60.000000 1 +9 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365467 149.164215 55.000000 1 +10 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365009 149.165482 39.889999 1 +11 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.363041 149.165222 0.000000 1 diff --git a/Tools/autotest/ArduPlane_Tests/TakeoffAuto2/catapult.txt b/Tools/autotest/ArduPlane_Tests/TakeoffAuto2/catapult.txt new file mode 100644 index 0000000000..0aa9cd624d --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/TakeoffAuto2/catapult.txt @@ -0,0 +1,13 @@ +QGC WPL 110 +0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.363262 149.165237 584.390015 1 +1 0 3 22 15.000000 0.000000 0.000000 0.000000 -35.361279 149.164230 100.000000 1 +2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361229 149.163025 100.000000 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364563 149.163773 100.000000 1 +4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364384 149.164795 80.000000 1 +5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361027 149.164093 80.000000 1 +6 0 0 177 2.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 +7 0 3 189 0.000000 0.000000 0.000000 0.000000 -35.362915 149.162613 60.000000 1 +8 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363136 149.162750 60.000000 1 +9 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365467 149.164215 55.000000 1 +10 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365009 149.165482 39.889999 1 +11 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.363041 149.165222 0.000000 1 diff --git a/Tools/autotest/ArduPlane_Tests/TakeoffAuto3/catapult.txt b/Tools/autotest/ArduPlane_Tests/TakeoffAuto3/catapult.txt new file mode 100644 index 0000000000..0aa9cd624d --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/TakeoffAuto3/catapult.txt @@ -0,0 +1,13 @@ +QGC WPL 110 +0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.363262 149.165237 584.390015 1 +1 0 3 22 15.000000 0.000000 0.000000 0.000000 -35.361279 149.164230 100.000000 1 +2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361229 149.163025 100.000000 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364563 149.163773 100.000000 1 +4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364384 149.164795 80.000000 1 +5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361027 149.164093 80.000000 1 +6 0 0 177 2.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 +7 0 3 189 0.000000 0.000000 0.000000 0.000000 -35.362915 149.162613 60.000000 1 +8 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363136 149.162750 60.000000 1 +9 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365467 149.164215 55.000000 1 +10 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365009 149.165482 39.889999 1 +11 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.363041 149.165222 0.000000 1 diff --git a/Tools/autotest/ArduPlane_Tests/TakeoffAuto4/catapult.txt b/Tools/autotest/ArduPlane_Tests/TakeoffAuto4/catapult.txt new file mode 100644 index 0000000000..0aa9cd624d --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/TakeoffAuto4/catapult.txt @@ -0,0 +1,13 @@ +QGC WPL 110 +0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.363262 149.165237 584.390015 1 +1 0 3 22 15.000000 0.000000 0.000000 0.000000 -35.361279 149.164230 100.000000 1 +2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361229 149.163025 100.000000 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364563 149.163773 100.000000 1 +4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364384 149.164795 80.000000 1 +5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361027 149.164093 80.000000 1 +6 0 0 177 2.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 +7 0 3 189 0.000000 0.000000 0.000000 0.000000 -35.362915 149.162613 60.000000 1 +8 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363136 149.162750 60.000000 1 +9 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365467 149.164215 55.000000 1 +10 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365009 149.165482 39.889999 1 +11 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.363041 149.165222 0.000000 1 diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index cf319f97da..45a8f9ddf9 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -4371,6 +4371,349 @@ class AutoTestPlane(vehicle_test_suite.TestSuite): self.wait_disarmed(timeout=180) + def TakeoffAuto1(self): + '''Test the behaviour of an AUTO takeoff, pt1. + + Conditions: + - ARSPD_USE=1 + - TKOFF_TYPE=0 + - TKOFF_THR_MAX < THR_MAX + ''' + + self.customise_SITL_commandline( + [], + model='plane-catapult', + defaults_filepath=self.model_defaults_filepath("plane") + ) + self.set_parameters({ + "ARSPD_USE": 1.0, + "THR_MAX": 100.0, + "TKOFF_THR_MAX": 80.0, + "TKOFF_THR_MINACC": 3.0, + "TECS_PITCH_MAX": 35.0, + "PTCH_LIM_MAX_DEG": 35.0, + "RTL_AUTOLAND": 2, # The mission contains a DO_LAND_START item. + }) + + # Load and start mission. It contains a MAV_CMD_NAV_TAKEOFF item at 100m. + self.load_mission("catapult.txt", strict=True) + self.change_mode('AUTO') + + self.wait_ready_to_arm() + self.arm_vehicle() + + # Throw the catapult. + self.set_servo(7, 2000) + + # Wait until we're midway through the climb. + test_alt = 50 + self.wait_altitude(test_alt, test_alt+2, relative=True) + + # Ensure that by then the aircraft still goes at max allowed throttle. + self.assert_servo_channel_value(3, 1000+10*self.get_parameter("TKOFF_THR_MAX")) + + # Wait for landing waypoint. + self.wait_current_waypoint(11, timeout=1200) + self.wait_disarmed(120) + + def TakeoffAuto2(self): + '''Test the behaviour of an AUTO takeoff, pt2. + + Conditions: + - ARSPD_USE=1 + - TKOFF_MODE=0 + - TKOFF_THR_MAX > THR_MAX + ''' + + self.customise_SITL_commandline( + [], + model='plane-catapult', + defaults_filepath=self.model_defaults_filepath("plane") + ) + self.set_parameters({ + "ARSPD_USE": 0.0, + "THR_MAX": 80.0, + "TKOFF_THR_MAX": 100.0, + "TKOFF_THR_MINACC": 3.0, + "TECS_PITCH_MAX": 35.0, + "PTCH_LIM_MAX_DEG": 35.0, + "RTL_AUTOLAND": 2, # The mission contains a DO_LAND_START item. + }) + + # Load and start mission. It contains a MAV_CMD_NAV_TAKEOFF item at 100m. + self.load_mission("catapult.txt", strict=True) + self.change_mode('AUTO') + + self.wait_ready_to_arm() + self.arm_vehicle() + + # Throw the catapult. + self.set_servo(7, 2000) + + # Wait until we're midway through the climb. + test_alt = 50 + self.wait_altitude(test_alt, test_alt+2, relative=True) + + # Ensure that by then the aircraft still goes at max allowed throttle. + self.assert_servo_channel_value(3, 1000+10*self.get_parameter("TKOFF_THR_MAX")) + + # Wait for landing waypoint. + self.wait_current_waypoint(11, timeout=1200) + self.wait_disarmed(120) + + def TakeoffAuto3(self): + '''Test the behaviour of an AUTO takeoff, pt3. + + Conditions: + - ARSPD_USE=1 + - TKOFF_MODE=1 + ''' + + self.customise_SITL_commandline( + [], + model='plane-catapult', + defaults_filepath=self.model_defaults_filepath("plane") + ) + self.set_parameters({ + "ARSPD_USE": 1.0, + "THR_MAX": 80.0, + "THR_MIN": 0.0, + "TKOFF_MODE": 1.0, + "TKOFF_THR_MAX": 100.0, + "TKOFF_THR_MINACC": 3.0, + "TECS_PITCH_MAX": 35.0, + "TKOFF_THR_MAX_T": 3.0, + "PTCH_LIM_MAX_DEG": 35.0, + "RTL_AUTOLAND": 2, # The mission contains a DO_LAND_START item. + }) + + # Load and start mission. It contains a MAV_CMD_NAV_TAKEOFF item at 100m. + self.load_mission("catapult.txt", strict=True) + self.change_mode('AUTO') + + self.wait_ready_to_arm() + self.arm_vehicle() + + # Throw the catapult. + self.set_servo(7, 2000) + + # Ensure that TKOFF_THR_MAX_T is respected. + self.delay_sim_time(self.get_parameter("TKOFF_THR_MAX_T")-1) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX")-1)) + + # Ensure that after that the aircraft does not go full throttle anymore. + test_alt = 50 + self.wait_altitude(test_alt, test_alt+2, relative=True) + self.assert_servo_channel_value(3, 1000+10*self.get_parameter("TKOFF_THR_MAX")-10, operator.lt) + + # Wait for landing waypoint. + self.wait_current_waypoint(11, timeout=1200) + self.wait_disarmed(120) + + def TakeoffAuto4(self): + '''Test the behaviour of an AUTO takeoff, pt4. + + Conditions: + - ARSPD_USE=0 + - TKOFF_MODE=1 + ''' + + self.customise_SITL_commandline( + [], + model='plane-catapult', + defaults_filepath=self.model_defaults_filepath("plane") + ) + self.set_parameters({ + "ARSPD_USE": 0.0, + "THR_MAX": 80.0, + "THR_MIN": 0.0, + "TKOFF_MODE": 1.0, + "TKOFF_THR_MAX": 100.0, + "TKOFF_THR_MINACC": 3.0, + "TECS_PITCH_MAX": 35.0, + "TKOFF_THR_MAX_T": 3.0, + "PTCH_LIM_MAX_DEG": 35.0, + "RTL_AUTOLAND": 2, # The mission contains a DO_LAND_START item. + }) + + # Load and start mission. It contains a MAV_CMD_NAV_TAKEOFF item at 100m. + self.load_mission("catapult.txt", strict=True) + self.change_mode('AUTO') + + self.wait_ready_to_arm() + self.arm_vehicle() + + # Throw the catapult. + self.set_servo(7, 2000) + + # Ensure that TKOFF_THR_MAX_T is respected. + self.delay_sim_time(self.get_parameter("TKOFF_THR_MAX_T")-1) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX")), operator.le) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX"))-10, operator.ge) + + # Ensure that after that the aircraft still goes to maximum throttle. + test_alt = 50 + self.wait_altitude(test_alt, test_alt+2, relative=True) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX")), operator.le) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX"))-10, operator.ge) + + # Wait for landing waypoint. + self.wait_current_waypoint(11, timeout=1200) + self.wait_disarmed(120) + + def TakeoffTakeoff1(self): + '''Test the behaviour of a takeoff in TAKEOFF mode, pt1. + + Conditions: + - ARSPD_USE=1 + - TKOFF_MODE=0 + - TKOFF_THR_MAX < THR_MAX + ''' + + self.customise_SITL_commandline( + [], + model='plane-catapult', + defaults_filepath=self.model_defaults_filepath("plane") + ) + self.set_parameters({ + "ARSPD_USE": 1.0, + "THR_MAX": 100.0, + "TKOFF_LVL_ALT": 30.0, + "TKOFF_ALT": 100.0, + "TKOFF_MODE": 0.0, + "TKOFF_THR_MINACC": 3.0, + "TKOFF_THR_MAX": 80.0, + "TECS_PITCH_MAX": 35.0, + "PTCH_LIM_MAX_DEG": 35.0, + }) + self.change_mode("TAKEOFF") + + self.wait_ready_to_arm() + self.arm_vehicle() + + # Throw the catapult. + self.set_servo(7, 2000) + + # Check whether we're at max throttle below TKOFF_LVL_ALT. + test_alt = self.get_parameter("TKOFF_LVL_ALT")-10 + self.wait_altitude(test_alt, test_alt+2, relative=True) + self.assert_servo_channel_value(3, 1000+10*self.get_parameter("TKOFF_THR_MAX")) + + # Check whether we're still at max throttle past TKOFF_LVL_ALT. + test_alt = self.get_parameter("TKOFF_LVL_ALT")+10 + self.wait_altitude(test_alt, test_alt+2, relative=True) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX")), operator.le) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX"))-1, operator.ge) + + # Wait for the takeoff to complete. + target_alt = self.get_parameter("TKOFF_ALT") + self.wait_altitude(target_alt-5, target_alt, relative=True) + + self.fly_home_land_and_disarm() + + def TakeoffTakeoff2(self): + '''Test the behaviour of a takeoff in TAKEOFF mode, pt2. + + Conditions: + - ARSPD_USE=1 + - TKOFF_MODE=1 + - TKOFF_THR_MAX < THR_MAX + ''' + + self.customise_SITL_commandline( + [], + model='plane-catapult', + defaults_filepath=self.model_defaults_filepath("plane") + ) + self.set_parameters({ + "ARSPD_USE": 1.0, + "THR_MAX": 100.0, + "TKOFF_LVL_ALT": 80.0, + "TKOFF_ALT": 150.0, + "TKOFF_MODE": 1.0, + "TKOFF_THR_MINACC": 3.0, + "TKOFF_THR_MAX": 80.0, + "TECS_PITCH_MAX": 35.0, + "PTCH_LIM_MAX_DEG": 35.0, + }) + self.change_mode("TAKEOFF") + + self.wait_ready_to_arm() + self.arm_vehicle() + + # Throw the catapult. + self.set_servo(7, 2000) + + # Check whether we're at max throttle below TKOFF_LVL_ALT. + test_alt = self.get_parameter("TKOFF_LVL_ALT")-10 + self.wait_altitude(test_alt, test_alt+2, relative=True) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX")), operator.le) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX"))-1, operator.ge) + + # Check whether we've receded from max throttle past TKOFF_LVL_ALT. + test_alt = self.get_parameter("TKOFF_LVL_ALT")+10 + self.wait_altitude(test_alt, test_alt+2, relative=True) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX")), operator.le) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MIN"))-1, operator.ge) + + # Wait for the takeoff to complete. + target_alt = self.get_parameter("TKOFF_ALT") + self.wait_altitude(target_alt-5, target_alt, relative=True) + + self.fly_home_land_and_disarm() + + def TakeoffTakeoff3(self): + '''Test the behaviour of a takeoff in TAKEOFF mode, pt3. + + This is the same as case #1, but with disabled airspeed sensor. + + Conditions: + - ARSPD_USE=0 + - TKOFF_MODE=0 + - TKOFF_THR_MAX < THR_MAX + ''' + + self.customise_SITL_commandline( + [], + model='plane-catapult', + defaults_filepath=self.model_defaults_filepath("plane") + ) + self.set_parameters({ + "ARSPD_USE": 0.0, + "THR_MAX": 100.0, + "TKOFF_LVL_ALT": 30.0, + "TKOFF_ALT": 100.0, + "TKOFF_MODE": 0.0, + "TKOFF_THR_MINACC": 3.0, + "TKOFF_THR_MAX": 80.0, + "TECS_PITCH_MAX": 35.0, + "PTCH_LIM_MAX_DEG": 35.0, + }) + self.change_mode("TAKEOFF") + + self.wait_ready_to_arm() + self.arm_vehicle() + + # Throw the catapult. + self.set_servo(7, 2000) + + # Check whether we're at max throttle below TKOFF_LVL_ALT. + test_alt = self.get_parameter("TKOFF_LVL_ALT")-10 + self.wait_altitude(test_alt, test_alt+2, relative=True) + self.assert_servo_channel_value(3, 1000+10*self.get_parameter("TKOFF_THR_MAX")) + + # Check whether we're still at max throttle past TKOFF_LVL_ALT. + test_alt = self.get_parameter("TKOFF_LVL_ALT")+10 + self.wait_altitude(test_alt, test_alt+2, relative=True) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX")), operator.le) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX"))-1, operator.ge) + + # Wait for the takeoff to complete. + target_alt = self.get_parameter("TKOFF_ALT") + self.wait_altitude(target_alt-5, target_alt, relative=True) + + self.fly_home_land_and_disarm() + def DCMFallback(self): '''Really annoy the EKF and force fallback''' self.reboot_sitl() @@ -5712,6 +6055,13 @@ class AutoTestPlane(vehicle_test_suite.TestSuite): self.AHRS_ORIENTATION, self.AHRSTrim, self.LandingDrift, + self.TakeoffAuto1, + self.TakeoffAuto2, + self.TakeoffAuto3, + self.TakeoffAuto4, + self.TakeoffTakeoff1, + self.TakeoffTakeoff2, + self.TakeoffTakeoff3, self.ForcedDCM, self.DCMFallback, self.MAVFTP, diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index f3458fee9d..19b9f946d9 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -5571,6 +5571,10 @@ class TestSuite(ABC): """Setup a simulated RC control to a PWM value""" self.set_rc_from_map({chan: pwm}, timeout=timeout) + def set_servo(self, chan, pwm): + """Replicate the functionality of MAVProxy: servo set """ + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, p1=chan, p2=pwm) + def location_offset_ne(self, location, north, east): '''move location in metres''' print("old: %f %f" % (location.lat, location.lng))