ardupilot/Tools/autotest/arduplane.py

#!/usr/bin/env python

# Fly ArduPlane in SITL
from __future__ import print_function
import math
import os

import pexpect
from pymavlink import mavutil

from pysim import util

from common import AutoTest

# get location of scripts
testdir = os.path.dirname(os.path.realpath(__file__))
HOME = mavutil.location(-35.362938, 149.165085, 585, 354)
WIND = "0,180,0.2"  # speed,direction,variance


class AutoTestPlane(AutoTest):
    def __init__(self,
                 binary,
                 valgrind=False,
                 gdb=False,
                 speedup=10,
                 frame=None,
                 params=None,
                 gdbserver=False,
                 **kwargs):
        super(AutoTestPlane, self).__init__(**kwargs)
        self.binary = binary
        self.valgrind = valgrind
        self.gdb = gdb
        self.frame = frame
        self.params = params
        self.gdbserver = gdbserver

        self.home = "%f,%f,%u,%u" % (HOME.lat,
                                     HOME.lng,
                                     HOME.alt,
                                     HOME.heading)
        self.homeloc = None
        self.speedup = speedup
        self.speedup_default = 10

        self.sitl = None
        self.hasInit = False

        self.log_name = "ArduPlane"

    def init(self):
        if self.frame is None:
            self.frame = 'plane-elevrev'

        defaults_file = os.path.join(testdir,
                                     'default_params/plane-jsbsim.parm')
        self.sitl = util.start_SITL(self.binary,
                                    wipe=True,
                                    model=self.frame,
                                    home=self.home,
                                    speedup=self.speedup,
                                    defaults_file=defaults_file,
                                    valgrind=self.valgrind,
                                    gdb=self.gdb,
                                    gdbserver=self.gdbserver)
        self.mavproxy = util.start_MAVProxy_SITL(
            'ArduPlane', options=self.mavproxy_options())
        self.mavproxy.expect('Telemetry log: (\S+)\r\n')
        logfile = self.mavproxy.match.group(1)
        self.progress("LOGFILE %s" % logfile)

        buildlog = self.buildlogs_path("ArduPlane-test.tlog")
        self.progress("buildlog=%s" % buildlog)
        if os.path.exists(buildlog):
            os.unlink(buildlog)
        try:
            os.link(logfile, buildlog)
        except Exception:
            pass

        self.mavproxy.expect('Received [0-9]+ parameters')

        util.expect_setup_callback(self.mavproxy, self.expect_callback)

        self.expect_list_clear()
        self.expect_list_extend([self.sitl, self.mavproxy])

        self.progress("Started simulator")

        # get a mavlink connection going
        connection_string = '127.0.0.1:19550'
        try:
            self.mav = mavutil.mavlink_connection(connection_string,
                                                  robust_parsing=True)
        except Exception as msg:
            self.progress("Failed to start mavlink connection on %s: %s"
                          % (connection_string, msg))
            raise
        self.mav.message_hooks.append(self.message_hook)
        self.mav.idle_hooks.append(self.idle_hook)
        self.hasInit = True
        self.progress("Ready to start testing!")

    def takeoff(self):
        """Takeoff get to 30m altitude."""

        self.wait_ready_to_arm()
        self.arm_vehicle()

        self.mavproxy.send('switch 4\n')
        self.wait_mode('FBWA')

        # some rudder to counteract the prop torque
        self.set_rc(4, 1700)

        # some up elevator to keep the tail down
        self.set_rc(2, 1200)

        # get it moving a bit first
        self.set_rc(3, 1300)
        self.mav.recv_match(condition='VFR_HUD.groundspeed>6', blocking=True)

        # a bit faster again, straighten rudder
        self.set_rc(3, 1600)
        self.set_rc(4, 1500)
        self.mav.recv_match(condition='VFR_HUD.groundspeed>12', blocking=True)

        # hit the gas harder now, and give it some more elevator
        self.set_rc(2, 1100)
        self.set_rc(3, 2000)

        # gain a bit of altitude
        if not self.wait_altitude(self.homeloc.alt+150,
                                  self.homeloc.alt+180,
                                  timeout=30):
            return False

        # level off
        self.set_rc(2, 1500)

        self.progress("TAKEOFF COMPLETE")
        return True

    def fly_left_circuit(self):
        """Fly a left circuit, 200m on a side."""
        self.mavproxy.send('switch 4\n')
        self.wait_mode('FBWA')
        self.set_rc(3, 2000)
        if not self.wait_level_flight():
            return False

        self.progress("Flying left circuit")
        # do 4 turns
        for i in range(0, 4):
            # hard left
            self.progress("Starting turn %u" % i)
            self.set_rc(1, 1000)
            if not self.wait_heading(270 - (90*i), accuracy=10):
                return False
            self.set_rc(1, 1500)
            self.progress("Starting leg %u" % i)
            if not self.wait_distance(100, accuracy=20):
                return False
        self.progress("Circuit complete")
        return True

    def fly_RTL(self):
        """Fly to home."""
        self.progress("Flying home in RTL")
        self.mavproxy.send('switch 2\n')
        self.wait_mode('RTL')
        if not self.wait_location(self.homeloc,
                                  accuracy=120,
                                  target_altitude=self.homeloc.alt+100,
                                  height_accuracy=20,
                                  timeout=180):
            return False
        self.progress("RTL Complete")
        return True

    def fly_LOITER(self, num_circles=4):
        """Loiter where we are."""
        self.progress("Testing LOITER for %u turns" % num_circles)
        self.mavproxy.send('loiter\n')
        self.wait_mode('LOITER')

        m = self.mav.recv_match(type='VFR_HUD', blocking=True)
        initial_alt = m.alt
        self.progress("Initial altitude %u\n" % initial_alt)

        while num_circles > 0:
            if not self.wait_heading(0, accuracy=10, timeout=60):
                return False
            if not self.wait_heading(180, accuracy=10, timeout=60):
                return False
            num_circles -= 1
            self.progress("Loiter %u circles left" % num_circles)

        m = self.mav.recv_match(type='VFR_HUD', blocking=True)
        final_alt = m.alt
        self.progress("Final altitude %u initial %u\n" %
                      (final_alt, initial_alt))

        self.mavproxy.send('mode FBWA\n')
        self.wait_mode('FBWA')

        if abs(final_alt - initial_alt) > 20:
            self.progress("Failed to maintain altitude")
            return False

        self.progress("Completed Loiter OK")
        return True

    def fly_CIRCLE(self, num_circles=1):
        """Circle where we are."""
        self.progress("Testing CIRCLE for %u turns" % num_circles)
        self.mavproxy.send('mode CIRCLE\n')
        self.wait_mode('CIRCLE')

        m = self.mav.recv_match(type='VFR_HUD', blocking=True)
        initial_alt = m.alt
        self.progress("Initial altitude %u\n" % initial_alt)

        while num_circles > 0:
            if not self.wait_heading(0, accuracy=10, timeout=60):
                return False
            if not self.wait_heading(180, accuracy=10, timeout=60):
                return False
            num_circles -= 1
            self.progress("CIRCLE %u circles left" % num_circles)

        m = self.mav.recv_match(type='VFR_HUD', blocking=True)
        final_alt = m.alt
        self.progress("Final altitude %u initial %u\n" %
                      (final_alt, initial_alt))

        self.mavproxy.send('mode FBWA\n')
        self.wait_mode('FBWA')

        if abs(final_alt - initial_alt) > 20:
            self.progress("Failed to maintain altitude")
            return False

        self.progress("Completed CIRCLE OK")
        return True

    def wait_level_flight(self, accuracy=5, timeout=30):
        """Wait for level flight."""
        tstart = self.get_sim_time()
        self.progress("Waiting for level flight")
        self.set_rc(1, 1500)
        self.set_rc(2, 1500)
        self.set_rc(4, 1500)
        while self.get_sim_time() < tstart + timeout:
            m = self.mav.recv_match(type='ATTITUDE', blocking=True)
            roll = math.degrees(m.roll)
            pitch = math.degrees(m.pitch)
            self.progress("Roll=%.1f Pitch=%.1f" % (roll, pitch))
            if math.fabs(roll) <= accuracy and math.fabs(pitch) <= accuracy:
                self.progress("Attained level flight")
                return True
        self.progress("Failed to attain level flight")
        return False

    def change_altitude(self, altitude, accuracy=30):
        """Get to a given altitude."""
        self.mavproxy.send('mode FBWA\n')
        self.wait_mode('FBWA')
        alt_error = self.mav.messages['VFR_HUD'].alt - altitude
        if alt_error > 0:
            self.set_rc(2, 2000)
        else:
            self.set_rc(2, 1000)
        if not self.wait_altitude(altitude-accuracy/2, altitude+accuracy/2):
            return False
        self.set_rc(2, 1500)
        self.progress("Reached target altitude at %u" %
                      self.mav.messages['VFR_HUD'].alt)
        return self.wait_level_flight()

    def axial_left_roll(self, count=1):
        """Fly a left axial roll."""
        # full throttle!
        self.set_rc(3, 2000)
        if not self.change_altitude(self.homeloc.alt+300):
            return False

        # fly the roll in manual
        self.mavproxy.send('switch 6\n')
        self.wait_mode('MANUAL')

        while count > 0:
            self.progress("Starting roll")
            self.set_rc(1, 1000)
            if not self.wait_roll(-150, accuracy=90):
                self.set_rc(1, 1500)
                return False
            if not self.wait_roll(150, accuracy=90):
                self.set_rc(1, 1500)
                return False
            if not self.wait_roll(0, accuracy=90):
                self.set_rc(1, 1500)
                return False
            count -= 1

        # back to FBWA
        self.set_rc(1, 1500)
        self.mavproxy.send('switch 4\n')
        self.wait_mode('FBWA')
        self.set_rc(3, 1700)
        return self.wait_level_flight()

    def inside_loop(self, count=1):
        """Fly a inside loop."""
        # full throttle!
        self.set_rc(3, 2000)
        if not self.change_altitude(self.homeloc.alt+300):
            return False

        # fly the loop in manual
        self.mavproxy.send('switch 6\n')
        self.wait_mode('MANUAL')

        while count > 0:
            self.progress("Starting loop")
            self.set_rc(2, 1000)
            if not self.wait_pitch(-60, accuracy=20):
                return False
            if not self.wait_pitch(0, accuracy=20):
                return False
            count -= 1

        # back to FBWA
        self.set_rc(2, 1500)
        self.mavproxy.send('switch 4\n')
        self.wait_mode('FBWA')
        self.set_rc(3, 1700)
        return self.wait_level_flight()

    def test_stabilize(self, count=1):
        """Fly stabilize mode."""
        # full throttle!
        self.set_rc(3, 2000)
        self.set_rc(2, 1300)
        if not self.change_altitude(self.homeloc.alt+300):
            return False
        self.set_rc(2, 1500)

        self.mavproxy.send("mode STABILIZE\n")
        self.wait_mode('STABILIZE')

        count = 1
        while count > 0:
            self.progress("Starting roll")
            self.set_rc(1, 2000)
            if not self.wait_roll(-150, accuracy=90):
                return False
            if not self.wait_roll(150, accuracy=90):
                return False
            if not self.wait_roll(0, accuracy=90):
                return False
            count -= 1

        self.set_rc(1, 1500)
        if not self.wait_roll(0, accuracy=5):
            return False

        # back to FBWA
        self.mavproxy.send('mode FBWA\n')
        self.wait_mode('FBWA')
        self.set_rc(3, 1700)
        return self.wait_level_flight()

    def test_acro(self, count=1):
        """Fly ACRO mode."""
        # full throttle!
        self.set_rc(3, 2000)
        self.set_rc(2, 1300)
        if not self.change_altitude(self.homeloc.alt+300):
            return False
        self.set_rc(2, 1500)

        self.mavproxy.send("mode ACRO\n")
        self.wait_mode('ACRO')

        count = 1
        while count > 0:
            self.progress("Starting roll")
            self.set_rc(1, 1000)
            if not self.wait_roll(-150, accuracy=90):
                return False
            if not self.wait_roll(150, accuracy=90):
                return False
            if not self.wait_roll(0, accuracy=90):
                return False
            count -= 1
        self.set_rc(1, 1500)

        # back to FBWA
        self.mavproxy.send('mode FBWA\n')
        self.wait_mode('FBWA')

        self.wait_level_flight()

        self.mavproxy.send("mode ACRO\n")
        self.wait_mode('ACRO')

        count = 2
        while count > 0:
            self.progress("Starting loop")
            self.set_rc(2, 1000)
            if not self.wait_pitch(-60, accuracy=20):
                return False
            if not self.wait_pitch(0, accuracy=20):
                return False
            count -= 1

        self.set_rc(2, 1500)

        # back to FBWA
        self.mavproxy.send('mode FBWA\n')
        self.wait_mode('FBWA')
        self.set_rc(3, 1700)
        return self.wait_level_flight()

    def test_FBWB(self, count=1, mode='FBWB'):
        """Fly FBWB or CRUISE mode."""
        self.mavproxy.send("mode %s\n" % mode)
        self.wait_mode(mode)
        self.set_rc(3, 1700)
        self.set_rc(2, 1500)

        # lock in the altitude by asking for an altitude change then releasing
        self.set_rc(2, 1000)
        self.wait_distance(50, accuracy=20)
        self.set_rc(2, 1500)
        self.wait_distance(50, accuracy=20)

        m = self.mav.recv_match(type='VFR_HUD', blocking=True)
        initial_alt = m.alt
        self.progress("Initial altitude %u\n" % initial_alt)

        self.progress("Flying right circuit")
        # do 4 turns
        for i in range(0, 4):
            # hard left
            self.progress("Starting turn %u" % i)
            self.set_rc(1, 1800)
            if not self.wait_heading(0 + (90*i), accuracy=20, timeout=60):
                self.set_rc(1, 1500)
                return False
            self.set_rc(1, 1500)
            self.progress("Starting leg %u" % i)
            if not self.wait_distance(100, accuracy=20):
                return False
        self.progress("Circuit complete")

        self.progress("Flying rudder left circuit")
        # do 4 turns
        for i in range(0, 4):
            # hard left
            self.progress("Starting turn %u" % i)
            self.set_rc(4, 1900)
            if not self.wait_heading(360 - (90*i), accuracy=20, timeout=60):
                self.set_rc(4, 1500)
                return False
            self.set_rc(4, 1500)
            self.progress("Starting leg %u" % i)
            if not self.wait_distance(100, accuracy=20):
                return False
        self.progress("Circuit complete")

        m = self.mav.recv_match(type='VFR_HUD', blocking=True)
        final_alt = m.alt
        self.progress("Final altitude %u initial %u\n" %
                      (final_alt, initial_alt))

        # back to FBWA
        self.mavproxy.send('mode FBWA\n')
        self.wait_mode('FBWA')

        if abs(final_alt - initial_alt) > 20:
            self.progress("Failed to maintain altitude")
            return False

        return self.wait_level_flight()

    def fly_mission(self, filename, height_accuracy=-1, target_altitude=None):
        """Fly a mission from a file."""
        self.progress("Flying mission %s" % filename)
        self.mavproxy.send('wp load %s\n' % filename)
        self.mavproxy.expect('Flight plan received')
        self.mavproxy.send('wp list\n')
        self.mavproxy.expect('Requesting [0-9]+ waypoints')
        self.mavproxy.send('switch 1\n')  # auto mode
        self.wait_mode('AUTO')
        if not self.wait_waypoint(1, 7, max_dist=60):
            return False
        if not self.wait_groundspeed(0, 0.5, timeout=60):
            return False
        self.mavproxy.expect("Auto disarmed")
        self.progress("Mission OK")
        return True

    def autotest(self):
        """Autotest ArduPlane in SITL."""
        if not self.hasInit:
            self.init()

        failed = False
        fail_list = []
        e = 'None'
        try:
            self.progress("Waiting for a heartbeat with mavlink protocol %s"
                          % self.mav.WIRE_PROTOCOL_VERSION)
            self.mav.wait_heartbeat()
            self.progress("Setting up RC parameters")
            self.set_rc_default()
            self.set_rc(3, 1000)
            self.set_rc(8, 1800)
            self.progress("Waiting for GPS fix")
            self.mav.recv_match(condition='VFR_HUD.alt>10', blocking=True)
            self.mav.wait_gps_fix()
            while self.mav.location().alt < 10:
                self.mav.wait_gps_fix()
            self.homeloc = self.mav.location()
            self.progress("Home location: %s" % self.homeloc)
            if not self.takeoff():
                self.progress("Failed takeoff")
                failed = True
                fail_list.append("takeoff")
            if not self.fly_left_circuit():
                self.progress("Failed left circuit")
                failed = True
                fail_list.append("left_circuit")
            if not self.axial_left_roll(1):
                self.progress("Failed left roll")
                failed = True
                fail_list.append("left_roll")
            if not self.inside_loop():
                self.progress("Failed inside loop")
                failed = True
                fail_list.append("inside_loop")
            if not self.test_stabilize():
                self.progress("Failed stabilize test")
                failed = True
                fail_list.append("stabilize")
            if not self.test_acro():
                self.progress("Failed ACRO test")
                failed = True
                fail_list.append("acro")
            if not self.test_FBWB():
                self.progress("Failed FBWB test")
                failed = True
                fail_list.append("fbwb")
            if not self.test_FBWB(mode='CRUISE'):
                self.progress("Failed CRUISE test")
                failed = True
                fail_list.append("cruise")
            if not self.fly_RTL():
                self.progress("Failed RTL")
                failed = True
                fail_list.append("RTL")
            if not self.fly_LOITER():
                self.progress("Failed LOITER")
                failed = True
                fail_list.append("LOITER")
            if not self.fly_CIRCLE():
                self.progress("Failed CIRCLE")
                failed = True
                fail_list.append("LOITER")
            if not self.fly_mission(os.path.join(testdir, "ap1.txt"),
                                    height_accuracy=10,
                                    target_altitude=self.homeloc.alt+100):
                self.progress("Failed mission")
                failed = True
                fail_list.append("mission")
            if not self.log_download(self.buildlogs_path("ArduPlane-log.bin")):
                self.progress("Failed log download")
                failed = True
                fail_list.append("log_download")
        except pexpect.TIMEOUT as e:
            self.progress("Failed with timeout")
            failed = True
            fail_list.append("timeout")

        self.close()

        if failed:
            self.progress("FAILED: %s" % e)
            self.progress("Fail list: %s" % fail_list)
            return False
        return True