ardupilot/Tools/autotest/sim_vehicle.py

#!/usr/bin/env python

"""
Framework to start a simulated vehicle and connect it to MAVProxy.

Peter Barker, April 2016
based on sim_vehicle.sh by Andrew Tridgell, October 2011
"""
from __future__ import print_function

import atexit
import errno
import getpass
import optparse
import os
import os.path
import re
import signal
import subprocess
import sys
import tempfile
import textwrap
import time
import shlex

# List of open terminal windows for macosx
windowID = []


class CompatError(Exception):
    """A custom exception class to hold state if we encounter the parse error we are looking for"""
    def __init__(self, error, opts, rargs):
        Exception.__init__(self, error)
        self.opts = opts
        self.rargs = rargs


class CompatOptionParser(optparse.OptionParser):
    """An option parser which emulates the behaviour of the old sim_vehicle.sh; if passed -C, the first argument not understood starts a list of arguments that are passed straight to mavproxy"""

    class CustomFormatter(optparse.IndentedHelpFormatter):
        def __init__(self, *args, **kwargs):
            optparse.IndentedHelpFormatter.__init__(self,*args, **kwargs)

        # taken and modified from from optparse.py's format_option
        def format_option_preserve_nl(self, option):
            # The help for each option consists of two parts:
            #   * the opt strings and metavars
            #     eg. ("-x", or "-fFILENAME, --file=FILENAME")
            #   * the user-supplied help string
            #     eg. ("turn on expert mode", "read data from FILENAME")
            #
            # If possible, we write both of these on the same line:
            #   -x      turn on expert mode
            #
            # But if the opt string list is too long, we put the help
            # string on a second line, indented to the same column it would
            # start in if it fit on the first line.
            #   -fFILENAME, --file=FILENAME
            #           read data from FILENAME
            result = []
            opts = self.option_strings[option]
            opt_width = self.help_position - self.current_indent - 2
            if len(opts) > opt_width:
                opts = "%*s%s\n" % (self.current_indent, "", opts)
                indent_first = self.help_position
            else:                       # start help on same line as opts
                opts = "%*s%-*s  " % (self.current_indent, "", opt_width, opts)
                indent_first = 0
            result.append(opts)
            if option.help:
                help_text = self.expand_default(option)
                tw = textwrap.TextWrapper(replace_whitespace=False, initial_indent="", subsequent_indent="    ", width=self.help_width)

                for line in help_text.split("\n"):
                    help_lines = tw.wrap(line)
                    for line in help_lines:
                        result.extend(["%*s%s\n" % (self.help_position, "", line)])
            elif opts[-1] != "\n":
                result.append("\n")
            return "".join(result)

        def format_option(self, option):
            if str(option).find('frame') != -1:
                return self.format_option_preserve_nl(option)
            return optparse.IndentedHelpFormatter.format_option(self, option)

    def __init__(self, *args, **kwargs):
        formatter = CompatOptionParser.CustomFormatter()
        optparse.OptionParser.__init__(self, *args, formatter=formatter, **kwargs)

    def error(self, error):
        """Override default error handler called by optparse.OptionParser.parse_args when a parse error occurs; raise a detailed exception which can be caught"""
        if error.find("no such option") != -1:
            raise CompatError(error, self.values, self.rargs)

        optparse.OptionParser.error(self, error)

    def parse_args(self, args=None, values=None):
        """Wrap parse_args so we can catch the exception raised upon discovering the known parameter parsing error"""
        try:
            opts, args = optparse.OptionParser.parse_args(self)
        except CompatError as e:
            if not e.opts.sim_vehicle_sh_compatible:
                print(e)
                print("Perhaps you want --sim_vehicle_sh_compatible (-C)?")
                sys.exit(1)
            if e.opts.mavproxy_args:
                print("--mavproxy-args not permitted in compat mode")
                sys.exit(1)

            args = []
            opts = e.opts
            mavproxy_args = [str(e)[16:]]  # this trims "no such option" off
            mavproxy_args.extend(e.rargs)
            opts.ensure_value("mavproxy_args", " ".join(mavproxy_args))

        return opts, args


def cygwin_pidof(proc_name):
    """ Thanks to kata198 for this:
    https://github.com/kata198/cygwin-ps-misc/blob/master/pidof
    """
    pipe = subprocess.Popen("ps -ea | grep " + proc_name, shell=True, stdout=subprocess.PIPE)
    output_lines = pipe.stdout.read().replace("\r", "").split("\n")
    ret = pipe.wait()
    pids = []
    if ret != 0:
        # No results
        return []
    for line in output_lines:
        if not line:
            continue
        line_split = [item for item in line.split(' ') if item]
        cmd = line_split[-1].split('/')[-1]
        if cmd == proc_name:
            try:
                pid = int(line_split[0].strip())
            except:
                pid = int(line_split[1].strip())
            if pid not in pids:
                pids.append(pid)
    return pids


