diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index a6d6f18f8c6db7..468aec6119fe35 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -1876,6 +1876,22 @@ def GPSGlitchAuto(self, timeout=180): self.show_gps_and_sim_positions(False) raise e + # stop and test loss of GPS for a short time - it should resume GPS use without falling back into a non aiding mode + self.change_mode("LOITER") + self.set_parameters({ + "SIM_GPS_DISABLE": 1, + }) + self.delay_sim_time(2) + self.set_parameters({ + "SIM_GPS_DISABLE": 0, + }) + # regaining GPS should not result in it falling back to a non-navigation mode + self.wait_ekf_flags(mavutil.mavlink.ESTIMATOR_POS_HORIZ_ABS, 0, timeout=1) + # It should still be navigating after enougnh time has passed for any pending timeouts to activate. + self.delay_sim_time(10) + self.wait_ekf_flags(mavutil.mavlink.ESTIMATOR_POS_HORIZ_ABS, 0, timeout=1) + self.change_mode("AUTO") + # record time and position tstart = self.get_sim_time() diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 1bf046d5f2e8a2..395a8936bd1edb 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -2139,8 +2139,26 @@ def validate_global_position_int_against_simstate(mav, m): self.delay_sim_time(20) self.change_mode("RTL") self.wait_distance_to_home(100, 200, timeout=200) + # go into LOITER to create additonal time for a GPS re-enable test + self.change_mode("LOITER") self.set_parameter("SIM_GPS_DISABLE", 0) + t_enabled = self.get_sim_time() + # The EKF should wait for GPS checks to pass when we are still able to navigate using dead reckoning + # to prevent bad GPS being used when coming back after loss of lock due to interence. + self.wait_ekf_flags(mavutil.mavlink.ESTIMATOR_POS_HORIZ_ABS, 0, timeout=15) + if self.get_sim_time() < (t_enabled+9): + raise NotAchievedException("GPS use re-started too quickly") + # wait for EKF and vehicle position to stabilise, then test response to jamming + self.delay_sim_time(20) + self.set_parameter("SIM_GPS_JAM", 1) self.delay_sim_time(10) + self.set_parameter("SIM_GPS_JAM", 0) + t_enabled = self.get_sim_time() + # The EKF should wait for GPS checks to pass when we are still able to navigate using dead reckoning + # to prevent bad GPS being used when coming back after loss of lock due to interence. + self.wait_ekf_flags(mavutil.mavlink.ESTIMATOR_POS_HORIZ_ABS, 0, timeout=15) + if self.get_sim_time() < (t_enabled+9): + raise NotAchievedException("GPS use re-started too quickly after jamming") self.set_rc(3, 1000) self.fly_home_land_and_disarm() self.progress("max-divergence: %fm" % (self.max_divergence,))