From ada3537b8f414327a0da3fb115346716502c1f1f Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 3 Jul 2022 15:46:32 +1000 Subject: [PATCH] Tools: Add autotest for pitot tube blockage handling --- Tools/autotest/arduplane.py | 29 +++++++++++++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index ef02ebe3f2..f55629fdff 100644 --- a/Tools/autotest/arduplane.py +++ b/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,