def under_cygwin():
    """Return if Cygwin binary exist"""
    return os.path.exists("/usr/bin/cygstart")


def under_macos():
    return sys.platform == 'darwin'


def kill_tasks_cygwin(victims):
    """Shell out to ps -ea to find processes to kill"""
    for victim in list(victims):
        pids = cygwin_pidof(victim)
#        progress("pids for (%s): %s" % (victim,",".join([ str(p) for p in pids])))
        for apid in pids:
            os.kill(apid, signal.SIGKILL)


def kill_tasks_macos():
    for window in windowID:
        cmd = "osascript -e \'tell application \"Terminal\" to close (window(get index of window id %s))\'" % window
        os.system(cmd)


def kill_tasks_psutil(victims):
    """Use the psutil module to kill tasks by name.  Sadly, this module is not available on Windows, but when it is we should be able to *just* use this routine"""
    import psutil
    for proc in psutil.process_iter():
        if proc.status == psutil.STATUS_ZOMBIE:
            continue
        if proc.name in victims:
            proc.kill()


def kill_tasks_pkill(victims):
    """Shell out to pkill(1) to kill processed by name"""
    for victim in victims:  # pkill takes a single pattern, so iterate
        cmd = ["pkill", victim]
        run_cmd_blocking("pkill", cmd, quiet=True)


class BobException(Exception):
    """Handle Bob's Exceptions"""
    pass


def kill_tasks():
    """Clean up stray processes by name.  This is a somewhat shotgun approach"""
    progress("Killing tasks")
    try:
        victim_names = {
            'JSBSim',
            'lt-JSBSim',
            'ArduPlane.elf',
            'ArduCopter.elf',
            'ArduSub.elf',
            'APMrover2.elf',
            'AntennaTracker.elf',
            'JSBSIm.exe',
            'MAVProxy.exe',
            'runsim.py',
            'AntennaTracker.elf',
        }
        for vehicle in _options:
            for frame in _options[vehicle]["frames"]:
                frame_info = _options[vehicle]["frames"][frame]
                if "waf_target" not in frame_info:
                    continue
                exe_name = os.path.basename(frame_info["waf_target"])
                victim_names.add(exe_name)

        if under_cygwin():
            return kill_tasks_cygwin(victim_names)
        if under_macos():
            return kill_tasks_macos()

        try:
            kill_tasks_psutil(victim_names)
        except ImportError:
            kill_tasks_pkill(victim_names)
    except Exception as e:
        progress("kill_tasks failed: {}".format(str(e)))


def check_jsbsim_version():
    """Assert that the JSBSim we will run is the one we expect to run"""
    jsbsim_cmd = ["JSBSim", "--version"]
    progress_cmd("Get JSBSim version", jsbsim_cmd)
    try:
        jsbsim_version = subprocess.Popen(jsbsim_cmd, stdout=subprocess.PIPE).communicate()[0]
    except OSError:
        jsbsim_version = ''     # this value will trigger the ".index"
                                # check below and produce a reasonable
                                # error message
    try:
        jsbsim_version.index(b"ArduPilot")
    except ValueError:
        print(r"""
=========================================================
You need the latest ArduPilot version of JSBSim installed
and in your \$PATH

Please get it from git://github.com/tridge/jsbsim.git
See
http://ardupilot.org/dev/docs/setting-up-sitl-on-linux.html
for more details
=========================================================
""")
        sys.exit(1)


def progress(text):
    """Display sim_vehicle progress text"""
    print("SIM_VEHICLE: " + text)


def find_autotest_dir():
    """Return path to autotest directory"""
    return os.path.dirname(os.path.realpath(__file__))


def find_root_dir():
    """Return path to root directory"""
    return os.path.realpath(os.path.join(find_autotest_dir(), '../..'))


def wait_unlimited():
    """Wait until signal received"""
    time.sleep(987654321987654321)


