Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-21 16:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

autotest: add test for GuidedRequest handling in Plane

Browse Source
This commit is contained in:
Peter Barker 2019-11-11 11:20:03 +11:00 committed by Peter Barker
parent 7c034990b5
commit f8d10ebc0f
1 changed files with 66 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
Tools/autotest/arduplane.py
View File

@ -1403,6 +1403,68 @@ class AutoTestPlane(AutoTest):
if m.threat_level != 2:
raise NotAchievedException("Expected some threat at least")
def fly_do_guided_request(self, target_system=1, target_component=1):
self.progress("Takeoff")
self.takeoff(alt=50)
self.set_rc(3, 1500)
self.start_subtest("Ensure command bounced outside guided mode")
desired_relative_alt = 33
loc = self.mav.location()
self.location_offset_ne(loc, 300, 300)
loc.alt += desired_relative_alt
self.mav.mav.mission_item_int_send(
target_system,
target_component,
0, # seq
mavutil.mavlink.MAV_FRAME_GLOBAL,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
2, # current - guided-mode request
0, # autocontinue
0, # p1
0, # p2
0, # p3
0, # p4
int(loc.lat *1e7), # latitude
int(loc.lng *1e7), # longitude
loc.alt, # altitude
mavutil.mavlink.MAV_MISSION_TYPE_MISSION)
m = self.mav.recv_match(type='MISSION_ACK', blocking=True, timeout=5)
if m is None:
raise NotAchievedException("Did not get MISSION_ACK")
if m.type != mavutil.mavlink.MAV_MISSION_ERROR:
raise NotAchievedException("Did not get appropriate error")
self.start_subtest("Enter guided and flying somewhere constant")
self.change_mode("GUIDED")
self.mav.mav.mission_item_int_send(
target_system,
target_component,
0, # seq
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
2, # current - guided-mode request
0, # autocontinue
0, # p1
0, # p2
0, # p3
0, # p4
int(loc.lat *1e7), # latitude
int(loc.lng *1e7), # longitude
desired_relative_alt, # altitude
mavutil.mavlink.MAV_MISSION_TYPE_MISSION)
m = self.mav.recv_match(type='MISSION_ACK', blocking=True, timeout=5)
if m is None:
raise NotAchievedException("Did not get MISSION_ACK")
if m.type != mavutil.mavlink.MAV_MISSION_ACCEPTED:
raise NotAchievedException("Did not get accepted response")
self.wait_location(loc, accuracy=100) # based on loiter radius
self.delay_sim_time(20)
self.wait_altitude(alt_min=desired_relative_alt-3,
alt_max=desired_relative_alt+3,
relative=True)
self.fly_home_land_and_disarm()
def tests(self):
'''return list of all tests'''
ret = super(AutoTestPlane, self).tests()
@ -1430,6 +1492,10 @@ class AutoTestPlane(AutoTest):
"Test mavlink DO_REPOSITION command",
self.fly_do_reposition),
("GuidedRequest",
"Test handling of MISSION_ITEM in guided mode",
self.fly_do_guided_request),
("MainFlight",
"Lots of things in one flight",
self.test_main_flight),