From a87d388a6c633ee35f8355d592e602610f9db757 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Sun, 31 Mar 2019 06:27:32 +0530 Subject: [PATCH] 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 --- Tools/autotest/arducopter.py | 111 +++++++++++++++++++++++++++++++++++ 1 file changed, 111 insertions(+) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 74a8baf149..d278b9721b 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -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):