"""
make_target: option passed to make to create binaries.  Usually sitl, and "-debug" may be appended if -D is passed to sim_vehicle.py
default_params_filename: filename of default parameters file.  Taken to be relative to autotest dir.
extra_mavlink_cmds: extra parameters that will be passed to mavproxy
"""
_options = {
    "ArduCopter": {
        "default_frame": "quad",
        "frames": {
            # COPTER
            "+": {
                "waf_target": "bin/arducopter",
                "default_params_filename": "default_params/copter.parm",
            },
            "quad": {
                "model": "+",
                "waf_target": "bin/arducopter",
                "default_params_filename": "default_params/copter.parm",
            },
            "X": {
                "waf_target": "bin/arducopter",
                "default_params_filename": "default_params/copter.parm",
                # this param set FRAME doesn't actually work because mavproxy
                # won't set a parameter unless it knows of it, and the
                # param fetch happens asynchronously
                "extra_mavlink_cmds": "param fetch frame; param set FRAME 1;",
            },
            "hexa": {
                "make_target": "sitl",
                "waf_target": "bin/arducopter",
                "default_params_filename": "default_params/copter-hexa.parm",
            },
            "octa-quad": {
                "make_target": "sitl",
                "waf_target": "bin/arducopter",
                "default_params_filename": "default_params/copter-octaquad.parm",
            },
            "octa": {
                "make_target": "sitl",
                "waf_target": "bin/arducopter",
                "default_params_filename": "default_params/copter-octa.parm",
            },
            "tri": {
                "make_target": "sitl",
                "waf_target": "bin/arducopter",
                "default_params_filename": "default_params/copter-tri.parm",
            },
            "y6": {
                "make_target": "sitl",
                "waf_target": "bin/arducopter",
                "default_params_filename": "default_params/copter-y6.parm",
            },
            "firefly": {
                "waf_target": "bin/arduplane",
                "default_params_filename": "default_params/firefly.parm",
            },
            # SIM
            "IrisRos": {
                "waf_target": "bin/arducopter",
                "default_params_filename": "default_params/copter.parm",
            },
            "gazebo-iris": {
                "waf_target": "bin/arducopter",
                "default_params_filename": "default_params/gazebo-iris.parm",
            },
            # HELICOPTER
            "heli": {
                "make_target": "sitl-heli",
                "waf_target": "bin/arducopter-heli",
                "default_params_filename": "default_params/copter-heli.parm",
            },
            "heli-dual": {
                "make_target": "sitl-heli-dual",
                "waf_target": "bin/arducopter-heli",
                "default_params_filename": "default_params/copter-heli-dual.parm",
            },
            "heli-compound": {
                "make_target": "sitl-heli-compound",
                "waf_target": "bin/arducopter-heli",
            },
            "singlecopter": {
                "make_target": "sitl",
                "waf_target": "bin/arducopter",
                "default_params_filename": "default_params/copter-single.parm",
            },
            "coaxcopter": {
                "make_target": "sitl",
                "waf_target": "bin/arducopter",
                "default_params_filename": "default_params/copter-coax.parm",
            },
            "calibration": {
                "extra_mavlink_cmds": "module load sitl_calibration;",
            },
        },
    },
    "ArduPlane": {
        "default_frame": "jsbsim",
        "frames": {
            # PLANE
            "quadplane-tilttri": {
                "make_target": "sitl",
                "waf_target": "bin/arduplane",
                "default_params_filename": "default_params/quadplane-tilttri.parm",
            },
            "quadplane-tilttrivec": {
                "make_target": "sitl",
                "waf_target": "bin/arduplane",
                "default_params_filename": "default_params/quadplane-tilttrivec.parm",
            },
            "quadplane-tri": {
                "make_target": "sitl",
                "waf_target": "bin/arduplane",
                "default_params_filename": "default_params/quadplane-tri.parm",
            },
            "quadplane-cl84" : {
                "make_target" : "sitl",
                "waf_target" : "bin/arduplane",
                "default_params_filename": "default_params/quadplane-cl84.parm",
            },
            "quadplane": {
                "waf_target": "bin/arduplane",
                "default_params_filename": "default_params/quadplane.parm",
            },
            "plane-elevon": {
                "waf_target": "bin/arduplane",
                "default_params_filename": "default_params/plane-elevons.parm",
            },
            "plane-vtail": {
                "waf_target": "bin/arduplane",
                "default_params_filename": "default_params/plane-vtail.parm",
            },
            "plane-tailsitter": {
                "waf_target": "bin/arduplane",
                "default_params_filename": "default_params/plane-tailsitter.parm",
            },
            "plane": {
                "waf_target": "bin/arduplane",
                "default_params_filename": "default_params/plane.parm",
            },
            "gazebo-zephyr": {
                "waf_target": "bin/arduplane",
                "default_params_filename": "default_params/gazebo-zephyr.parm",
            },
            "last_letter": {
                "waf_target": "bin/arduplane",
            },
            "CRRCSim": {
                "waf_target": "bin/arduplane",
            },
            "jsbsim": {
                "waf_target": "bin/arduplane",
                "default_params_filename": "default_params/plane-jsbsim.parm",
            },
            "calibration": {
                "extra_mavlink_cmds": "module load sitl_calibration;",
            },
        },
    },
    "APMrover2": {
        "default_frame": "rover",
        "frames": {
            # ROVER
            "rover": {
                "waf_target": "bin/ardurover",
                "default_params_filename": "default_params/rover.parm",
            },
            "rover-skid": {
                "waf_target": "bin/ardurover",
                "default_params_filename": "default_params/rover-skid.parm",
            },
            "gazebo-rover": {
                "waf_target": "bin/ardurover",
                "default_params_filename": "default_params/rover-skid.parm",
            },
            "calibration": {
                "extra_mavlink_cmds": "module load sitl_calibration;",
            },
        },
    },
    "ArduSub": {
        "default_frame": "vectored",
        "frames": {
            "vectored": {
                "waf_target": "bin/ardusub",
                "default_params_filename": "default_params/sub.parm",
            },
        },
    },
    "AntennaTracker": {
        "default_frame": "tracker",
        "frames": {
            "tracker": {
                "waf_target": "bin/antennatracker",
            },
        },
    },
}

