From 528dcc3f6bb0ad4b7d4dad6262ded32d94502ca8 Mon Sep 17 00:00:00 2001 From: Samuel Tabor Date: Mon, 27 May 2019 14:30:52 +0100 Subject: [PATCH] Autotest: Add test for soaring. --- Tools/autotest/autotest.py | 4 + .../default_params/plane-soaring.parm | 3 +- Tools/autotest/soaring.py | 196 ++++++++++++++++++ 3 files changed, 202 insertions(+), 1 deletion(-) create mode 100644 Tools/autotest/soaring.py diff --git a/Tools/autotest/autotest.py b/Tools/autotest/autotest.py index 99a281dd6e..e33c95d74c 100755 --- a/Tools/autotest/autotest.py +++ b/Tools/autotest/autotest.py @@ -24,6 +24,7 @@ import ardusub import antennatracker import quadplane import balancebot +import soaring import examples from pysim import util @@ -252,6 +253,7 @@ __bin_names = { "QuadPlane": "arduplane", "Sub": "ardusub", "BalanceBot": "ardurover", + "Soaring": "arduplane", } @@ -304,6 +306,7 @@ tester_class_map = { "test.Helicopter": arducopter.AutoTestHeli, "test.Sub": ardusub.AutoTestSub, "test.Tracker": antennatracker.AutoTestTracker, + "test.Soaring": soaring.AutoTestSoaring, } def run_specific_test(step, *args, **kwargs): @@ -776,6 +779,7 @@ if __name__ == "__main__": 'defaults.Plane', 'test.Plane', 'test.QuadPlane', + 'test.Soaring', 'build.Rover', 'defaults.Rover', diff --git a/Tools/autotest/default_params/plane-soaring.parm b/Tools/autotest/default_params/plane-soaring.parm index 436f53a75a..8211237ab5 100644 --- a/Tools/autotest/default_params/plane-soaring.parm +++ b/Tools/autotest/default_params/plane-soaring.parm @@ -11,7 +11,7 @@ SOAR_ALT_MAX 350.000000 SOAR_ALT_MIN 50.000000 SOAR_DIST_AHEAD 5.000000 SOAR_ENABLE 1.000000 -SOAR_ENABLE_CH 0.000000 +SOAR_ENABLE_CH 7.000000 SOAR_MIN_CRSE_S 10.000000 SOAR_MIN_THML_S 20.000000 SOAR_POLAR_B 0.045000 @@ -19,6 +19,7 @@ SOAR_POLAR_CD0 0.050000 SOAR_POLAR_K 71.10000 SOAR_Q1 0.001000 SOAR_Q2 0.100000 + SOAR_R 0.200000 SOAR_VSPEED 0.700000 diff --git a/Tools/autotest/soaring.py b/Tools/autotest/soaring.py new file mode 100644 index 0000000000..64e6a92ab1 --- /dev/null +++ b/Tools/autotest/soaring.py @@ -0,0 +1,196 @@ +#!/usr/bin/env python + +# Fly ArduSoar in SITL +from __future__ import print_function +import os +import pexpect +from pymavlink import mavutil + +from common import AutoTest +from common import AutoTestTimeoutException + +from pysim import util +from pysim import vehicleinfo +import operator + + +# get location of scripts +testdir = os.path.dirname(os.path.realpath(__file__)) +SITL_START_LOCATION = mavutil.location(-35.362938, 149.165085, 585, 354) +MISSION = 'ArduPlane-Missions/CMAC-soar.txt' +WIND = "0,180,0.2" # speed,direction,variance + + +class AutoTestSoaring(AutoTest): + + def default_frame(self): + return "plane-soaring" + + def test_filepath(self): + return os.path.realpath(__file__) + + def sitl_start_location(self): + return SITL_START_LOCATION + + def log_name(self): + return "Soaring" + + def apply_defaultfile_parameters(self): + # plane passes in a defaults_file in place of applying + # parameters afterwards. + pass + + def defaults_filepath(self): + vinfo = vehicleinfo.VehicleInfo() + defaults_file = vinfo.options["ArduPlane"]["frames"][self.frame]["default_params_filename"] + if isinstance(defaults_file, str): + defaults_file = [defaults_file] + defaults_list = [] + for d in defaults_file: + defaults_list.append(os.path.join(testdir, d)) + return ','.join(defaults_list) + + def is_plane(self): + return True + + def get_stick_arming_channel(self): + return int(self.get_parameter("RCMAP_YAW")) + + def get_disarm_delay(self): + return int(self.get_parameter("LAND_DISARMDELAY")) + + def set_autodisarm_delay(self, delay): + self.set_parameter("LAND_DISARMDELAY", delay) + + def fly_mission(self, filename, fence, height_accuracy=-1): + + 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) + + + # 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.wait_seconds(5) + + # Now disable soaring (should hold altitude) + self.set_parameter("SOAR_ENABLE", 0) + self.wait_seconds(10) + + #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) + + 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() + + except Exception as e: + self.progress("Exception caught") + ex = e + self.context_pop() + if ex is not None: + raise ex + + self.progress("Mission OK") + + def default_mode(self): + return "MANUAL" + + def test_pid_tuning(self): + # Do nothing, covered by fly.ArduPlane + self.progress("Skipped") + + def test_arm_feature(self): + # Do nothing, covered by fly.ArduPlane + self.progress("Skipped") + + def fly_test_set_home(self): + # Do nothing, covered by fly.ArduPlane + self.progress("Skipped") + + 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)) + ]) + return ret