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.
  • Loading branch information
@Georacer
Georacer committed Aug 7, 2024
1 parent 5e82c16 commit 9883163
Show file tree
Hide file tree
Showing 2 changed files with 97 additions and 30 deletions.
113 changes: 83 additions & 30 deletions Tools/autotest/arduplane.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -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)
Expand All @@ -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
'''
Expand All @@ -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.
Expand All @@ -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)
Expand Down Expand Up @@ -4487,24 +4491,28 @@ 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.
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
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)
Expand Down Expand Up @@ -4536,26 +4544,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 @@ -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,
Expand All @@ -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):
Expand All @@ -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,
Expand All @@ -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):
Expand All @@ -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,
Expand All @@ -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):
Expand Down Expand Up @@ -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):
Expand Down Expand Up @@ -6138,6 +6190,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 @@ -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"""
Expand Down

0 comments on commit 9883163

Please sign in to comment.