def default_waf_target(vehicle):
    """Returns a waf target based on vehicle type, which is often determined by which directory the user is in"""
    default_frame = _options[vehicle]["default_frame"]
    return _options[vehicle]["frames"][default_frame]["waf_target"]


def options_for_frame(frame, vehicle, opts):
    """Return informatiom about how to sitl for frame e.g. build-type==sitl"""
    ret = None
    frames = _options[vehicle]["frames"]
    if frame in frames:
        ret = _options[vehicle]["frames"][frame]
    else:
        for p in ["octa", "tri", "y6", "firefly", "heli", "gazebo", "last_letter", "jsbsim", "quadplane", "plane-elevon", "plane-vtail", "plane"]:
            if frame.startswith(p):
                ret = _options[vehicle]["frames"][p]
                break
    if ret is None:
        if frame.endswith("-heli"):
            ret = _options[vehicle]["frames"]["heli"]
    if ret is None:
        progress("WARNING: no config for frame (%s)" % frame)
        ret = {}

    if "model" not in ret:
        ret["model"] = frame

    if "sitl-port" not in ret:
        ret["sitl-port"] = True

    if opts.model is not None:
        ret["model"] = opts.model

    if (ret["model"].find("xplane") != -1 or ret["model"].find("flightaxis") != -1):
        ret["sitl-port"] = False

    if "make_target" not in ret:
        ret["make_target"] = "sitl"

    if "waf_target" not in ret:
        ret["waf_target"] = default_waf_target(vehicle)

    if opts.build_target is not None:
        ret["make_target"] = opts.build_target
        ret["waf_target"] = opts.build_target

    return ret


def do_build_waf(opts, frame_options):
    """Build sitl using waf"""
    progress("WAF build")

    old_dir = os.getcwd()
    root_dir = find_root_dir()
    os.chdir(root_dir)

    waf_light = os.path.join(root_dir, "modules/waf/waf-light")

    cmd_configure = [waf_light, "configure", "--board", "sitl"]
    if opts.debug:
        cmd_configure.append("--debug")
    pieces = [ shlex.split(x) for x in opts.waf_configure_args ]
    for piece in pieces:
        cmd_configure.extend(piece)

    run_cmd_blocking("Configure waf", cmd_configure, check=True)

    if opts.clean:
        run_cmd_blocking("Building clean", [waf_light, "clean"])

    cmd_build = [waf_light, "build", "--target", frame_options["waf_target"]]
    if opts.jobs is not None:
        cmd_build += ['-j', str(opts.jobs)]
    pieces = [ shlex.split(x) for x in opts.waf_build_args ]
    for piece in pieces:
        cmd_build.extend(piece)

    _, sts = run_cmd_blocking("Building", cmd_build)

    if sts != 0:  # build failed
        if opts.rebuild_on_failure:
            progress("Build failed; cleaning and rebuilding")
            run_cmd_blocking("Building clean", [waf_light, "clean"])

            _, sts = run_cmd_blocking("Building", cmd_build)
            if sts != 0:
                progress("Build failed")
                sys.exit(1)
        else:
            progress("Build failed")
            sys.exit(1)

    os.chdir(old_dir)


def do_build(vehicledir, opts, frame_options):
    """Build build target (e.g. sitl) in directory vehicledir"""

    if opts.build_system == 'waf':
        return do_build_waf(opts, frame_options)

    old_dir = os.getcwd()

    os.chdir(vehicledir)

    if opts.clean:
        run_cmd_blocking("Building clean", ["make", "clean"])

    build_target = frame_options["make_target"]
    if opts.debug:
        build_target += "-debug"

    build_cmd = ["make", build_target]
    if opts.jobs is not None:
        build_cmd += ['-j', str(opts.jobs)]

    _, sts = run_cmd_blocking("Building %s" % build_target, build_cmd)
    if sts != 0:
        progress("Build failed; cleaning and rebuilding")
        run_cmd_blocking("Cleaning", ["make", "clean"])
        _, sts = run_cmd_blocking("Building %s" % build_target, build_cmd)
        if sts != 0:
            progress("Build failed")
            sys.exit(1)

    os.chdir(old_dir)


