Tools: autotest: Added tests for checking POSITION_TARGET messages for Copter

Tools: autotest: Added timeout, mavutil const in test_guided_local_target()

Tools: autotest: Corrected mesage checking in test_guided_local_target()

Tools: autotest: Fixed comments in test_guided_local_target()

Tools: autotest: Added test for checking whether correct local target is
received by Copter

Tools: autotest: added error tolerance, divided check into xyz components

Tools: autotest: Added test for correct local velocity target for Copter

Tools: autotest: Added test for checking if POSITION_TARGET_LOCAL messages are sent only in Guided Mode for Copter
This commit is contained in:
Rajat Singhal 2019-03-31 06:27:32 +05:30 committed by Peter Barker
parent 08b48142c8
commit a87d388a6c

View File

@ -2138,6 +2138,112 @@ class AutoTestCopter(AutoTest):
if z_achieved - z_up > 1:
raise NotAchievedException("Did not achieve z position: want=%f got=%f" % (z_up, z_achieved))
def test_guided_local_position_target(self, x, y, z_up):
""" Check target position being received by vehicle """
# set POSITION_TARGET_LOCAL_NED message rate using SET_MESSAGE_INTERVAL
self.progress("Setting local target in NED: (%f, %f, %f)" % (x, y, -z_up))
self.progress("Setting rate to 1 Hz")
self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, 1)
# set position target
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 xyz
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='POSITION_TARGET_LOCAL_NED', blocking=True, timeout=2)
self.progress("Received local target: %s" % str(m))
if not (m.type_mask == 0xFFF8 or m.type_mask == 0x0FF8):
raise NotAchievedException("Did not receive proper mask: expected=65528 or 4088, got=%u" % m.type_mask)
if x - m.x > 0.1:
raise NotAchievedException("Did not receive proper target position x: wanted=%f got=%f" % (x, m.x))
if y - m.y > 0.1:
raise NotAchievedException("Did not receive proper target position y: wanted=%f got=%f" % (y, m.y))
if z_up - (-m.z) > 0.1:
raise NotAchievedException("Did not receive proper target position z: wanted=%f got=%f" % (z_up, -m.z))
def test_guided_local_velocity_target(self, vx, vy, vz_up, timeout=3):
" Check local target velocity being recieved by vehicle "
self.progress("Setting local NED velocity target: (%f, %f, %f)" %(vx, vy, -vz_up))
self.progress("Setting POSITION_TARGET_LOCAL_NED message rate to 10Hz")
self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, 10)
# Drain old messages and ignore the ramp-up to the required target velocity
tstart = self.get_sim_time()
while self.get_sim_time_cached() - tstart < timeout:
# send velocity-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,
0b1111111111000111, # mask specifying use only vx,vy,vz
0, # x
0, # y
0, # z
vx, # vx
vy, # vy
-vz_up, # vz
0, # afx
0, # afy
0, # afz
0, # yaw
0, # yawrate
)
m = self.mav.recv_match(type='POSITION_TARGET_LOCAL_NED', blocking=True, timeout=1)
if m is None:
raise NotAchievedException("Did not receive any message for 1 sec")
self.progress("Received local target: %s" % str(m))
# Check the last received message
if not (m.type_mask == 0xFFC7 or m.type_mask == 0x0FC7):
raise NotAchievedException("Did not receive proper mask: expected=65479 or 4039, got=%u" % m.type_mask)
if vx - m.vx > 0.1:
raise NotAchievedException("Did not receive proper target velocity vx: wanted=%f got=%f" % (vx, m.vx))
if vy - m.vy > 0.1:
raise NotAchievedException("Did not receive proper target velocity vy: wanted=%f got=%f" % (vy, m.vy))
if vz_up - (-m.vz) > 0.1:
raise NotAchievedException("Did not receive proper target velocity vz: wanted=%f got=%f" % (vz_up, -m.vz))
self.progress("Received proper target velocity commands")
def test_position_target_message_mode(self):
" Ensure that POSITION_TARGET_LOCAL_NED messages are sent in Guided Mode only "
self.change_mode('LOITER')
self.progress("Setting POSITION_TARGET_LOCAL_NED message rate to 10Hz")
self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, 10)
tstart = self.get_sim_time()
while self.get_sim_time_cached() < tstart + 5:
m = self.mav.recv_match(type='POSITION_TARGET_LOCAL_NED', blocking=True, timeout=1)
if m is None:
continue
raise NotAchievedException("Received POSITION_TARGET message in LOITER mode: %s" % str(m))
self.progress("Did not receive any POSITION_TARGET_LOCAL_NED message in LOITER mode. Success")
def earth_to_body(self, vector):
m = self.mav.messages["ATTITUDE"]
x = rotmat.Vector3(m.roll, m.pitch, m.yaw)
@ -2262,6 +2368,11 @@ class AutoTestCopter(AutoTest):
self.fly_guided_stop()
self.fly_guided_move_local(5, 5, 10)
""" Check target position received by vehicle using SET_MESSAGE_INTERVAL """
self.test_guided_local_position_target(5, 5, 10)
self.test_guided_local_velocity_target(2, 2, 1)
self.test_position_target_message_mode()
self.do_RTL()
def test_gripper_mission(self):