Tools: Add autotest for pitot tube blockage handling

Tools/autotest/arduplane.py

@@ -1723,6 +1723,34 @@ class AutoTestPlane(AutoTest):
         self.run_subtest("Mission test",
                          lambda: self.fly_mission("ap1.txt", strict=False))
 
+    def PitotBlockage(self):
+        '''Test detection and isolation of a blocked pitot tube'''
+        self.set_parameter("ARSPD_OPTIONS",15)
+        self.set_parameter("ARSPD_USE",1)
+        self.set_parameter("SIM_WIND_SPD",10)
+        self.set_parameter("SIM_WIND_DIR",0)
+        self.set_parameter("ARSPD_WIND_MAX",15)
+        self.wait_ready_to_arm()
+        self.change_mode("TAKEOFF")
+        self.arm_vehicle()
+        self.delay_sim_time(60)
+        # simulate the effect of a blocked pitot tube
+        self.set_parameter("ARSPD_RATIO",0.1)
+        self.delay_sim_time(10)
+        if (self.get_parameter("ARSPD_USE") == 0):
+            self.progress("Faulty Sensor Disabled")
+        else:
+            raise NotAchievedException("Airspeed Sensor Not Disabled")
+        self.delay_sim_time(20)
+        # simulate the effect of blockage clearing
+        self.set_parameter("ARSPD_RATIO",2.0)
+        self.delay_sim_time(60)
+        if (self.get_parameter("ARSPD_USE") == 1):
+            self.progress("Sensor Re-Enabled")
+        else:
+            raise NotAchievedException("Airspeed Sensor Not Re-Enabled")
+        self.disarm_vehicle(force=True)
+
     def AIRSPEED_AUTOCAL(self):
         '''Test AIRSPEED_AUTOCAL'''
         self.progress("Ensure no AIRSPEED_AUTOCAL on ground")
@@ -3927,6 +3955,7 @@ class AutoTestPlane(AutoTest):
             self.TestGripperMission,
             self.Parachute,
             self.ParachuteSinkRate,
+            self.PitotBlockage,
             self.AIRSPEED_AUTOCAL,
             self.RangeFinder,
             self.FenceStatic,