def get_user_locations_path():
    '''The user locations.txt file is located by default in
    $XDG_CONFIG_DIR/ardupilot/locations.txt. If $XDG_CONFIG_DIR is
    not defined, we look in $HOME/.config/ardupilot/locations.txt.  If
    $HOME is not defined, we look in ./.config/ardpupilot/locations.txt.'''

    config_dir = os.environ.get(
        'XDG_CONFIG_DIR',
        os.path.join(os.environ.get('HOME', '.'), '.config'))

    user_locations_path = os.path.join(
        config_dir, 'ardupilot', 'locations.txt')

    return user_locations_path


def find_location_by_name(autotest, locname):
    """Search locations.txt for locname, return GPS coords"""
    locations_userpath = os.environ.get('ARDUPILOT_LOCATIONS',
                                        get_user_locations_path())
    locations_filepath = os.path.join(autotest, "locations.txt")
    comment_regex = re.compile("\s*#.*")
    for path in [locations_userpath, locations_filepath]:
        if not os.path.isfile(path):
            continue

        with open(path, 'r') as fd:
            for line in fd:
                line = re.sub(comment_regex, "", line)
                line = line.rstrip("\n")
                if len(line) == 0:
                    continue
                (name, loc) = line.split("=")
                if name == locname:
                    return loc

    print("Failed to find location (%s)" % cmd_opts.location)
    sys.exit(1)


def progress_cmd(what, cmd):
    """Print cmd in a way a user could cut-and-paste to get the same effect"""
    progress(what)
    shell_text = "%s" % (" ".join(['"%s"' % x for x in cmd]))
    progress(shell_text)


def run_cmd_blocking(what, cmd, quiet=False, check=False, **kw):
    if not quiet:
        progress_cmd(what, cmd)
    p = subprocess.Popen(cmd, **kw)
    ret = os.waitpid(p.pid, 0)
    _, sts = ret
    if check and sts != 0:
        progress("(%s) exited with code %d" % (what,sts,))
        sys.exit(1)
    return ret


def run_in_terminal_window(autotest, name, cmd):

    """Execute the run_in_terminal_window.sh command for cmd"""
    global windowID
    runme = [os.path.join(autotest, "run_in_terminal_window.sh"), name]
    runme.extend(cmd)
    progress_cmd("Run " + name, runme)

    if under_macos():
        # on MacOS record the window IDs so we can close them later
        out = subprocess.Popen(runme, stdout=subprocess.PIPE).communicate()[0]
        import re
        p = re.compile('tab 1 of window id (.*)')
        windowID.append(p.findall(out)[0])
    else:
        p = subprocess.Popen(runme)

tracker_uarta = None  # blemish


def start_antenna_tracker(autotest, opts):
    """Compile and run the AntennaTracker, add tracker to mavproxy"""
    global tracker_uarta
    progress("Preparing antenna tracker")
    tracker_home = find_location_by_name(find_autotest_dir(), opts.tracker_location)
    vehicledir = os.path.join(autotest, "../../" + "AntennaTracker")
    tracker_default_frame = _options["AntennaTracker"]["default_frame"]
    tracker_frame_options = _options["AntennaTracker"]["frames"][tracker_default_frame]
    do_build(vehicledir, opts, tracker_frame_options)
    tracker_instance = 1
    os.chdir(vehicledir)
    tracker_uarta = "tcp:127.0.0.1:" + str(5760 + 10 * tracker_instance)
    exe = os.path.join(vehicledir, "AntennaTracker.elf")
    run_in_terminal_window(autotest, "AntennaTracker", ["nice", exe, "-I" + str(tracker_instance), "--model=tracker", "--home=" + tracker_home])


