Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

autotest: Add test for mission using zero-turn loiters and skipping waypoint location

Browse Source
This commit is contained in:
James O'Shannessy 2024-11-14 11:09:09 +11:00
parent 81d1945347
commit d2afc05221
1 changed files with 60 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
Tools/autotest/arduplane.py
View File

@ -6317,6 +6317,65 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
self.wait_current_waypoint(4)
self.fly_home_land_and_disarm()
def MAV_CMD_NAV_LOITER_TURNS_zero_turn(self):
'''Ensure air vehicle achieves loiter target before exiting'''
offset = 500
alt = self.get_parameter("RTL_ALTITUDE")
waypoint_radius = 100
loiter_turns_loc_ccw = self.home_relative_loc_ne(offset, offset)
loiter_turns_loc_cw = self.home_relative_loc_ne(offset, -offset)
# upload a mission plan containing zero-turn loiters
self.upload_simple_relhome_mission([
(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 10),
self.create_MISSION_ITEM_INT(
mavutil.mavlink.MAV_CMD_NAV_LOITER_TURNS,
p1=0, # Turns
p3=-waypoint_radius, # Radius (If positive loiter clockwise, else counter-clockwise)
x=int(loiter_turns_loc_ccw.lat*1e7),
y=int(loiter_turns_loc_ccw.lng*1e7),
z=alt,
frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT
),
self.create_MISSION_ITEM_INT(
mavutil.mavlink.MAV_CMD_NAV_LOITER_TURNS,
p1=0, # Turns
p3=waypoint_radius, # Radius (If positive loiter clockwise, else counter-clockwise)
x=int(loiter_turns_loc_cw.lat*1e7),
y=int(loiter_turns_loc_cw.lng*1e7),
z=alt,
frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT
),
self.create_MISSION_ITEM_INT(
mavutil.mavlink.MAV_CMD_DO_JUMP,
p1=2, # waypoint to jump to
p2=1 # number of jumps (-1: infinite)
),
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, -offset, -offset, alt),
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, -offset, offset, alt),
])
self.change_mode("AUTO")
self.wait_ready_to_arm()
self.arm_vehicle()
# check the vehicle is flying to each waypoint as expected
self.wait_distance_to_waypoint(2, distance_min=90, distance_max=110, timeout=90) # North East Loiter
self.wait_distance_to_waypoint(3, distance_min=90, distance_max=110, timeout=90) # North West Loiter
# Jump to first 0-Turn Loiter
self.wait_distance_to_waypoint(2, distance_min=90, distance_max=110, timeout=90) # North East Loiter
# Ensure we go back through this loiter point
self.wait_distance_to_waypoint(3, distance_min=90, distance_max=110, timeout=90) # North West Loiter
self.wait_distance_to_waypoint(5, distance_min=10, distance_max=20, timeout=90) # South West Waypoint
self.wait_distance_to_waypoint(6, distance_min=10, distance_max=20, timeout=90) # South East Waypoint
self.fly_home_land_and_disarm()
def tests(self):
'''return list of all tests'''
ret = []
@ -6466,6 +6525,7 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
self.MAV_CMD_EXTERNAL_WIND_ESTIMATE,
self.GliderPullup,
self.BadRollChannelDefined,
self.MAV_CMD_NAV_LOITER_TURNS_zero_turn,
]
def disabled_tests(self):