Tools: autotest: add test for moving sub in guided mode

Tools/autotest/ardusub.py

@@ -164,6 +164,50 @@ class AutoTestSub(AutoTest):
         if ex is not None:
             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):
         """Reboot SITL instance and wait it to reconnect."""
         self.mavproxy.send("reboot\n")
@@ -190,6 +234,10 @@ class AutoTestSub(AutoTest):
              "Test gripper mission items",
              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:
              self.log_download(
                  self.buildlogs_path("APMrover2-log.bin"),