autotest: customise drive_rtl_mission for sailboats
This commit is contained in:
parent
2059d1485e
commit
81740893ee
@ -11,6 +11,9 @@ import os
|
||||
|
||||
from rover import AutoTestRover
|
||||
|
||||
from common import AutoTestTimeoutException
|
||||
from common import PreconditionFailedException
|
||||
|
||||
# get location of scripts
|
||||
testdir = os.path.dirname(os.path.realpath(__file__))
|
||||
|
||||
@ -29,6 +32,48 @@ class AutoTestSailboat(AutoTestRover):
|
||||
self.frame = 'sailboat'
|
||||
super(AutoTestSailboat, self).init()
|
||||
|
||||
def drive_rtl_mission(self, timeout=120):
|
||||
self.wait_ready_to_arm()
|
||||
self.arm_vehicle()
|
||||
|
||||
self.load_mission("rtl.txt")
|
||||
self.change_mode("AUTO")
|
||||
|
||||
tstart = self.get_sim_time()
|
||||
while True:
|
||||
now = self.get_sim_time_cached()
|
||||
if now - tstart > timeout:
|
||||
raise AutoTestTimeoutException("Didn't see wp 3")
|
||||
m = self.mav.recv_match(type='MISSION_CURRENT',
|
||||
blocking=True,
|
||||
timeout=1)
|
||||
self.progress("MISSION_CURRENT: %s" % str(m))
|
||||
if m.seq == 3:
|
||||
break
|
||||
|
||||
self.drain_mav()
|
||||
|
||||
m = self.assert_receive_message('NAV_CONTROLLER_OUTPUT', timeout=1)
|
||||
|
||||
wp_dist_min = 5
|
||||
if m.wp_dist < wp_dist_min:
|
||||
raise PreconditionFailedException(
|
||||
"Did not start at least %f metres from destination (is=%f)" %
|
||||
(wp_dist_min, m.wp_dist))
|
||||
|
||||
self.progress("NAV_CONTROLLER_OUTPUT.wp_dist looks good (%u >= %u)" %
|
||||
(m.wp_dist, wp_dist_min,))
|
||||
|
||||
# wait for mission to complete
|
||||
self.wait_statustext("Mission Complete", timeout=70)
|
||||
|
||||
# sailboat loiters around RTL point indefinitely:
|
||||
self.wait_groundspeed(0.5, 3, timeout=20, minimum_duration=10)
|
||||
|
||||
self.disarm_vehicle()
|
||||
|
||||
self.progress("RTL Mission OK")
|
||||
|
||||
def tests(self):
|
||||
'''return list of all tests'''
|
||||
ret = ([])
|
||||
|
Loading…
Reference in New Issue
Block a user