Tools: autotest: add test for MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED

Tools/autotest/arducopter.py

@@ -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')