autotest: Move soaring test into plane.

This commit is contained in:
Samuel Tabor 2020-04-11 11:18:20 +01:00 committed by Peter Barker
parent 52227872d2
commit e30f0a9173
4 changed files with 114 additions and 125 deletions

View File

@ -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

View File

@ -1642,6 +1642,106 @@ class AutoTestPlane(AutoTest):
self.load_mission("Kingaroy-vlarge.txt") self.load_mission("Kingaroy-vlarge.txt")
self.load_mission("Kingaroy-vlarge2.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): def tests(self):
'''return list of all tests''' '''return list of all tests'''
ret = super(AutoTestPlane, self).tests() ret = super(AutoTestPlane, self).tests()
@ -1747,6 +1847,10 @@ class AutoTestPlane(AutoTest):
"Test Manipulation of Large missions", "Test Manipulation of Large missions",
self.test_large_missions), self.test_large_missions),
("Soaring",
"Test Soaring feature",
self.fly_soaring),
("LogDownLoad", ("LogDownLoad",
"Log download", "Log download",
lambda: self.log_download( lambda: self.log_download(
@ -1755,121 +1859,3 @@ class AutoTestPlane(AutoTest):
upload_logs=True)) upload_logs=True))
]) ])
return ret 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

View File

@ -252,7 +252,6 @@ __bin_names = {
"QuadPlane": "arduplane", "QuadPlane": "arduplane",
"Sub": "ardusub", "Sub": "ardusub",
"BalanceBot": "ardurover", "BalanceBot": "ardurover",
"Soaring": "arduplane",
} }
@ -305,7 +304,6 @@ tester_class_map = {
"test.Helicopter": arducopter.AutoTestHeli, "test.Helicopter": arducopter.AutoTestHeli,
"test.Sub": ardusub.AutoTestSub, "test.Sub": ardusub.AutoTestSub,
"test.Tracker": antennatracker.AutoTestTracker, "test.Tracker": antennatracker.AutoTestTracker,
"test.Soaring": arduplane.AutoTestSoaring,
} }
def run_specific_test(step, *args, **kwargs): def run_specific_test(step, *args, **kwargs):
@ -778,7 +776,6 @@ if __name__ == "__main__":
'defaults.Plane', 'defaults.Plane',
'test.Plane', 'test.Plane',
'test.QuadPlane', 'test.QuadPlane',
'test.Soaring',
'build.Rover', 'build.Rover',
'defaults.Rover', 'defaults.Rover',

View File

@ -1011,7 +1011,7 @@ class AutoTest(ABC):
vehicle = self.log_name() vehicle = self.log_name()
if vehicle == "HeliCopter": if vehicle == "HeliCopter":
vehicle = "ArduCopter" vehicle = "ArduCopter"
if vehicle == "QuadPlane" or vehicle =="Soaring": if vehicle == "QuadPlane":
vehicle = "ArduPlane" vehicle = "ArduPlane"
cmd = [param_parse_filepath, '--vehicle', vehicle] cmd = [param_parse_filepath, '--vehicle', vehicle]
# cmd.append("--verbose") # cmd.append("--verbose")
@ -1074,8 +1074,6 @@ class AutoTest(ABC):
dirname = "ArduPlane" dirname = "ArduPlane"
elif dirname == "HeliCopter": elif dirname == "HeliCopter":
dirname = "ArduCopter" dirname = "ArduCopter"
elif dirname == "Soaring":
dirname = "ArduPlane"
return os.path.join(self.rootdir(), dirname) return os.path.join(self.rootdir(), dirname)
def all_log_format_ids(self): def all_log_format_ids(self):
@ -1298,7 +1296,6 @@ class AutoTest(ABC):
"HeliCopter": "Copter", "HeliCopter": "Copter",
"ArduPlane": "Plane", "ArduPlane": "Plane",
"QuadPlane": "Plane", "QuadPlane": "Plane",
"Soaring": "Plane",
"Rover": "Rover", "Rover": "Rover",
"AntennaTracker": "Tracker", "AntennaTracker": "Tracker",
"ArduSub": "Sub", "ArduSub": "Sub",