def start_vehicle(binary, autotest, opts, stuff, loc):
    """Run the ArduPilot binary"""

    cmd_name = opts.vehicle
    cmd = []
    if opts.valgrind:
        cmd_name += " (valgrind)"
        cmd.append("valgrind")
    if opts.gdb:
        cmd_name += " (gdb)"
        cmd.append("gdb")
        gdb_commands_file = tempfile.NamedTemporaryFile(delete=False)
        atexit.register(os.unlink, gdb_commands_file.name)

        for breakpoint in opts.breakpoint:
            gdb_commands_file.write("b %s\n" % (breakpoint,))
        gdb_commands_file.write("r\n")
        gdb_commands_file.close()
        cmd.extend(["-x", gdb_commands_file.name])
        cmd.append("--args")
    if opts.strace:
        cmd_name += " (strace)"
        cmd.append("strace")
        strace_options = ['-o', binary + '.strace', '-s', '8000', '-ttt']
        cmd.extend(strace_options)

    cmd.append(binary)
    cmd.append("-S")
    cmd.append("-I" + str(opts.instance))
    cmd.extend(["--home", loc])
    if opts.wipe_eeprom:
        cmd.append("-w")
    cmd.extend(["--model", stuff["model"]])
    cmd.extend(["--speedup", str(opts.speedup)])
    if opts.sitl_instance_args:
        cmd.extend(opts.sitl_instance_args.split(" "))  # this could be a lot better..
    if opts.mavlink_gimbal:
        cmd.append("--gimbal")
    if "default_params_filename" in stuff:
        paths = stuff["default_params_filename"]
        if not isinstance(paths,list):
            paths = [paths]
        paths =  [ os.path.join(autotest, x) for x in paths ]
        path = ",".join(paths)
        progress("Using defaults from (%s)" % (path,))
        cmd.extend(["--defaults", path])

    run_in_terminal_window(autotest, cmd_name, cmd)


def start_mavproxy(opts, stuff):
    """Run mavproxy"""
    # FIXME: would be nice to e.g. "mavproxy.mavproxy(....).run" rather than shelling out

    extra_cmd = ""
    cmd = []
    if under_cygwin():
        cmd.append("/usr/bin/cygstart")
        cmd.append("-w")
        cmd.append("/cygdrive/c/Program Files (x86)/MAVProxy/mavproxy.exe")
    else:
        cmd.append("mavproxy.py")

    if opts.hil:
        cmd.extend(["--load-module", "HIL"])
    else:
        cmd.extend(["--master", mavlink_port])
        if stuff["sitl-port"]:
            cmd.extend(["--sitl", simout_port])

    # If running inside of a vagrant guest, then we probably want to forward our mavlink out to the containing host OS
    ports = [p + 10 * cmd_opts.instance for p in [14550,14551]]
    for port in ports:
        if os.path.isfile("/ardupilot.vagrant"):
            cmd.extend(["--out", "10.0.2.2:" + str(port)])
        else:
            cmd.extend(["--out", "127.0.0.1:" + str(port)])

    if opts.tracker:
        cmd.extend(["--load-module", "tracker"])
        global tracker_uarta
        # tracker_uarta is set when we start the tracker...
        extra_cmd += "module load map; tracker set port %s; tracker start; tracker arm;" % (tracker_uarta,)

    if opts.mavlink_gimbal:
        cmd.extend(["--load-module", "gimbal"])

    if "extra_mavlink_cmds" in stuff:
        extra_cmd += " " + stuff["extra_mavlink_cmds"]

    if opts.mavproxy_args:
        cmd.extend(opts.mavproxy_args.split(" "))  # this could be a lot better..

    # compatibility pass-through parameters (for those that don't want
    # to use -C :-)
    for out in opts.out:
        cmd.extend(['--out', out])
    if opts.map:
        cmd.append('--map')
    if opts.console:
        cmd.append('--console')
    if opts.aircraft is not None:
        cmd.extend(['--aircraft', opts.aircraft])

    if len(extra_cmd):
        cmd.extend(['--cmd', extra_cmd])

    local_mp_modules_dir = os.path.abspath(
        os.path.join(__file__, '..', '..', 'mavproxy_modules'))
    env = dict(os.environ)
    env['PYTHONPATH'] = local_mp_modules_dir + os.pathsep + env.get('PYTHONPATH', '')

    run_cmd_blocking("Run MavProxy", cmd, env=env)
    progress("MAVProxy exited")


vehicle_options_string = '|'.join(_options.keys())

def generate_frame_help():
    ret = ""
    for vehicle in _options:
        frame_options_string = '|'.join(_options[vehicle]["frames"].keys())
        ret += "%s: %s\n" % (vehicle, frame_options_string)
    return ret

# define and run parser
parser = CompatOptionParser("sim_vehicle.py",
        epilog="eeprom.bin in the starting directory contains the parameters for your " \
               "simulated vehicle. Always start from the same directory. It is "\
               "recommended that you start in the main vehicle directory for the vehicle" \
               "you are simulating, for example, start in the ArduPlane directory to " \
               "simulate ArduPlane")

parser.add_option("-v", "--vehicle", type='choice', default=None, help="vehicle type (%s)" % vehicle_options_string, choices=_options.keys())
parser.add_option("-f", "--frame", type='string', default=None, help="""set vehicle frame type

%s""" % (generate_frame_help()))
parser.add_option("-C", "--sim_vehicle_sh_compatible", action='store_true', default=False, help="be compatible with the way sim_vehicle.sh works; make this the first option")
parser.add_option("-H", "--hil", action='store_true', default=False, help="start HIL")

