mirror of https://github.com/ArduPilot/ardupilot
Autotest: Simplify soaring.py.
This commit is contained in:
Samuel Tabor
Andrew Tridgell
parent
d04c6cb02e
commit
7168a65155
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue