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

Tools: autotest: add test for MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED

Browse Source
This commit is contained in:
Peter Barker 2019-02-22 16:29:13 +11:00 committed by Peter Barker
parent a48f461158
commit 4d0ae8c371
1 changed files with 90 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

90
Tools/autotest/arducopter.py
View File

@ -2063,6 +2063,41 @@ class AutoTestCopter(AutoTest):
if ex is not None:
raise ex
def fly_guided_stop(self,
timeout=20,
groundspeed_tolerance=0.05,
climb_tolerance=0.001):
"""stop the vehicle moving in guided mode"""
self.progress("Stopping vehicle")
tstart = self.get_sim_time()
while True:
if self.get_sim_time_cached() - tstart > timeout:
raise NotAchievedException("Vehicle did not stop")
# send a position-control command
self.mav.mav.set_position_target_local_ned_send(
0, # timestamp
1, # target system_id
1, # target component id
mavutil.mavlink.MAV_FRAME_BODY_NED,
0b1111111111111000, # mask specifying use-only-x-y-z
0, # x
0, # y
0, # z
0, # vx
0, # vy
0, # vz
0, # afx
0, # afy
0, # afz
0, # yaw
0, # yawrate
)
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
print("%s" % str(m))
if (m.groundspeed < groundspeed_tolerance and
m.climb < climb_tolerance):
break
def fly_guided_move_relative(self, lat, lon, alt):
startpos = self.mav.recv_match(type='GLOBAL_POSITION_INT',
blocking=True)
@ -2097,6 +2132,57 @@ class AutoTestCopter(AutoTest):
if delta > 10:
break
def fly_guided_move_local(self, x, y, z_up, timeout=100):
"""move the vehicle using MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED"""
startpos = self.mav.recv_match(type='LOCAL_POSITION_NED', blocking=True)
self.progress("startpos=%s" % str(startpos))
tstart = self.get_sim_time()
while True:
if self.get_sim_time() - tstart > timeout:
raise NotAchievedException("Did not start to move")
# send a position-control command
self.mav.mav.set_position_target_local_ned_send(
0, # timestamp
1, # target system_id
1, # target component id
mavutil.mavlink.MAV_FRAME_LOCAL_NED,
0b1111111111111000, # mask specifying use-only-x-y-z
x, # x
y, # y
-z_up,# z
0, # vx
0, # vy
0, # vz
0, # afx
0, # afy
0, # afz
0, # yaw
0, # yawrate
)
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
print("%s" % m)
if m.groundspeed > 0.5:
break
self.progress("Waiting for vehicle to stop...")
self.wait_groundspeed(1, 100, timeout=timeout)
stoppos = self.mav.recv_match(type='LOCAL_POSITION_NED', blocking=True)
self.progress("stop_pos=%s" % str(stoppos))
x_achieved = stoppos.x - startpos.x
if x_achieved - x > 1:
raise NotAchievedException("Did not achieve x position: want=%f got=%f" % (x, x_achieved))
y_achieved = stoppos.y - startpos.y
if y_achieved - y > 1:
raise NotAchievedException("Did not achieve y position: want=%f got=%f" % (y, y_achieved))
z_achieved = stoppos.z - startpos.z
if z_achieved - z_up > 1:
raise NotAchievedException("Did not achieve z position: want=%f got=%f" % (z_up, z_achieved))
def earth_to_body(self, vector):
m = self.mav.messages["ATTITUDE"]
x = rotmat.Vector3(m.roll, m.pitch, m.yaw)
@ -2218,6 +2304,10 @@ class AutoTestCopter(AutoTest):
"""move the vehicle using set_position_target_global_int"""
self.fly_guided_move_relative(5, 5, 10)
"""move the vehicle using MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED"""
self.fly_guided_stop()
self.fly_guided_move_local(5, 5, 10)
self.progress("Landing")
self.mavproxy.send('switch 2\n') # land mode
self.wait_mode('LAND')