diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 4f386413d3..2dc4a23918 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -2738,7 +2738,10 @@ class AutoTestPlane(AutoTest): attempt_fence_breached_disable(start_mode="FBWA", end_mode="FBWA", expected_mode="GUIDED", action=6) attempt_fence_breached_disable(start_mode="FBWA", end_mode="FBWA", expected_mode="GUIDED", action=7) - def run_auxfunc(self, function, level): + def run_auxfunc(self, + function, + level, + want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): self.run_cmd( mavutil.mavlink.MAV_CMD_DO_AUX_FUNCTION, function, # p1 @@ -2748,6 +2751,7 @@ class AutoTestPlane(AutoTest): 0, # p5 0, # p6 0, # p7 + want_result=want_result ) def fly_aux_function(self): @@ -2758,6 +2762,20 @@ class AutoTestPlane(AutoTest): self.wait_statustext("RevThrottle: DISABLE", check_context=True) self.run_auxfunc(65, 2) # 65 == GPS_DISABLE + self.start_subtest("Bad auxfunc") + self.run_auxfunc( + 65231, + 2, + want_result=mavutil.mavlink.MAV_RESULT_FAILED + ) + + self.start_subtest("Bad switchpos") + self.run_auxfunc( + 62, + 17, + want_result=mavutil.mavlink.MAV_RESULT_DENIED + ) + def tests(self): '''return list of all tests''' ret = super(AutoTestPlane, self).tests()