From 7168a651550a1ad56520d776f79b6dd5ad1c174f Mon Sep 17 00:00:00 2001 From: Samuel Tabor Date: Fri, 27 Dec 2019 12:13:35 +0000 Subject: [PATCH] Autotest: Simplify soaring.py. --- Tools/autotest/soaring.py | 158 ++++++++++++++++++-------------------- 1 file changed, 73 insertions(+), 85 deletions(-) diff --git a/Tools/autotest/soaring.py b/Tools/autotest/soaring.py index 64e6a92ab1..4b8b5b666a 100644 --- a/Tools/autotest/soaring.py +++ b/Tools/autotest/soaring.py @@ -62,108 +62,99 @@ class AutoTestSoaring(AutoTest): def set_autodisarm_delay(self, delay): self.set_parameter("LAND_DISARMDELAY", delay) - def fly_mission(self, filename, fence, height_accuracy=-1): + def fly_mission(self): - self.context_push() - ex = None - try: - self.set_parameter("SOAR_ENABLE", 1) - self.repeatedly_apply_parameter_file('default_params/plane-soaring.parm') - self.load_mission("ArduPlane-Missions/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) + self.set_parameter("SOAR_ENABLE", 1) + self.repeatedly_apply_parameter_file('default_params/plane-soaring.parm') + self.load_mission(MISSION) - # 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.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) - self.progress("Waiting for throttle up") - self.wait_servo_channel_value(3, 1200, timeout=2, comparator=operator.gt) + # Wait to detect thermal + self.progress("Waiting for thermal") + self.wait_mode('LOITER',timeout=600) - 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) + # 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) - # Now set FBWB mode - self.change_mode('FBWB') - self.wait_seconds(5) + # Wait for AUTO + self.progress("Waiting for AUTO mode") + self.wait_mode('AUTO') - # Now disable soaring (should hold altitude) - self.set_parameter("SOAR_ENABLE", 0) - self.wait_seconds(10) + # Disable thermals + self.set_parameter("SIM_THML_SCENARI", 0) - #And reenable. This should force throttle-down - self.set_parameter("SOAR_ENABLE", 1) - self.wait_seconds(10) - # Now wait for descent and check RTL - self.wait_altitude(alt_min-10, alt_min, timeout=600, relative=True) + # 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 RTL") - self.wait_mode('RTL') + self.progress("Waiting for throttle up") + self.wait_servo_channel_value(3, 1200, timeout=2, comparator=operator.gt) - alt_rtl = self.get_parameter('ALT_HOLD_RTL')/100 + 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) - # 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) + # Now set FBWB mode + self.change_mode('FBWB') + self.wait_seconds(5) - # Back to auto - self.change_mode('AUTO') + # Now disable soaring (should hold altitude) + self.set_parameter("SOAR_ENABLE", 0) + self.wait_seconds(10) - # Reenable thermals - self.set_parameter("SIM_THML_SCENARI", 1) + #And reenable. This should force throttle-down + self.set_parameter("SOAR_ENABLE", 1) + self.wait_seconds(10) - # Disable soaring using RC channel. - self.send_set_rc(rc_chan, 1100) + # Now wait for descent and check RTL + self.wait_altitude(alt_min-10, alt_min, timeout=600, relative=True) - # Wait to get back to waypoint before thermal. - self.progress("Waiting to get back to position") - self.wait_current_waypoint(3,timeout=1200) + self.progress("Waiting for RTL") + self.wait_mode('RTL') - # Enable soaring with mode changes suppressed) - self.send_set_rc(rc_chan, 1500) + alt_rtl = self.get_parameter('ALT_HOLD_RTL')/100 - # Make sure this causes throttle down. - self.wait_servo_channel_value(3, 1200, timeout=2, comparator=operator.lt) + # 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) - self.progress("Waiting for next WP with no loiter") - self.wait_waypoint(4,4,timeout=1200,max_dist=120) + # Back to auto + self.change_mode('AUTO') - # Disarm - self.disarm_vehicle() + # Reenable thermals + self.set_parameter("SIM_THML_SCENARI", 1) - except Exception as e: - self.progress("Exception caught") - ex = e - self.context_pop() - if ex is not None: - raise ex + # 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") @@ -184,13 +175,10 @@ class AutoTestSoaring(AutoTest): def tests(self): '''return list of all tests''' - m = os.path.join(testdir, "ArduPlane-Missions/CMAC-soar.txt") - f = os.path.join(testdir, - "ArduPlane-Missions/Dalby-OBC2016-fence.txt") ret = super(AutoTestSoaring, self).tests() ret.extend([ ("Mission", "CMAC Mission", - lambda: self.fly_mission(m, f)) + self.fly_mission) ]) return ret