Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

autotest: add a drive_to_location for Rover

This commit is contained in:
Peter Barker 2022-09-18 18:51:40 +10:00 committed by Peter Barker
parent 8ad7d00a35
commit d4587e787e
1 changed files with 42 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
Tools/autotest/rover.py
View File

@ -3642,6 +3642,48 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
if delta < max_delta:
self.progress("Reached destination")
def drive_to_location(self, loc, tolerance=1, timeout=30, target_system=1, target_component=1):
self.assert_mode('GUIDED')
type_mask = (mavutil.mavlink.POSITION_TARGET_TYPEMASK_VX_IGNORE +
mavutil.mavlink.POSITION_TARGET_TYPEMASK_VY_IGNORE +
mavutil.mavlink.POSITION_TARGET_TYPEMASK_VZ_IGNORE +
mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE +
mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE +
mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE +
mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE +
mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE)
last_sent = 0
tstart = self.get_sim_time()
while True:
now = self.get_sim_time_cached()
if now - tstart > timeout:
raise NotAchievedException("Did not get to location")
if now - last_sent > 10:
last_sent = now
self.mav.mav.set_position_target_global_int_send(
0,
target_system,
target_component,
mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
type_mask,
int(loc.lat * 1.0e7),
int(loc.lng * 1.0e7),
0, # alt
0, # x-ve
0, # y-vel
0, # z-vel
0, # afx
0, # afy
0, # afz
0, # yaw,
0, # yaw-rate
)
if self.get_distance(self.mav.location(), loc) > tolerance:
continue
return
def drive_somewhere_breach_boundary_and_rtl(self, loc, target_system=1, target_component=1, timeout=60):
tstart = self.get_sim_time()
last_sent = 0