From d5e282c5a133ea0c58a44da5de786cb8ae7538be Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 10 May 2022 07:53:10 -0500 Subject: [PATCH] Tools: allow FBWB alt control option in LOITER --- Tools/autotest/arduplane.py | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 7bb074126c..1202602821 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -2215,6 +2215,8 @@ function''' self.fly_home_land_and_disarm() def LOITER(self): + # first test old loiter behavour + self.set_parameter("FLIGHT_OPTIONS", 0) self.takeoff(alt=200) self.set_rc(3, 1500) self.change_mode("LOITER") @@ -2260,6 +2262,20 @@ function''' self.progress("Centering elevator and ensuring we get back to loiter altitude") self.set_rc(2, 1500) self.wait_altitude(initial_alt-1, initial_alt+1) + # Test new loiter behavour + self.set_parameter("FLIGHT_OPTIONS", 1 << 12) + # should decend at max stick + self.set_rc(2, int(rc2_max)) + self.wait_altitude(initial_alt - 110, initial_alt - 90, timeout=90) + # should not climb back at mid stick + self.set_rc(2, 1500) + self.delay_sim_time(60) + self.wait_altitude(initial_alt - 110, initial_alt - 90) + # should climb at min stick + self.set_rc(2, 1100) + self.wait_altitude(initial_alt - 10, initial_alt + 10, timeout=90) + # return stick to center and fly home + self.set_rc(2, 1500) self.fly_home_land_and_disarm() def CPUFailsafe(self):