Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

autotest: Improved takeoff tests 

- Also added a ground rolling takeoff test.
- Rebased conflict resolution originating from https://github.com/ArduPilot/ardupilot/pull/28030
This commit is contained in:
George Zogopoulos 2024-08-06 12:45:27 +02:00 committed by Andrew Tridgell
parent 6ce6ef8fff
commit 36991de2b8
2 changed files with 92 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

106
Tools/autotest/arduplane.py
View File

@ -4397,11 +4397,13 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
"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.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.
@ -4412,7 +4414,8 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
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)
@ -4422,7 +4425,7 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
'''Test the behaviour of an AUTO takeoff, pt2.'''
'''
Conditions:
- ARSPD_USE=1
- ARSPD_USE=0
- TKOFF_OPTIONS[0]=0
- TKOFF_THR_MAX > THR_MAX
'''
@ -4442,11 +4445,13 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
"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.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.
@ -4457,7 +4462,8 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
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)
@ -4489,11 +4495,13 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
"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.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.
@ -4501,7 +4509,8 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
# 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
@ -4545,11 +4554,12 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
"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.
@ -4557,14 +4567,14 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
# 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)
@ -4588,7 +4598,7 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
"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,
@ -4606,18 +4616,22 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
# 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):
@ -4637,8 +4651,8 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
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,
@ -4656,19 +4670,23 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
# 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):
@ -4690,6 +4708,7 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
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,
@ -4717,7 +4736,7 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
self,
3, # throttle
expected_takeoff_throttle,
epsilon=1,
epsilon=10,
minimum_duration=1,
)
w.run()
@ -4730,7 +4749,7 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
self,
3, # throttle
expected_takeoff_throttle,
epsilon=1,
epsilon=10,
minimum_duration=1,
)
w.run()
@ -4766,19 +4785,49 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
# 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):
@ -6326,6 +6375,7 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
self.TakeoffTakeoff2,
self.TakeoffTakeoff3,
self.TakeoffTakeoff4,
self.TakeoffGround,
self.ForcedDCM,
self.DCMFallback,
self.MAVFTP,

14
Tools/autotest/vehicle_test_suite.py
View File

@ -7977,6 +7977,20 @@ class TestSuite(ABC):
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"""
channel_field = "chan%u_raw" % channel