From e30f0a9173f018c56ce1cd0d341a7913123193de Mon Sep 17 00:00:00 2001 From: Samuel Tabor Date: Sat, 11 Apr 2020 11:18:20 +0100 Subject: [PATCH] autotest: Move soaring test into plane. --- .../ArduPlane_Tests/Soaring/CMAC-soar.txt | 9 + Tools/autotest/arduplane.py | 222 ++++++++---------- Tools/autotest/autotest.py | 3 - Tools/autotest/common.py | 5 +- 4 files changed, 114 insertions(+), 125 deletions(-) create mode 100644 Tools/autotest/ArduPlane_Tests/Soaring/CMAC-soar.txt diff --git a/Tools/autotest/ArduPlane_Tests/Soaring/CMAC-soar.txt b/Tools/autotest/ArduPlane_Tests/Soaring/CMAC-soar.txt new file mode 100644 index 0000000000..fd18d731d0 --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/Soaring/CMAC-soar.txt @@ -0,0 +1,9 @@ +QGC WPL 110 +0 1 0 16 0.000000 0.000000 0.000000 0.000000 -35.362938 149.165085 584.409973 1 +1 0 3 22 15.000000 0.000000 0.000000 0.000000 -35.361164 149.163986 28.110001 1 +2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.359467 149.161697 400.000000 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.366333 149.162659 400.000000 1 +4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.366131 149.164581 400.000000 1 +5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.359272 149.163757 400.000000 1 +6 0 3 177 2.000000 -1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 +7 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.359272 149.163757 400.000000 1 diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 39a05b23c1..f7bfc818e2 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -1642,6 +1642,106 @@ class AutoTestPlane(AutoTest): self.load_mission("Kingaroy-vlarge.txt") self.load_mission("Kingaroy-vlarge2.txt") + def fly_soaring(self): + + self.customise_SITL_commandline([], + model="plane-soaring", + defaults_filepath="default_params/plane.parm") + + self.set_parameter("SOAR_ENABLE", 1) + self.repeatedly_apply_parameter_file(os.path.join(testdir, 'default_params/plane-soaring.parm')) + self.load_mission('CMAC-soar.txt') + + + self.mavproxy.send("wp set 1\n") + self.change_mode('AUTO') + self.wait_ready_to_arm() + self.arm_vehicle() + + # Enable thermalling RC + rc_chan = self.get_parameter('SOAR_ENABLE_CH') + self.send_set_rc(rc_chan, 1900) + + # Wait to detect thermal + self.progress("Waiting for thermal") + self.wait_mode('LOITER',timeout=600) + + # Wait to climb to SOAR_ALT_MAX + self.progress("Waiting for climb to max altitude") + alt_max = self.get_parameter('SOAR_ALT_MAX') + self.wait_altitude(alt_max-10, alt_max, timeout=600, relative=True) + + # Wait for AUTO + self.progress("Waiting for AUTO mode") + self.wait_mode('AUTO') + + # Disable thermals + self.set_parameter("SIM_THML_SCENARI", 0) + + + # Wait to descent to SOAR_ALT_MIN + self.progress("Waiting for glide to min altitude") + alt_min = self.get_parameter('SOAR_ALT_MIN') + self.wait_altitude(alt_min-10, alt_min, timeout=600, relative=True) + + self.progress("Waiting for throttle up") + self.wait_servo_channel_value(3, 1200, timeout=2, comparator=operator.gt) + + self.progress("Waiting for climb to cutoff altitude") + alt_ctf = self.get_parameter('SOAR_ALT_CUTOFF') + self.wait_altitude(alt_ctf-10, alt_ctf, timeout=600, relative=True) + + # Now set FBWB mode + self.change_mode('FBWB') + self.delay_sim_time(5) + + # Now disable soaring (should hold altitude) + self.set_parameter("SOAR_ENABLE", 0) + self.delay_sim_time(10) + + #And reenable. This should force throttle-down + self.set_parameter("SOAR_ENABLE", 1) + self.delay_sim_time(10) + + # Now wait for descent and check RTL + self.wait_altitude(alt_min-10, alt_min, timeout=600, relative=True) + + self.progress("Waiting for RTL") + self.wait_mode('RTL') + + alt_rtl = self.get_parameter('ALT_HOLD_RTL')/100 + + # Wait for climb to RTL. + self.progress("Waiting for climb to RTL altitude") + self.wait_altitude(alt_rtl-5, alt_rtl+5, timeout=60, relative=True) + + # Back to auto + self.change_mode('AUTO') + + # Reenable thermals + self.set_parameter("SIM_THML_SCENARI", 1) + + # Disable soaring using RC channel. + self.send_set_rc(rc_chan, 1100) + + # Wait to get back to waypoint before thermal. + self.progress("Waiting to get back to position") + self.wait_current_waypoint(3,timeout=1200) + + # Enable soaring with mode changes suppressed) + self.send_set_rc(rc_chan, 1500) + + # Make sure this causes throttle down. + self.wait_servo_channel_value(3, 1200, timeout=2, comparator=operator.lt) + + self.progress("Waiting for next WP with no loiter") + self.wait_waypoint(4,4,timeout=1200,max_dist=120) + + # Disarm + self.disarm_vehicle() + + self.progress("Mission OK") + def tests(self): '''return list of all tests''' ret = super(AutoTestPlane, self).tests() @@ -1747,6 +1847,10 @@ class AutoTestPlane(AutoTest): "Test Manipulation of Large missions", self.test_large_missions), + ("Soaring", + "Test Soaring feature", + self.fly_soaring), + ("LogDownLoad", "Log download", lambda: self.log_download( @@ -1755,121 +1859,3 @@ class AutoTestPlane(AutoTest): upload_logs=True)) ]) return ret - -class AutoTestSoaring(AutoTestPlane): - - def log_name(self): - return "Soaring" - - def default_frame(self): - return "plane-soaring" - - def defaults_filepath(self): - return os.path.join(testdir, 'default_params/plane.parm') - - def fly_mission(self): - - self.set_parameter("SOAR_ENABLE", 1) - self.repeatedly_apply_parameter_file(os.path.join(testdir, 'default_params/plane.parm')) - self.load_mission("CMAC-soar.txt") - - - self.mavproxy.send("wp set 1\n") - self.change_mode('AUTO') - self.wait_ready_to_arm() - self.arm_vehicle() - - # Enable thermalling RC - rc_chan = self.get_parameter('SOAR_ENABLE_CH') - self.send_set_rc(rc_chan, 1900) - - # Wait to detect thermal - self.progress("Waiting for thermal") - self.wait_mode('LOITER',timeout=600) - - # Wait to climb to SOAR_ALT_MAX - self.progress("Waiting for climb to max altitude") - alt_max = self.get_parameter('SOAR_ALT_MAX') - self.wait_altitude(alt_max-10, alt_max, timeout=600, relative=True) - - # Wait for AUTO - self.progress("Waiting for AUTO mode") - self.wait_mode('AUTO') - - # Disable thermals - self.set_parameter("SIM_THML_SCENARI", 0) - - - # Wait to descent to SOAR_ALT_MIN - self.progress("Waiting for glide to min altitude") - alt_min = self.get_parameter('SOAR_ALT_MIN') - self.wait_altitude(alt_min-10, alt_min, timeout=600, relative=True) - - self.progress("Waiting for throttle up") - self.wait_servo_channel_value(3, 1200, timeout=2, comparator=operator.gt) - - self.progress("Waiting for climb to cutoff altitude") - alt_ctf = self.get_parameter('SOAR_ALT_CUTOFF') - self.wait_altitude(alt_ctf-10, alt_ctf, timeout=600, relative=True) - - # Now set FBWB mode - self.change_mode('FBWB') - self.delay_sim_time(5) - - # Now disable soaring (should hold altitude) - self.set_parameter("SOAR_ENABLE", 0) - self.delay_sim_time(10) - - #And reenable. This should force throttle-down - self.set_parameter("SOAR_ENABLE", 1) - self.delay_sim_time(10) - - # Now wait for descent and check RTL - self.wait_altitude(alt_min-10, alt_min, timeout=600, relative=True) - - self.progress("Waiting for RTL") - self.wait_mode('RTL') - - alt_rtl = self.get_parameter('ALT_HOLD_RTL')/100 - - # Wait for climb to RTL. - self.progress("Waiting for climb to RTL altitude") - self.wait_altitude(alt_rtl-5, alt_rtl+5, timeout=60, relative=True) - - # Back to auto - self.change_mode('AUTO') - - # Reenable thermals - self.set_parameter("SIM_THML_SCENARI", 1) - - # Disable soaring using RC channel. - self.send_set_rc(rc_chan, 1100) - - # Wait to get back to waypoint before thermal. - self.progress("Waiting to get back to position") - self.wait_current_waypoint(3,timeout=1200) - - # Enable soaring with mode changes suppressed) - self.send_set_rc(rc_chan, 1500) - - # Make sure this causes throttle down. - self.wait_servo_channel_value(3, 1200, timeout=2, comparator=operator.lt) - - self.progress("Waiting for next WP with no loiter") - self.wait_waypoint(4,4,timeout=1200,max_dist=120) - - # Disarm - self.disarm_vehicle() - - self.progress("Mission OK") - - def tests(self): - '''return list of all tests''' - ret = AutoTest.tests(self) - - '''return list of all tests''' - ret.extend([ - ("Mission", "Soaring mission", - self.fly_mission) - ]) - return ret diff --git a/Tools/autotest/autotest.py b/Tools/autotest/autotest.py index 8c8bd8998f..800e171be0 100755 --- a/Tools/autotest/autotest.py +++ b/Tools/autotest/autotest.py @@ -252,7 +252,6 @@ __bin_names = { "QuadPlane": "arduplane", "Sub": "ardusub", "BalanceBot": "ardurover", - "Soaring": "arduplane", } @@ -305,7 +304,6 @@ tester_class_map = { "test.Helicopter": arducopter.AutoTestHeli, "test.Sub": ardusub.AutoTestSub, "test.Tracker": antennatracker.AutoTestTracker, - "test.Soaring": arduplane.AutoTestSoaring, } def run_specific_test(step, *args, **kwargs): @@ -778,7 +776,6 @@ if __name__ == "__main__": 'defaults.Plane', 'test.Plane', 'test.QuadPlane', - 'test.Soaring', 'build.Rover', 'defaults.Rover', diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 0cb695b217..1cb4e0b86d 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -1011,7 +1011,7 @@ class AutoTest(ABC): vehicle = self.log_name() if vehicle == "HeliCopter": vehicle = "ArduCopter" - if vehicle == "QuadPlane" or vehicle =="Soaring": + if vehicle == "QuadPlane": vehicle = "ArduPlane" cmd = [param_parse_filepath, '--vehicle', vehicle] # cmd.append("--verbose") @@ -1074,8 +1074,6 @@ class AutoTest(ABC): dirname = "ArduPlane" elif dirname == "HeliCopter": dirname = "ArduCopter" - elif dirname == "Soaring": - dirname = "ArduPlane" return os.path.join(self.rootdir(), dirname) def all_log_format_ids(self): @@ -1298,7 +1296,6 @@ class AutoTest(ABC): "HeliCopter": "Copter", "ArduPlane": "Plane", "QuadPlane": "Plane", - "Soaring": "Plane", "Rover": "Rover", "AntennaTracker": "Tracker", "ArduSub": "Sub",