mirror of https://github.com/ArduPilot/ardupilot
94 lines
2.3 KiB
Python
94 lines
2.3 KiB
Python
'''
|
|
Drive a Sailboat in SITL
|
|
|
|
AP_FLAKE8_CLEAN
|
|
|
|
'''
|
|
|
|
from __future__ import print_function
|
|
|
|
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__))
|
|
|
|
|
|
def log_name(self):
|
|
return "Sailboat"
|
|
|
|
|
|
class AutoTestSailboat(AutoTestRover):
|
|
|
|
def vehicleinfo_key(self):
|
|
return "Rover"
|
|
|
|
def init(self):
|
|
if self.frame is None:
|
|
self.frame = 'sailboat'
|
|
super(AutoTestSailboat, self).init()
|
|
|
|
def DriveRTL(self, timeout=120):
|
|
'''Drive an RTL Mission'''
|
|
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 DriveMission(self):
|
|
'''sail a simple mission'''
|
|
self.drive_mission("balancebot1.txt", strict=False)
|
|
|
|
def tests(self):
|
|
'''return list of all tests'''
|
|
ret = ([])
|
|
|
|
ret.extend([
|
|
self.DriveRTL,
|
|
self.DriveMission,
|
|
])
|
|
return ret
|
|
|
|
def default_mode(self):
|
|
return 'MANUAL'
|