Skip to content

Commit

Permalink
autotest: Improved takeoff tests
Browse files Browse the repository at this point in the history 
- Also added a ground rolling takeoff test.
- Rebased conflict resolution originating from ArduPilot#28030
  • Loading branch information
@Georacer @tridge
Georacer authored and tridge committed Sep 25, 2024
1 parent 308b6a3 commit 3494cd9
Show file tree
Hide file tree
Showing 2 changed files with 92 additions and 28 deletions.
106 changes: 78 additions & 28 deletions Tools/autotest/arduplane.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4395,11 +4395,13 @@ def TakeoffAuto1(self):
"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.
Expand All @@ -4410,7 +4412,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)
Expand All @@ -4420,7 +4423,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
'''
Expand All @@ -4440,11 +4443,13 @@ def TakeoffAuto2(self):
"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.
Expand All @@ -4455,7 +4460,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)
Expand Down Expand Up @@ -4487,19 +4493,22 @@ def TakeoffAuto3(self):
"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.
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))
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
Expand Down Expand Up @@ -4543,26 +4552,27 @@ 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.
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)
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)
Expand All @@ -4586,7 +4596,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,
Expand All @@ -4604,18 +4614,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):
Expand All @@ -4635,8 +4649,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,
Expand All @@ -4654,19 +4668,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):
Expand All @@ -4688,6 +4706,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,
Expand Down Expand Up @@ -4715,7 +4734,7 @@ def TakeoffTakeoff3(self):
self,
3, # throttle
expected_takeoff_throttle,
epsilon=1,
epsilon=10,
minimum_duration=1,
)
w.run()
Expand All @@ -4728,7 +4747,7 @@ def TakeoffTakeoff3(self):
self,
3, # throttle
expected_takeoff_throttle,
epsilon=1,
epsilon=10,
minimum_duration=1,
)
w.run()
Expand Down Expand Up @@ -4764,19 +4783,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):
Expand Down Expand Up @@ -6292,6 +6341,7 @@ def tests(self):
self.TakeoffTakeoff2,
self.TakeoffTakeoff3,
self.TakeoffTakeoff4,
self.TakeoffGround,
self.ForcedDCM,
self.DCMFallback,
self.MAVFTP,
Expand Down
14 changes: 14 additions & 0 deletions Tools/autotest/vehicle_test_suite.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7974,6 +7974,20 @@ def assert_servo_channel_value(self, channel, value, comparator=operator.eq):
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
Expand Down

0 comments on commit 3494cd9

Please sign in to comment.