Skip to content
Open
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
91 changes: 91 additions & 0 deletions Tools/autotest/arducopter.py
Original file line number Diff line number Diff line change
Expand Up @@ -5855,6 +5855,96 @@ def CANGPSCopterMission(self):

self.CopterMission()

def AutoTakeoffWhileMoving(self):
"""Check horizontal motion during an airborne AUTO takeoff."""
self.context_set_message_rate_hz('LOCAL_POSITION_NED', 20)
self.set_parameter('MIS_RESTART', 1)
self.upload_simple_relhome_mission([
(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 50),
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 100, 0, 50),
])

self.takeoff(altitude_min=10, altitude_max=20, mode='LOITER')
self.set_rc(2, 1000)
self.wait_groundspeed(10, 12, timeout=30, minimum_duration=1)

motion = self.assert_receive_message('LOCAL_POSITION_NED')
speed = (motion.vx ** 2 + motion.vy ** 2) ** 0.5
self.progress(f'Initial horizontal speed: {speed:.2f} m/s')
north, east = motion.vx / speed, motion.vy / speed
samples = []
in_auto = False
takeoff_started = False
takeoff_finished = False

def collect_position(mav, message):
nonlocal in_auto, takeoff_finished
if message.get_srcSystem() != 1:
return
if message.get_type() == 'HEARTBEAT':
in_auto = mavutil.mode_string_v10(message) == 'AUTO'
elif message.get_type() == 'MISSION_CURRENT':
if takeoff_started and message.seq >= 2:
takeoff_finished = True
elif (in_auto and not takeoff_finished and
message.get_type() == 'LOCAL_POSITION_NED'):
samples.append((
message.x, message.y, message.vx, message.vy,
message.time_boot_ms,
))

self.install_message_hook_context(collect_position)
self.change_mode('AUTO')
self.set_rc(2, 1500)
self.wait_current_waypoint(1)
takeoff_started = True
self.wait_groundspeed(0, 0.6, timeout=10, minimum_duration=1)
self.assert_current_waypoint(1)
self.wait_current_waypoint(2, timeout=90)
if not takeoff_finished:
raise NotAchievedException('Takeoff completion was not observed')

recorded = list(samples)
if len(recorded) < 10:
raise NotAchievedException('Too few AUTO position samples')
if not in_auto:
raise NotAchievedException('Left AUTO during observation')

elapsed_ms = (recorded[-1][4] - recorded[0][4]) % (1 << 32)
max_gap_ms = max(
(later[4] - earlier[4]) % (1 << 32)
for earlier, later in zip(recorded, recorded[1:])
)
self.progress(
f'AUTO samples span {elapsed_ms} ms; max gap {max_gap_ms} ms'
)
if elapsed_ms < 7000 or max_gap_ms > 2000:
raise NotAchievedException(
f'Incomplete AUTO position data: {elapsed_ms} ms total, '
f'{max_gap_ms} ms maximum gap'
)

first_speed = recorded[0][2] * north + recorded[0][3] * east
if first_speed < 1:
raise NotAchievedException(
f'AUTO recording started too late: {first_speed:.2f} m/s'
)

furthest = recorded[0][0] * north + recorded[0][1] * east
max_return = 0.0
for x, y, *_ in recorded:
along = x * north + y * east
furthest = max(furthest, along)
max_return = max(max_return, furthest - along)

self.progress(f'Maximum return during takeoff: {max_return:.2f} m')
if max_return > 1.5:
raise NotAchievedException(
f'Copter moved back {max_return:.2f} m during AUTO takeoff'
)
self.disarm_vehicle(force=True)
self.reboot_sitl()

def TakeoffAlt(self):
'''Test Takeoff command altitude'''
# Test case #1 (set target altitude to relative -10m from the ground, -10m is invalid, so it is set to 1m)
Expand Down Expand Up @@ -20239,6 +20329,7 @@ def tests1d(self):
self.LoiterNoCompassYaw,
self.CopterMission,
self.TakeoffAlt,
self.AutoTakeoffWhileMoving,
self.AutoYawDO_MOUNT_CONTROL,
self.ParameterChecks,
self.ModeFollow_with_FOLLOW_TARGET,
Expand Down
Loading