CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
 Path: blob/master/Tools/autotest/sailboat.py
Views: 1798
1
'''

2
Drive a Sailboat in SITL

3


4
AP_FLAKE8_CLEAN

5


6
'''

7


8
from __future__ import print_function

9


10
import os

11


12
from rover import AutoTestRover

13


14
from vehicle_test_suite import AutoTestTimeoutException

15
from vehicle_test_suite import PreconditionFailedException

16


17
# get location of scripts

18
testdir = os.path.dirname(os.path.realpath(__file__))

19


20


21
def log_name(self):

22
    return "Sailboat"

23


24


25
class AutoTestSailboat(AutoTestRover):

26


27
    def vehicleinfo_key(self):

28
        return "Rover"

29


30
    def init(self):

31
        if self.frame is None:

32
            self.frame = 'sailboat'

33
        super(AutoTestSailboat, self).init()

34


35
    def DriveRTL(self, timeout=120):

36
        '''Drive an RTL Mission'''

37
        self.wait_ready_to_arm()

38
        self.arm_vehicle()

39


40
        self.load_mission("rtl.txt")

41
        self.change_mode("AUTO")

42


43
        tstart = self.get_sim_time()

44
        while True:

45
            now = self.get_sim_time_cached()

46
            if now - tstart > timeout:

47
                raise AutoTestTimeoutException("Didn't see wp 3")

48
            m = self.mav.recv_match(type='MISSION_CURRENT',

49
                                    blocking=True,

50
                                    timeout=1)

51
            self.progress("MISSION_CURRENT: %s" % str(m))

52
            if m.seq == 3:

53
                break

54


55
        self.drain_mav()

56


57
        m = self.assert_receive_message('NAV_CONTROLLER_OUTPUT', timeout=1)

58


59
        wp_dist_min = 5

60
        if m.wp_dist < wp_dist_min:

61
            raise PreconditionFailedException(

62
                "Did not start at least %f metres from destination (is=%f)" %

63
                (wp_dist_min, m.wp_dist))

64


65
        self.progress("NAV_CONTROLLER_OUTPUT.wp_dist looks good (%u >= %u)" %

66
                      (m.wp_dist, wp_dist_min,))

67


68
        # wait for mission to complete

69
        self.wait_statustext("Mission Complete", timeout=70)

70


71
        # sailboat loiters around RTL point indefinitely:

72
        self.wait_groundspeed(0.5, 3, timeout=20, minimum_duration=10)

73


74
        self.disarm_vehicle()

75


76
        self.progress("RTL Mission OK")

77


78
    def DriveMission(self):

79
        '''sail a simple mission'''

80
        self.drive_mission("balancebot1.txt", strict=False)

81


82
    def tests(self):

83
        '''return list of all tests'''

84
        ret = ([])

85


86
        ret.extend([

87
            self.DriveRTL,

88
            self.DriveMission,

89
        ])

90
        return ret

91


92
    def default_mode(self):

93
        return 'MANUAL'

94


95