mirror of https://github.com/ArduPilot/ardupilot
Tools: autotest: add test for moving sub in guided mode
This commit is contained in:
parent
6fcdfbbe32
commit
6d914f8af3
|
@ -164,6 +164,50 @@ class AutoTestSub(AutoTest):
|
||||||
if ex is not None:
|
if ex is not None:
|
||||||
raise ex
|
raise ex
|
||||||
|
|
||||||
|
def dive_set_position_target(self):
|
||||||
|
self.change_mode('GUIDED')
|
||||||
|
self.wait_ready_to_arm()
|
||||||
|
self.arm_vehicle()
|
||||||
|
|
||||||
|
startpos = self.mav.recv_match(type='GLOBAL_POSITION_INT',
|
||||||
|
blocking=True)
|
||||||
|
|
||||||
|
lat = 5
|
||||||
|
lon = 5
|
||||||
|
alt = 10
|
||||||
|
|
||||||
|
tstart = self.get_sim_time()
|
||||||
|
while True:
|
||||||
|
if self.get_sim_time() - tstart > 200:
|
||||||
|
raise NotAchievedException("Did not move far enough")
|
||||||
|
# send a position-control command
|
||||||
|
self.mav.mav.set_position_target_global_int_send(
|
||||||
|
0, # timestamp
|
||||||
|
1, # target system_id
|
||||||
|
1, # target component id
|
||||||
|
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
|
||||||
|
0b1111111111111000, # mask specifying use-only-lat-lon-alt
|
||||||
|
lat, # lat
|
||||||
|
lon, # lon
|
||||||
|
alt, # alt
|
||||||
|
0, # vx
|
||||||
|
0, # vy
|
||||||
|
0, # vz
|
||||||
|
0, # afx
|
||||||
|
0, # afy
|
||||||
|
0, # afz
|
||||||
|
0, # yaw
|
||||||
|
0, # yawrate
|
||||||
|
)
|
||||||
|
pos = self.mav.recv_match(type='GLOBAL_POSITION_INT',
|
||||||
|
blocking=True)
|
||||||
|
delta = self.get_distance_int(startpos, pos)
|
||||||
|
self.progress("delta=%f (want >10)" % delta)
|
||||||
|
if delta > 10:
|
||||||
|
break
|
||||||
|
self.change_mode('MANUAL')
|
||||||
|
self.disarm_vehicle()
|
||||||
|
|
||||||
def reboot_sitl(self):
|
def reboot_sitl(self):
|
||||||
"""Reboot SITL instance and wait it to reconnect."""
|
"""Reboot SITL instance and wait it to reconnect."""
|
||||||
self.mavproxy.send("reboot\n")
|
self.mavproxy.send("reboot\n")
|
||||||
|
@ -190,6 +234,10 @@ class AutoTestSub(AutoTest):
|
||||||
"Test gripper mission items",
|
"Test gripper mission items",
|
||||||
self.test_gripper_mission),
|
self.test_gripper_mission),
|
||||||
|
|
||||||
|
("SET_POSITION_TARGET_GLOBAL_INT",
|
||||||
|
"Move vehicle using SET_POSITION_TARGET_GLOBAL_INT",
|
||||||
|
self.dive_set_position_target),
|
||||||
|
|
||||||
("DownLoadLogs", "Download logs", lambda:
|
("DownLoadLogs", "Download logs", lambda:
|
||||||
self.log_download(
|
self.log_download(
|
||||||
self.buildlogs_path("APMrover2-log.bin"),
|
self.buildlogs_path("APMrover2-log.bin"),
|
||||||
|
|
Loading…
Reference in New Issue