Tools: autotest: add test for Rover offboard-in-auto function

Tools/autotest/apmrover2.py

@@ -891,6 +891,63 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
         self.wait_location(loc, accuracy=accuracy)
         self.disarm_vehicle()
 
+    def test_offboard(self, timeout=90):
+        self.load_mission("rover-guided-mission.txt")
+        self.wait_ready_to_arm(require_absolute=True)
+        self.arm_vehicle()
+        self.change_mode("AUTO")
+
+        offboard_expected_duration = 10 # see mission file
+
+        if self.mav.messages.get("SET_POSITION_TARGET_GLOBAL_INT", None):
+            raise PreconditionFailedException("Already have SET_POSITION_TARGET_GLOBAL_INT")
+
+        tstart = self.get_sim_time_cached()
+        last_heartbeat_sent = 0
+        got_sptgi = False
+        magic_waypoint_tstart = 0
+        magic_waypoint_tstop = 0
+        while True:
+            if self.mode_is("HOLD", cached=True):
+                break
+
+            now = self.get_sim_time_cached()
+            if now - last_heartbeat_sent > 1:
+                last_heartbeat_sent = now
+                self.mav.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_ONBOARD_CONTROLLER,
+                                            mavutil.mavlink.MAV_AUTOPILOT_INVALID,
+                                            0,
+                                            0,
+                                            0)
+
+            if now - tstart > timeout:
+                raise AutoTestTimeoutException("Didn't complete")
+            magic_waypoint = 3
+#            mc = self.mav.messages.get("MISSION_CURRENT", None)
+            mc = self.mav.recv_match(type="MISSION_CURRENT", blocking=False)
+            if mc is not None:
+                print("%s" % str(mc))
+                if mc.seq == magic_waypoint:
+                    print("At magic waypoint")
+                    if magic_waypoint_tstart == 0:
+                        magic_waypoint_tstart = self.get_sim_time_cached()
+                    sptgi = self.mav.messages.get("SET_POSITION_TARGET_GLOBAL_INT", None)
+                    if sptgi is not None:
+                        got_sptgi = True
+                elif mc.seq > magic_waypoint:
+                    if magic_waypoint_tstop == 0:
+                        magic_waypoint_tstop = self.get_sim_time_cached()
+
+        self.disarm_vehicle()
+        offboard_duration = magic_waypoint_tstop - magic_waypoint_tstart
+        if abs(offboard_duration - offboard_expected_duration) > 1:
+            raise NotAchievedException("Did not stay in offboard control for correct time (want=%f got=%f)" %
+                                       (offboard_expected_duration, offboard_duration))
+
+        if not got_sptgi:
+            raise NotAchievedException("Did not get sptgi message")
+        print("spgti: %s" % str(sptgi))
+
     def tests(self):
         '''return list of all tests'''
         ret = super(AutoTestRover, self).tests()
@@ -979,6 +1036,10 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
              "Test Rally Points",
              self.test_rally_points),
 
+            ("Offboard",
+             "Test Offboard Control",
+             self.test_offboard),
+
             ("DataFlashOverMAVLink",
              "Test DataFlash over MAVLink",
              self.test_dataflash_over_mavlink),

Tools/autotest/common.py

@@ -1574,8 +1574,9 @@ class AutoTest(ABC):
         raise WaitWaypointTimeout("Timed out waiting for waypoint %u of %u" %
                                   (wpnum_end, wpnum_end))
 
-    def mode_is(self, mode):
-        self.wait_heartbeat()
+    def mode_is(self, mode, cached=False):
+        if not cached:
+            self.wait_heartbeat()
         return self.get_mode_from_mode_mapping(self.mav.flightmode) == self.get_mode_from_mode_mapping(mode)
 
     def wait_mode(self, mode, timeout=60):

Tools/autotest/rover-guided-mission.txt

@@ -0,0 +1,7 @@
+QGC WPL 110
+0	0	0	16	0.000000	0.000000	0.000000	0.000000	40.0713750	-105.229789	0	1
+1	0	3	16	0.000000	0.000000	0.000000	0.000000	40.0713350	-105.229779	0.000000	1
+2	0	0	222	10.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	1
+3	0	0	92	1.000000	0.000000	0.000000	0.000000	39.000000	104.000000	0.000000	1
+4	0	3	16	0.000000	0.000000	0.000000	0.000000	40.0713550	-105.229769	0.000000	1
+5	0	0	20	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	1