Tools: add test.Copter.GPSViconSwitching to CopterTests1d

Tools/autotest/arducopter.py

@@ -2238,6 +2238,84 @@ class AutoTestCopter(AutoTest):
         if ex is not None:
             raise ex
 
+    def fly_gps_vicon_switching(self):
+        """Fly GPS and Vicon switching test"""
+        self.customise_SITL_commandline(["--uartF=sim:vicon:"])
+
+        """Setup parameters including switching to EKF3"""
+        self.context_push()
+        ex = None
+        try:
+            self.set_parameter("VISO_TYPE", 2)      # enable vicon
+            self.set_parameter("SERIAL5_PROTOCOL", 2)
+            self.set_parameter("EK3_ENABLE", 1)
+            self.set_parameter("EK3_SRC2_POSXY", 6) # External Nav
+            self.set_parameter("EK3_SRC2_POSZ", 6)  # External Nav
+            self.set_parameter("EK3_SRC2_VELXY", 6) # External Nav
+            self.set_parameter("EK3_SRC2_VELZ", 6)  # External Nav
+            self.set_parameter("EK3_SRC2_YAW", 2)   # External Nav
+            self.set_parameter("RC7_OPTION", 80)    # RC aux switch 7 set to Viso Align
+            self.set_parameter("RC8_OPTION", 90)    # RC aux switch 8 set to EKF source selector
+            self.reboot_sitl()
+            self.set_parameter("EK2_ENABLE", 0)
+            self.set_parameter("AHRS_EKF_TYPE", 3)
+
+            # switch to use GPS
+            self.set_rc(8, 1000)
+
+            # require_absolute=True infers a GPS is present
+            self.wait_ready_to_arm(require_absolute=True)
+
+            # record starting position
+            old_pos = self.get_global_position_int()
+            print("old_pos=%s" % str(old_pos))
+
+            # align vicon yaw with ahrs heading
+            self.set_rc(7, 2000)
+
+            # takeoff to 10m in Loiter
+            self.progress("Moving to ensure location is tracked")
+            self.takeoff(10, mode="LOITER")
+
+            # fly forward in Loiter
+            self.set_rc(2, 1300)
+
+            # disable vicon
+            self.set_parameter("SIM_VICON_FAIL", 1)
+
+            # ensure vehicle remain in Loiter for 15 seconds
+            tstart = self.get_sim_time();
+            while self.get_sim_time() - tstart < 15:
+                if not self.mode_is('LOITER'):
+                    raise NotAchievedException("Expected to stay in loiter for >15 seconds")
+
+            # re-enable vicon
+            self.set_parameter("SIM_VICON_FAIL", 0)
+
+            # switch to vicon, disable GPS and wait 10sec to ensure vehicle remains in Loiter
+            self.set_rc(8, 1500)
+            self.set_parameter("GPS_TYPE", 0)
+
+            # ensure vehicle remain in Loiter for 15 seconds
+            tstart = self.get_sim_time();
+            while self.get_sim_time() - tstart < 15:
+                if not self.mode_is('LOITER'):
+                    raise NotAchievedException("Expected to stay in loiter for >15 seconds")
+
+            # RTL and check vehicle arrives within 10m of home
+            self.set_rc(2, 1500)
+            self.do_RTL()
+
+        except Exception as e:
+            self.progress("Caught exception: %s" %
+                          self.get_exception_stacktrace(e))
+            ex = e
+        self.context_pop()
+        self.disarm_vehicle(force=True)
+        self.reboot_sitl()
+        if ex is not None:
+            raise ex
+
     def fly_rtl_speed(self):
         """Test RTL Speed parameters"""
         rtl_speed_ms = 7
@@ -5552,6 +5630,10 @@ class AutoTestCopter(AutoTest):
             ("VisionPosition",
              "Fly Vision Position",
              self.fly_vision_position), #24s
+
+            ("GPSViconSwitching",
+             "Fly GPS and Vicon Switching",
+             self.fly_gps_vicon_switching),
         ])
         return ret