group_build = optparse.OptionGroup(parser, "Build options")
group_build.add_option("-N", "--no-rebuild", action='store_true', default=False, help="don't rebuild before starting ardupilot")
group_build.add_option("-D", "--debug", action='store_true', default=False, help="build with debugging")
group_build.add_option("-c", "--clean", action='store_true', default=False, help="do a make clean before building")
group_build.add_option("-j", "--jobs", default=None, type='int', help="number of processors to use during build (default for waf : number of processor, for make : 1)")
group_build.add_option("-b", "--build-target", default=None, type='string', help="override SITL build target")
group_build.add_option("-s", "--build-system", default="waf", type='choice', choices=["make", "waf"], help="build system to use")
group_build.add_option("", "--rebuild-on-failure", dest="rebuild_on_failure", action='store_true', default=False, help="if build fails, do not clean and rebuild")
group_build.add_option("", "--waf-configure-arg", action="append", dest="waf_configure_args", type="string", default=[], help="extra arguments to pass to waf in its configure step")
group_build.add_option("", "--waf-build-arg", action="append", dest="waf_build_args", type="string", default=[], help="extra arguments to pass to waf in its build step")
parser.add_option_group(group_build)

group_sim = optparse.OptionGroup(parser, "Simulation options")
group_sim.add_option("-I", "--instance", default=0, type='int', help="instance of simulator")
group_sim.add_option("-V", "--valgrind", action='store_true', default=False, help="enable valgrind for memory access checking (very slow!)")
group_sim.add_option("-T", "--tracker", action='store_true', default=False, help="start an antenna tracker instance")
group_sim.add_option("-A", "--sitl-instance-args", type='string', default=None, help="pass arguments to SITL instance")
# group_sim.add_option("-R", "--reverse-throttle", action='store_true', default=False, help="reverse throttle in plane")
group_sim.add_option("-G", "--gdb", action='store_true', default=False, help="use gdb for debugging ardupilot")
group_sim.add_option("-g", "--gdb-stopped", action='store_true', default=False, help="use gdb for debugging ardupilot (no auto-start)")
group_sim.add_option("-d", "--delay-start", default=0, type='float', help="delays the start of mavproxy by the number of seconds")
group_sim.add_option("-B", "--breakpoint", type='string', action="append", default=[], help="add a breakpoint at given location in debugger")
group_sim.add_option("-M", "--mavlink-gimbal", action='store_true', default=False, help="enable MAVLink gimbal")
group_sim.add_option("-L", "--location", type='string', default='CMAC', help="select start location from Tools/autotest/locations.txt")
group_sim.add_option("-l", "--custom-location", type='string', default=None, help="set custom start location")
group_sim.add_option("-S", "--speedup", default=1, type='int', help="set simulation speedup (1 for wall clock time)")
group_sim.add_option("-t", "--tracker-location", default='CMAC_PILOTSBOX', type='string', help="set antenna tracker start location")
group_sim.add_option("-w", "--wipe-eeprom", action='store_true', default=False, help="wipe EEPROM and reload parameters")
group_sim.add_option("-m", "--mavproxy-args", default=None, type='string', help="additional arguments to pass to mavproxy.py")
group_sim.add_option("", "--strace", action='store_true', default=False, help="strace the ArduPilot binary")
group_sim.add_option("", "--model", type='string', default=None, help="Override simulation model to use")
group_sim.add_option("", "--use-dir", type='string', default=None, help="Store SITL state and output in named directory")
group_sim.add_option("", "--no-mavproxy", action='store_true', default=False, help="Don't launch MAVProxy")
parser.add_option_group(group_sim)


# special-cased parameters for mavproxy, because some people's fingers
# have long memories, and they don't want to use -C :-)
group = optparse.OptionGroup(parser, "Compatibility MAVProxy options (consider using --mavproxy-args instead)")
group.add_option("", "--out", default=[], type='string', action="append", help="create an additional mavlink output")
group.add_option("", "--map", default=False, action='store_true', help="load map module on startup")
group.add_option("", "--console", default=False, action='store_true', help="load console module on startup")
group.add_option("", "--aircraft", default=None, help="store state and logs in named directory")
parser.add_option_group(group)

cmd_opts, cmd_args = parser.parse_args()

# clean up processes at exit:
atexit.register(kill_tasks)

progress("Start")

if cmd_opts.sim_vehicle_sh_compatible and cmd_opts.jobs is None:
    cmd_opts.jobs = 1

