From 9883163a188cf9d4585bfb77962ae83652f68749 Mon Sep 17 00:00:00 2001 From: George Zogopoulos Date: Tue, 6 Aug 2024 12:45:27 +0200 Subject: [PATCH] autotest: Improved takeoff tests Also added a ground rolling takeoff test. --- Tools/autotest/arduplane.py | 113 ++++++++++++++++++++------- Tools/autotest/vehicle_test_suite.py | 14 ++++ 2 files changed, 97 insertions(+), 30 deletions(-) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 3ca7780e5ff453..2beacc2801e0f4 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -4395,11 +4395,12 @@ def TakeoffAuto1(self): "RTL_AUTOLAND": 2, # The mission contains a DO_LAND_START item. }) + self.wait_ready_to_arm() + # 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. @@ -4410,7 +4411,8 @@ def TakeoffAuto1(self): 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")) + target_throttle = 1000+10*(self.get_parameter("TKOFF_THR_MAX")) + self.assert_servo_channel_range(3, target_throttle-10, target_throttle+10) # Wait for landing waypoint. self.wait_current_waypoint(11, timeout=1200) @@ -4420,7 +4422,7 @@ def TakeoffAuto2(self): '''Test the behaviour of an AUTO takeoff, pt2. Conditions: - - ARSPD_USE=1 + - ARSPD_USE=0 - TKOFF_OPTIONS[0]=0 - TKOFF_THR_MAX > THR_MAX ''' @@ -4440,11 +4442,12 @@ def TakeoffAuto2(self): "RTL_AUTOLAND": 2, # The mission contains a DO_LAND_START item. }) + self.wait_ready_to_arm() + # 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. @@ -4455,7 +4458,8 @@ def TakeoffAuto2(self): 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")) + target_throttle = 1000+10*(self.get_parameter("TKOFF_THR_MAX")) + self.assert_servo_channel_range(3, target_throttle-10, target_throttle+10) # Wait for landing waypoint. self.wait_current_waypoint(11, timeout=1200) @@ -4487,11 +4491,12 @@ def TakeoffAuto3(self): "RTL_AUTOLAND": 2, # The mission contains a DO_LAND_START item. }) + self.wait_ready_to_arm() + # 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. @@ -4499,12 +4504,15 @@ def TakeoffAuto3(self): # 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)) + target_throttle = 1000+10*(self.get_parameter("TKOFF_THR_MAX")) + self.assert_servo_channel_range(3, target_throttle-10, target_throttle+10) # 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) + max_throttle = 1000+10*(self.get_parameter("TKOFF_THR_MAX")) + min_throttle = 1000+10*(self.get_parameter("THR_MIN")) + self.assert_servo_channel_range(3, min_throttle, max_throttle-1) # Wait for landing waypoint. self.wait_current_waypoint(11, timeout=1200) @@ -4536,11 +4544,12 @@ def TakeoffAuto4(self): "RTL_AUTOLAND": 2, # The mission contains a DO_LAND_START item. }) + self.wait_ready_to_arm() + # 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. @@ -4548,14 +4557,14 @@ def TakeoffAuto4(self): # 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) + target_throttle = 1000+10*(self.get_parameter("TKOFF_THR_MAX")) + self.assert_servo_channel_range(3, target_throttle-10, target_throttle+10) # 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) + target_throttle = 1000+10*(self.get_parameter("TKOFF_THR_MAX")) + self.assert_servo_channel_range(3, target_throttle-10, target_throttle+10) # Wait for landing waypoint. self.wait_current_waypoint(11, timeout=1200) @@ -4579,7 +4588,7 @@ def TakeoffTakeoff1(self): "ARSPD_USE": 1.0, "THR_MAX": 100.0, "TKOFF_LVL_ALT": 30.0, - "TKOFF_ALT": 100.0, + "TKOFF_ALT": 80.0, "TKOFF_OPTIONS": 0.0, "TKOFF_THR_MINACC": 3.0, "TKOFF_THR_MAX": 80.0, @@ -4597,18 +4606,22 @@ def TakeoffTakeoff1(self): # 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")) + target_throttle = 1000+10*(self.get_parameter("TKOFF_THR_MAX")) + self.assert_servo_channel_range(3, target_throttle-10, target_throttle+10) # 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) + target_throttle = 1000+10*(self.get_parameter("TKOFF_THR_MAX")) + self.assert_servo_channel_range(3, target_throttle-10, target_throttle+10) # Wait for the takeoff to complete. target_alt = self.get_parameter("TKOFF_ALT") self.wait_altitude(target_alt-5, target_alt, relative=True) + # Wait a bit for the Takeoff altitude to settle. + self.delay_sim_time(5) + self.fly_home_land_and_disarm() def TakeoffTakeoff2(self): @@ -4628,8 +4641,8 @@ def TakeoffTakeoff2(self): self.set_parameters({ "ARSPD_USE": 1.0, "THR_MAX": 100.0, - "TKOFF_LVL_ALT": 80.0, - "TKOFF_ALT": 150.0, + "TKOFF_LVL_ALT": 30.0, + "TKOFF_ALT": 80.0, "TKOFF_OPTIONS": 1.0, "TKOFF_THR_MINACC": 3.0, "TKOFF_THR_MAX": 80.0, @@ -4647,19 +4660,23 @@ def TakeoffTakeoff2(self): # 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) + target_throttle = 1000+10*(self.get_parameter("TKOFF_THR_MAX")) + self.assert_servo_channel_range(3, target_throttle-10, target_throttle+10) # 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) + thr_min = 1000+10*(self.get_parameter("TKOFF_THR_MIN"))-1 + thr_max = 1000+10*(self.get_parameter("TKOFF_THR_MAX"))-10 + self.assert_servo_channel_range(3, thr_min, thr_max) # Wait for the takeoff to complete. target_alt = self.get_parameter("TKOFF_ALT") self.wait_altitude(target_alt-5, target_alt, relative=True) + # Wait a bit for the Takeoff altitude to settle. + self.delay_sim_time(5) + self.fly_home_land_and_disarm() def TakeoffTakeoff3(self): @@ -4681,6 +4698,7 @@ def TakeoffTakeoff3(self): self.set_parameters({ "ARSPD_USE": 0.0, "THR_MAX": 100.0, + "TKOFF_DIST": 400.0, "TKOFF_LVL_ALT": 30.0, "TKOFF_ALT": 100.0, "TKOFF_OPTIONS": 0.0, @@ -4700,18 +4718,22 @@ def TakeoffTakeoff3(self): # 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")) + target_throttle = 1000+10*(self.get_parameter("TKOFF_THR_MAX")) + self.assert_servo_channel_range(3, target_throttle-10, target_throttle+10) # 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) + target_throttle = 1000+10*(self.get_parameter("TKOFF_THR_MAX")) + self.assert_servo_channel_range(3, target_throttle-10, target_throttle+10) # Wait for the takeoff to complete. target_alt = self.get_parameter("TKOFF_ALT") self.wait_altitude(target_alt-5, target_alt, relative=True) + # Wait a bit for the Takeoff altitude to settle. + self.delay_sim_time(5) + self.fly_home_land_and_disarm() def TakeoffTakeoff4(self): @@ -4739,19 +4761,49 @@ def TakeoffTakeoff4(self): # 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("THR_MAX")), operator.le) - self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("THR_MAX"))-10, operator.ge) + target_throttle = 1000+10*(self.get_parameter("THR_MAX")) + self.assert_servo_channel_range(3, target_throttle-10, target_throttle+10) # 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("THR_MAX")), operator.le) - self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("THR_MAX"))-10, operator.ge) + target_throttle = 1000+10*(self.get_parameter("THR_MAX")) + self.assert_servo_channel_range(3, target_throttle-10, target_throttle+10) # Wait for the takeoff to complete. target_alt = self.get_parameter("TKOFF_ALT") self.wait_altitude(target_alt-5, target_alt, relative=True) + # Wait a bit for the Takeoff altitude to settle. + self.delay_sim_time(5) + + self.fly_home_land_and_disarm() + + def TakeoffGround(self): + '''Test a rolling TAKEOFF.''' + + self.set_parameters({ + "TKOFF_ROTATE_SPD": 15.0, + }) + self.change_mode("TAKEOFF") + + self.wait_ready_to_arm() + self.arm_vehicle() + + # Check that we demand minimum pitch below rotation speed. + self.wait_groundspeed(8, 10) + m = self.assert_receive_message('NAV_CONTROLLER_OUTPUT', timeout=5) + nav_pitch = m.nav_pitch + if nav_pitch > 5.1 or nav_pitch < 4.9: + raise NotAchievedException(f"Did not achieve correct takeoff pitch ({nav_pitch}).") + + # Check whether we've achieved correct target pitch after rotation. + self.wait_groundspeed(23, 24) + m = self.assert_receive_message('NAV_CONTROLLER_OUTPUT', timeout=5) + nav_pitch = m.nav_pitch + if nav_pitch > 15.1 or nav_pitch < 14.9: + raise NotAchievedException(f"Did not achieve correct takeoff pitch ({nav_pitch}).") + self.fly_home_land_and_disarm() def DCMFallback(self): @@ -6138,6 +6190,7 @@ def tests(self): self.TakeoffTakeoff2, self.TakeoffTakeoff3, self.TakeoffTakeoff4, + self.TakeoffGround, self.ForcedDCM, self.DCMFallback, self.MAVFTP, diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index f4cdceaf44412b..dfb383a9c1d2aa 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -7983,6 +7983,20 @@ def assert_servo_channel_value(self, channel, value, comparator=operator.eq): if comparator(m_value, value): return m_value raise NotAchievedException("Wrong value") + + def assert_servo_channel_range(self, channel, value_min, value_max): + """assert channel value is within the range [value_min, value_max]""" + channel_field = "servo%u_raw" % channel + m = self.assert_receive_message('SERVO_OUTPUT_RAW', timeout=1) + m_value = getattr(m, channel_field, None) + if m_value is None: + raise ValueError("message (%s) has no field %s" % + (str(m), channel_field)) + self.progress("assert SERVO_OUTPUT_RAW.%s=%u in [%u, %u]" % + (channel_field, m_value, value_min, value_max)) + if m_value >= value_min and m_value <= value_max: + return m_value + raise NotAchievedException("Wrong value") def get_rc_channel_value(self, channel, timeout=2): """wait for channel to hit value"""