# validate parameters
if cmd_opts.hil:
    if cmd_opts.valgrind:
        print("May not use valgrind with hil")
        sys.exit(1)
    if cmd_opts.gdb or cmd_opts.gdb_stopped:
        print("May not use gdb with hil")
        sys.exit(1)
    if cmd_opts.strace:
        print("May not use strace with hil")
        sys.exit(1)

if cmd_opts.valgrind and (cmd_opts.gdb or cmd_opts.gdb_stopped):
    print("May not use valgrind with gdb")
    sys.exit(1)

if cmd_opts.strace and (cmd_opts.gdb or cmd_opts.gdb_stopped):
    print("May not use strace with gdb")
    sys.exit(1)

if cmd_opts.strace and cmd_opts.valgrind:
    print("valgrind and strace almost certainly not a good idea")

# magically determine vehicle type (if required):
if cmd_opts.vehicle is None:
    cwd = os.getcwd()
    cmd_opts.vehicle = os.path.basename(cwd)

if cmd_opts.vehicle not in _options:
    # try in parent directories, useful for having config in subdirectories
    cwd = os.getcwd()
    while cwd:
        bname = os.path.basename(cwd)
        if not bname:
            break
        if bname in _options:
            cmd_opts.vehicle = bname
            break
        cwd = os.path.dirname(cwd)

# try to validate vehicle
if cmd_opts.vehicle not in _options:
    progress('''
** Is (%s) really your vehicle type?
Perhaps you could try -v %s
You could also try changing directory to e.g. the ArduCopter subdirectory
''' % (cmd_opts.vehicle, vehicle_options_string))
    sys.exit(1)

# determine frame options (e.g. build type might be "sitl")
if cmd_opts.frame is None:
    cmd_opts.frame = _options[cmd_opts.vehicle]["default_frame"]

# setup ports for this instance
mavlink_port = "tcp:127.0.0.1:" + str(5760 + 10 * cmd_opts.instance)
simout_port = "127.0.0.1:" + str(5501 + 10 * cmd_opts.instance)

frame_infos = options_for_frame(cmd_opts.frame, cmd_opts.vehicle, cmd_opts)

if frame_infos["model"] == "jsbsim":
    check_jsbsim_version()

vehicle_dir = os.path.realpath(os.path.join(find_root_dir(), cmd_opts.vehicle))
if not os.path.exists(vehicle_dir):
    print("vehicle directory (%s) does not exist" % (vehicle_dir,))
    sys.exit(1)

if not cmd_opts.hil:
    if cmd_opts.instance == 0:
        kill_tasks()

if cmd_opts.tracker:
    start_antenna_tracker(find_autotest_dir(), cmd_opts)

if cmd_opts.custom_location:
    location = cmd_opts.custom_location
    progress("Starting up at %s" % (location,))
else:
    location = find_location_by_name(find_autotest_dir(), cmd_opts.location)
    progress("Starting up at %s (%s)" % (location, cmd_opts.location))

if cmd_opts.use_dir is not None:
    new_dir = cmd_opts.use_dir
    try:
        os.makedirs(os.path.realpath(new_dir))
    except OSError as exception:
        if exception.errno != errno.EEXIST:
            raise
    os.chdir(new_dir)

if cmd_opts.hil:
    # (unlikely)
    run_in_terminal_window(find_autotest_dir(), "JSBSim", [os.path.join(find_autotest_dir(), "jsb_sim/runsim.py"), "--home", location, "--speedup=" + str(cmd_opts.speedup)])
else:
    if not cmd_opts.no_rebuild:  # i.e. we should rebuild
        do_build(vehicle_dir, cmd_opts, frame_infos)

    if cmd_opts.build_system == "waf":
        if cmd_opts.debug:
            binary_basedir = "build/sitl-debug"
        else:
            binary_basedir = "build/sitl"
        vehicle_binary = os.path.join(find_root_dir(), binary_basedir, frame_infos["waf_target"])
    else:
        vehicle_binary = os.path.join(vehicle_dir, cmd_opts.vehicle + ".elf")

    if not os.path.exists(vehicle_binary):
        print("Vehicle binary (%s) does not exist" % (vehicle_binary,))
        sys.exit(1)

    start_vehicle(vehicle_binary, find_autotest_dir(), cmd_opts, frame_infos, location)

if cmd_opts.delay_start:
    progress("Sleeping for %f seconds" % (cmd_opts.delay_start,))
    time.sleep(float(cmd_opts.delay_start))

try:
    if cmd_opts.no_mavproxy:
        time.sleep(3)  # Just wait to output the last command after run_in_terminal_window.sh
        progress("Waiting for SITL to exit")
        wait_unlimited()
    else:
        start_mavproxy(cmd_opts, frame_infos)
except KeyboardInterrupt:
    progress("Keyboard Interrupt received ...")

sys.exit(0)