ardupilot/Tools/autotest/rover.py

'''
Drive Rover in SITL

AP_FLAKE8_CLEAN

'''

from __future__ import print_function

import copy
import math
import operator
import os
import sys
import time

from common import AutoTest
from pysim import util

from common import AutoTestTimeoutException
from common import MsgRcvTimeoutException
from common import NotAchievedException
from common import PreconditionFailedException

from pymavlink import mavextra
from pymavlink import mavutil

# get location of scripts
testdir = os.path.dirname(os.path.realpath(__file__))

SITL_START_LOCATION = mavutil.location(40.071374969556928,
                                       -105.22978898137808,
                                       1583.702759,
                                       246)


class AutoTestRover(AutoTest):
    @staticmethod
    def get_not_armable_mode_list():
        return ["RTL", "SMART_RTL"]

    @staticmethod
    def get_not_disarmed_settable_modes_list():
        return ["FOLLOW"]

    @staticmethod
    def get_no_position_not_settable_modes_list():
        return []

    @staticmethod
    def get_position_armable_modes_list():
        return ["GUIDED", "LOITER", "STEERING", "AUTO"]

    @staticmethod
    def get_normal_armable_modes_list():
        return ["ACRO", "HOLD", "MANUAL"]

    def log_name(self):
        return "Rover"

    def test_filepath(self):
        return os.path.realpath(__file__)

    def set_current_test_name(self, name):
        self.current_test_name_directory = "ArduRover_Tests/" + name + "/"

    def sitl_start_location(self):
        return SITL_START_LOCATION

    def default_frame(self):
        return "rover"

    def default_speedup(self):
        return 30

    def is_rover(self):
        return True

    def get_stick_arming_channel(self):
        return int(self.get_parameter("RCMAP_ROLL"))

    ##########################################################
    #   TESTS DRIVE
    ##########################################################
    # Drive a square in manual mode
    def DriveSquare(self, side=50):
        """Learn/Drive Square with Ch7 option"""

        self.context_push()
        ex = None
        try:
            self.progress("TEST SQUARE")
            self.set_parameters({
                "RC7_OPTION": 7,
                "RC9_OPTION": 58,
            })

            self.change_mode('MANUAL')

            self.wait_ready_to_arm()
            self.arm_vehicle()

            self.clear_wp(9)

            # first aim north
            self.progress("\nTurn right towards north")
            self.reach_heading_manual(10)
            # save bottom left corner of box as home AND waypoint
            self.progress("Save HOME")
            self.save_wp()

            self.progress("Save WP")
            self.save_wp()

            # pitch forward to fly north
            self.progress("\nGoing north %u meters" % side)
            self.reach_distance_manual(side)
            # save top left corner of square as waypoint
            self.progress("Save WP")
            self.save_wp()

            # roll right to fly east
            self.progress("\nGoing east %u meters" % side)
            self.reach_heading_manual(100)
            self.reach_distance_manual(side)
            # save top right corner of square as waypoint
            self.progress("Save WP")
            self.save_wp()

            # pitch back to fly south
            self.progress("\nGoing south %u meters" % side)
            self.reach_heading_manual(190)
            self.reach_distance_manual(side)
            # save bottom right corner of square as waypoint
            self.progress("Save WP")
            self.save_wp()

            # roll left to fly west
            self.progress("\nGoing west %u meters" % side)
            self.reach_heading_manual(280)
            self.reach_distance_manual(side)
            # save bottom left corner of square (should be near home) as waypoint
            self.progress("Save WP")
            self.save_wp()

            self.progress("Checking number of saved waypoints")
            mavproxy = self.start_mavproxy()
            num_wp = self.save_mission_to_file_using_mavproxy(
                mavproxy,
                os.path.join(testdir, "ch7_mission.txt"))
            self.stop_mavproxy(mavproxy)
            expected = 7 # home + 6 toggled in
            if num_wp != expected:
                raise NotAchievedException("Did not get %u waypoints; got %u" %
                                           (expected, num_wp))

            # TODO: actually drive the mission

            self.clear_wp(9)
        except Exception as e:
            self.print_exception_caught(e)
            ex = e

        self.disarm_vehicle()
        self.context_pop()

        if ex:
            raise ex

    def drive_left_circuit(self):
        """Drive a left circuit, 50m on a side."""
        self.change_mode('MANUAL')
        self.set_rc(3, 2000)

        self.progress("Driving left circuit")
        # do 4 turns
        for i in range(0, 4):
            # hard left
            self.progress("Starting turn %u" % i)
            self.set_rc(1, 1000)
            self.wait_heading(270 - (90*i), accuracy=10)
            self.set_rc(1, 1500)
            self.progress("Starting leg %u" % i)
            self.wait_distance(50, accuracy=7)
        self.set_rc(3, 1500)
        self.progress("Circuit complete")

    # def test_throttle_failsafe(self, home, distance_min=10, side=60,
    #                            timeout=300):
    #     """Fly east, Failsafe, return, land."""
    #
    #     self.mavproxy.send('switch 6\n')  # manual mode
    #     self.wait_mode('MANUAL')
    #     self.mavproxy.send("param set FS_ACTION 1\n")
    #
    #     # first aim east
    #     self.progress("turn east")
    #     if not self.reach_heading_manual(135):
    #         return False
    #
    #     # fly east 60 meters
    #     self.progress("# Going forward %u meters" % side)
    #     if not self.reach_distance_manual(side):
    #         return False
    #
    #     # pull throttle low
    #     self.progress("# Enter Failsafe")
    #     self.mavproxy.send('rc 3 900\n')
    #
    #     tstart = self.get_sim_time()
    #     success = False
    #     while self.get_sim_time() < tstart + timeout and not success:
    #         m = self.mav.recv_match(type='VFR_HUD', blocking=True)
    #         pos = self.mav.location()
    #         home_distance = self.get_distance(home, pos)
    #         self.progress("Alt: %u  HomeDistance: %.0f" %
    #                       (m.alt, home_distance))
    #         # check if we've reached home
    #         if home_distance <= distance_min:
    #             self.progress("RTL Complete")
    #             success = True
    #
    #     # reduce throttle
    #     self.mavproxy.send('rc 3 1500\n')
    #     self.mavproxy.expect('AP: Failsafe ended')
    #     self.mavproxy.send('switch 2\n')  # manual mode
    #     self.wait_heartbeat()
    #     self.wait_mode('MANUAL')
    #
    #     if success:
    #         self.progress("Reached failsafe home OK")
    #         return True
    #     else:
    #         self.progress("Failed to reach Home on failsafe RTL - "
    #         "timed out after %u seconds" % timeout)
    #         return False

    def Sprayer(self):
        """Test sprayer functionality."""
        self.context_push()
        ex = None
        try:
            rc_ch = 5
            pump_ch = 5
            spinner_ch = 6
            pump_ch_min = 1050
            pump_ch_trim = 1520
            pump_ch_max = 1950
            spinner_ch_min = 975
            spinner_ch_trim = 1510
            spinner_ch_max = 1975

            self.set_parameters({
                "SPRAY_ENABLE": 1,

                "SERVO%u_FUNCTION" % pump_ch: 22,
                "SERVO%u_MIN" % pump_ch: pump_ch_min,
                "SERVO%u_TRIM" % pump_ch: pump_ch_trim,
                "SERVO%u_MAX" % pump_ch: pump_ch_max,

                "SERVO%u_FUNCTION" % spinner_ch: 23,
                "SERVO%u_MIN" % spinner_ch: spinner_ch_min,
                "SERVO%u_TRIM" % spinner_ch: spinner_ch_trim,
                "SERVO%u_MAX" % spinner_ch: spinner_ch_max,

                "SIM_SPR_ENABLE": 1,
                "SIM_SPR_PUMP": pump_ch,
                "SIM_SPR_SPIN": spinner_ch,

                "RC%u_OPTION" % rc_ch: 15,
                "LOG_DISARMED": 1,
            })

            self.reboot_sitl()

            self.wait_ready_to_arm()
            self.arm_vehicle()

            self.progress("test bootup state - it's zero-output!")
            self.wait_servo_channel_value(spinner_ch, 0)
            self.wait_servo_channel_value(pump_ch, 0)

            self.progress("Enable sprayer")
            self.set_rc(rc_ch, 2000)

            self.progress("Testing zero-speed state")
            self.wait_servo_channel_value(spinner_ch, spinner_ch_min)
            self.wait_servo_channel_value(pump_ch, pump_ch_min)

            self.progress("Testing turning it off")
            self.set_rc(rc_ch, 1000)
            self.wait_servo_channel_value(spinner_ch, spinner_ch_min)
            self.wait_servo_channel_value(pump_ch, pump_ch_min)

            self.progress("Testing turning it back on")
            self.set_rc(rc_ch, 2000)
            self.wait_servo_channel_value(spinner_ch, spinner_ch_min)
            self.wait_servo_channel_value(pump_ch, pump_ch_min)

            self.progress("Testing speed-ramping")
            self.set_rc(3, 1700) # start driving forward

            # this is somewhat empirical...
            self.wait_servo_channel_value(pump_ch, 1695, timeout=60)

            self.progress("Turning it off again")
            self.set_rc(rc_ch, 1000)
            self.wait_servo_channel_value(spinner_ch, spinner_ch_min)
            self.wait_servo_channel_value(pump_ch, pump_ch_min)

            self.start_subtest("Sprayer Mission")
            self.load_mission("sprayer-mission.txt")
            self.change_mode("AUTO")
#            self.send_debug_trap()
            self.progress("Waiting for sprayer to start")
            self.wait_servo_channel_value(pump_ch, 1250, timeout=60, comparator=operator.gt)
            self.progress("Waiting for sprayer to stop")
            self.wait_servo_channel_value(pump_ch, pump_ch_min, timeout=120)

            self.start_subtest("Checking mavlink commands")
            self.change_mode("MANUAL")
            self.progress("Starting Sprayer")
            self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SPRAYER,
                         1,  # p1
                         0,  # p2
                         0,  # p3
                         0,  # p4
                         0,  # p5
                         0,  # p6
                         0)  # p7

            self.progress("Testing speed-ramping")
            self.set_rc(3, 1700) # start driving forward
            self.wait_servo_channel_value(pump_ch, 1690, timeout=60, comparator=operator.gt)
            self.start_subtest("Stopping Sprayer")
            self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SPRAYER,
                         0,  # p1
                         0,  # p2
                         0,  # p3
                         0,  # p4
                         0,  # p5
                         0,  # p6
                         0)  # p7
            self.wait_servo_channel_value(pump_ch, pump_ch_min)
            self.set_rc(3, 1000) # start driving forward

            self.progress("Sprayer OK")
        except Exception as e:
            self.print_exception_caught(e)
            ex = e
        self.context_pop()
        self.disarm_vehicle(force=True)
        self.reboot_sitl()
        if ex:
            raise ex

    def DriveMaxRCIN(self, timeout=30):
        """Drive rover at max RC inputs"""
        self.context_push()
        ex = None

        try:
            self.progress("Testing max RC inputs")
            self.change_mode("MANUAL")

            self.wait_ready_to_arm()
            self.arm_vehicle()

            self.set_rc(3, 2000)
            self.set_rc(1, 1000)

            tstart = self.get_sim_time()
            while self.get_sim_time_cached() - tstart < timeout:
                m = self.mav.recv_match(type='VFR_HUD', blocking=True, timeout=1)
                if m is not None:
                    self.progress("Current speed: %f" % m.groundspeed)

            # reduce throttle
            self.set_rc(3, 1500)
            self.set_rc(1, 1500)

        except Exception as e:
            self.print_exception_caught(e)
            ex = e

        self.disarm_vehicle()
        self.context_pop()

        if ex:
            raise ex

    #################################################
    # AUTOTEST ALL
    #################################################
    def drive_mission(self, filename, strict=True):
        """Drive a mission from a file."""
        self.progress("Driving mission %s" % filename)
        wp_count = self.load_mission(filename, strict=strict)
        self.wait_ready_to_arm()
        self.arm_vehicle()
        self.change_mode('AUTO')
        self.wait_waypoint(1, wp_count-1, max_dist=5)
        self.wait_statustext("Mission Complete", timeout=600)
        self.disarm_vehicle()
        self.progress("Mission OK")

    def DriveMission(self):
        '''Drive Mission rover1.txt'''
        self.drive_mission("rover1.txt", strict=False)

    def GripperMission(self):
        '''Test Gripper Mission Items'''
        self.load_mission("rover-gripper-mission.txt")
        self.change_mode('AUTO')
        self.wait_ready_to_arm()
        self.arm_vehicle()
        self.context_collect('STATUSTEXT')
        self.wait_statustext("Gripper Grabbed", timeout=60, check_context=True)
        self.wait_statustext("Gripper Released", timeout=60, check_context=True)
        self.wait_statustext("Mission Complete", timeout=60, check_context=True)
        self.disarm_vehicle()

    def GetBanner(self):
        '''Get Banner'''
        target_sysid = self.sysid_thismav()
        target_compid = 1
        self.mav.mav.command_long_send(
            target_sysid,
            target_compid,
            mavutil.mavlink.MAV_CMD_DO_SEND_BANNER,
            1, # confirmation
            1, # send it
            0,
            0,
            0,
            0,
            0,
            0)
        start = time.time()
        while True:
            m = self.mav.recv_match(type='STATUSTEXT',
                                    blocking=True,
                                    timeout=1)
            if m is not None and "ArduRover" in m.text:
                self.progress("banner received: %s" % m.text)
                return
            if time.time() - start > 10:
                break

        raise MsgRcvTimeoutException("banner not received")

    def drive_brake_get_stopping_distance(self, speed):
        '''measure our stopping distance'''

        self.context_push()

        # controller tends not to meet cruise speed (max of ~14 when 15
        # set), thus *1.2
        # at time of writing, the vehicle is only capable of 10m/s/s accel
        self.set_parameters({
            'CRUISE_SPEED': speed*1.2,
            'ATC_ACCEL_MAX': 15,
        })
        self.change_mode("STEERING")
        self.set_rc(3, 2000)
        self.wait_groundspeed(15, 100)
        initial = self.mav.location()
        initial_time = time.time()
        while time.time() - initial_time < 2:
            # wait for a position update from the autopilot
            start = self.mav.location()
            if start != initial:
                break
        self.set_rc(3, 1500)
        self.wait_groundspeed(0, 0.2)  # why do we not stop?!
        initial = self.mav.location()
        initial_time = time.time()
        while time.time() - initial_time < 2:
            # wait for a position update from the autopilot
            stop = self.mav.location()
            if stop != initial:
                break
        delta = self.get_distance(start, stop)

        self.context_pop()

        return delta

    def DriveBrake(self):
        '''Test braking'''
        self.set_parameters({
            'CRUISE_SPEED': 15,
            'ATC_BRAKE': 0,
        })

        self.arm_vehicle()

        distance_without_brakes = self.drive_brake_get_stopping_distance(15)

        # brakes on:
        self.set_parameter('ATC_BRAKE', 1)
        distance_with_brakes = self.drive_brake_get_stopping_distance(15)

        delta = distance_without_brakes - distance_with_brakes

        self.disarm_vehicle()

        if delta < distance_without_brakes * 0.05:  # 5% isn't asking for much
            raise NotAchievedException("""
Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
""" %
                                       (distance_with_brakes,
                                        distance_without_brakes,
                                        delta))

        self.progress(
            "Brakes work (with=%0.2fm without=%0.2fm delta=%0.2fm)" %
            (distance_with_brakes, distance_without_brakes, delta))

    def drive_rtl_mission_max_distance_from_home(self):
        '''maximum distance allowed from home at end'''
        return 6.5

    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)

        # the EKF doesn't pull us down to 0 speed:
        self.wait_groundspeed(0, 0.5, timeout=600)

        # current Rover blows straight past the home position and ends
        # up ~6m past the home point.
        home_distance = self.distance_to_home()
        home_distance_min = 5.5
        home_distance_max = self.drive_rtl_mission_max_distance_from_home()
        if home_distance > home_distance_max:
            raise NotAchievedException(
                "Did not stop near home (%f metres distant (%f > want > %f))" %
                (home_distance, home_distance_min, home_distance_max))
        self.disarm_vehicle()
        self.progress("RTL Mission OK (%fm)" % home_distance)

    def AC_Avoidance(self):
        '''Test AC Avoidance switch'''
        self.context_push()
        ex = None
        try:
            self.load_fence("rover-fence-ac-avoid.txt")
            self.set_parameters({
                "FENCE_ENABLE": 0,
                "PRX1_TYPE": 10,
                "RC10_OPTION": 40, # proximity-enable
            })
            self.reboot_sitl()
            # start = self.mav.location()
            self.wait_ready_to_arm()
            self.arm_vehicle()
            # first make sure we can breach the fence:
            self.set_rc(10, 1000)
            self.change_mode("ACRO")
            self.set_rc(3, 1550)
            self.wait_distance_to_home(25, 100000, timeout=60)
            self.change_mode("RTL")
            self.wait_statustext("Reached destination", timeout=60)
            # now enable avoidance and make sure we can't:
            self.set_rc(10, 2000)
            self.change_mode("ACRO")
            self.wait_groundspeed(0, 0.7, timeout=60)
            # watch for speed zero
            self.wait_groundspeed(0, 0.2, timeout=120)

        except Exception as e:
            self.print_exception_caught(e)
            ex = e
        self.context_pop()
        self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_FENCE)
        self.disarm_vehicle(force=True)
        self.reboot_sitl()
        if ex:
            raise ex

    def ServoRelayEvents(self):
        '''Test ServoRelayEvents'''
        self.do_set_relay(0, 0)
        off = self.get_parameter("SIM_PIN_MASK")
        self.do_set_relay(0, 1)
        on = self.get_parameter("SIM_PIN_MASK")
        if on == off:
            raise NotAchievedException(
                "Pin mask unchanged after relay cmd")
        self.progress("Pin mask changed after relay command")

    def MAVProxy_SetModeUsingSwitch(self):
        """Set modes via mavproxy switch"""
        port = self.sitl_rcin_port(offset=1)
        self.customise_SITL_commandline([
            "--rc-in-port", str(port),
        ])
        ex = None
        try:
            self.load_mission(self.arming_test_mission())
            self.wait_ready_to_arm()
            fnoo = [(1, 'MANUAL'),
                    (2, 'MANUAL'),
                    (3, 'RTL'),
                    (4, 'AUTO'),
                    (5, 'AUTO'),  # non-existant mode, should stay in RTL
                    (6, 'MANUAL')]
            mavproxy = self.start_mavproxy(sitl_rcin_port=port)
            for (num, expected) in fnoo:
                mavproxy.send('switch %u\n' % num)
                self.wait_mode(expected)
            self.stop_mavproxy(mavproxy)
        except Exception as e:
            self.print_exception_caught(e)
            ex = e

        # if we don't put things back ourselves then the test cleanup
        # doesn't go well as we can't set the RC defaults correctly:
        self.customise_SITL_commandline([
        ])

        if ex is not None:
            raise ex

    def MAVProxy_SetModeUsingMode(self):
        '''Set modes via mavproxy mode command'''
        fnoo = [(1, 'ACRO'),
                (3, 'STEERING'),
                (4, 'HOLD'),
                ]
        mavproxy = self.start_mavproxy()
        for (num, expected) in fnoo:
            mavproxy.send('mode manual\n')
            self.wait_mode("MANUAL")
            mavproxy.send('mode %u\n' % num)
            self.wait_mode(expected)
            mavproxy.send('mode manual\n')
            self.wait_mode("MANUAL")
            mavproxy.send('mode %s\n' % expected)
            self.wait_mode(expected)
        self.stop_mavproxy(mavproxy)

    def ModeSwitch(self):
        ''''Set modes via modeswitch'''
        self.context_push()
        ex = None
        try:
            self.set_parameter("MODE_CH", 8)
            self.set_rc(8, 1000)
            # mavutil.mavlink.ROVER_MODE_HOLD:
            self.set_parameter("MODE6", 4)
            # mavutil.mavlink.ROVER_MODE_ACRO
            self.set_parameter("MODE5", 1)
            self.set_rc(8, 1800) # PWM for mode6
            self.wait_mode("HOLD")
            self.set_rc(8, 1700) # PWM for mode5
            self.wait_mode("ACRO")
            self.set_rc(8, 1800) # PWM for mode6
            self.wait_mode("HOLD")
            self.set_rc(8, 1700) # PWM for mode5
            self.wait_mode("ACRO")
        except Exception as e:
            self.print_exception_caught(e)
            ex = e

        self.context_pop()

        if ex is not None:
            raise ex

    def AuxModeSwitch(self):
        '''Set modes via auxswitches'''
        self.context_push()
        ex = None
        try:
            # from mavproxy_rc.py
            mapping = [0, 1165, 1295, 1425, 1555, 1685, 1815]
            self.set_parameter("MODE1", 1)  # acro
            self.set_rc(8, mapping[1])
            self.wait_mode('ACRO')

            self.set_rc(9, 1000)
            self.set_rc(10, 1000)
            self.set_parameters({
                "RC9_OPTION": 53, # steering
                "RC10_OPTION": 54, # hold
            })
            self.set_rc(9, 1900)
            self.wait_mode("STEERING")
            self.set_rc(10, 1900)
            self.wait_mode("HOLD")

            # reset both switches - should go back to ACRO
            self.set_rc(9, 1000)
            self.set_rc(10, 1000)
            self.wait_mode("ACRO")

            self.set_rc(9, 1900)
            self.wait_mode("STEERING")
            self.set_rc(10, 1900)
            self.wait_mode("HOLD")

            self.set_rc(10, 1000) # this re-polls the mode switch
            self.wait_mode("ACRO")
            self.set_rc(9, 1000)
        except Exception as e:
            self.print_exception_caught(e)
            ex = e

        self.context_pop()

        if ex is not None:
            raise ex

    def RCOverridesCancel(self):
        '''Test RC overrides Cancel'''
        self.set_parameter("SYSID_MYGCS", self.mav.source_system)
        self.change_mode('MANUAL')
        self.wait_ready_to_arm()
        self.zero_throttle()
        self.arm_vehicle()
        # start moving forward a little:
        normal_rc_throttle = 1700
        throttle_override = 1900

        self.progress("Establishing baseline RC input")
        self.set_rc(3, normal_rc_throttle)
        self.drain_mav()
        tstart = self.get_sim_time()
        while True:
            if self.get_sim_time_cached() - tstart > 10:
                raise AutoTestTimeoutException("Did not get rc change")
            m = self.mav.recv_match(type='RC_CHANNELS', blocking=True)
            if m.chan3_raw == normal_rc_throttle:
                break

        self.progress("Set override with RC_CHANNELS_OVERRIDE")
        self.drain_mav()
        tstart = self.get_sim_time()
        while True:
            if self.get_sim_time_cached() - tstart > 10:
                raise AutoTestTimeoutException("Did not override")
            self.progress("Sending throttle of %u" % (throttle_override,))
            self.mav.mav.rc_channels_override_send(
                1, # target system
                1, # targe component
                65535, # chan1_raw
                65535, # chan2_raw
                throttle_override, # chan3_raw
                65535, # chan4_raw
                65535, # chan5_raw
                65535, # chan6_raw
                65535, # chan7_raw
                65535) # chan8_raw

            m = self.mav.recv_match(type='RC_CHANNELS', blocking=True)
            self.progress("chan3=%f want=%f" % (m.chan3_raw, throttle_override))
            if m.chan3_raw == throttle_override:
                break

        self.progress("disabling override and making sure we revert to RC input in good time")
        self.drain_mav()
        tstart = self.get_sim_time()
        while True:
            if self.get_sim_time_cached() - tstart > 0.5:
                raise AutoTestTimeoutException("Did not cancel override")
            self.progress("Sending cancel of throttle override")
            self.mav.mav.rc_channels_override_send(
                1, # target system
                1, # targe component
                65535, # chan1_raw
                65535, # chan2_raw
                0,     # chan3_raw
                65535, # chan4_raw
                65535, # chan5_raw
                65535, # chan6_raw
                65535, # chan7_raw
                65535) # chan8_raw
            self.do_timesync_roundtrip()
            m = self.mav.recv_match(type='RC_CHANNELS', blocking=True)
            self.progress("chan3=%f want=%f" % (m.chan3_raw, normal_rc_throttle))
            if m.chan3_raw == normal_rc_throttle:
                break
        self.disarm_vehicle()

    def RCOverrides(self):
        '''Test RC overrides'''
        self.context_push()
        self.set_parameter("SYSID_MYGCS", self.mav.source_system)
        ex = None
        try:
            self.set_parameter("RC12_OPTION", 46)
            self.reboot_sitl()

            self.change_mode('MANUAL')
            self.wait_ready_to_arm()
            self.set_rc(3, 1500)  # throttle at zero
            self.arm_vehicle()
            # start moving forward a little:
            normal_rc_throttle = 1700
            self.set_rc(3, normal_rc_throttle)
            self.wait_groundspeed(5, 100)

            # allow overrides:
            self.set_rc(12, 2000)

            # now override to stop:
            throttle_override = 1500

            tstart = self.get_sim_time_cached()
            while True:
                if self.get_sim_time_cached() - tstart > 10:
                    raise AutoTestTimeoutException("Did not reach speed")
                self.progress("Sending throttle of %u" % (throttle_override,))
                self.mav.mav.rc_channels_override_send(
                    1, # target system
                    1, # targe component
                    65535, # chan1_raw
                    65535, # chan2_raw
                    throttle_override, # chan3_raw
                    65535, # chan4_raw
                    65535, # chan5_raw
                    65535, # chan6_raw
                    65535, # chan7_raw
                    65535) # chan8_raw

                m = self.mav.recv_match(type='VFR_HUD', blocking=True)
                want_speed = 2.0
                self.progress("Speed=%f want=<%f" % (m.groundspeed, want_speed))
                if m.groundspeed < want_speed:
                    break

            # now override to stop - but set the switch on the RC
            # transmitter to deny overrides; this should send the
            # speed back up to 5 metres/second:
            self.set_rc(12, 1000)

            throttle_override = 1500
            tstart = self.get_sim_time_cached()
            while True:
                if self.get_sim_time_cached() - tstart > 10:
                    raise AutoTestTimeoutException("Did not speed back up")
                self.progress("Sending throttle of %u" % (throttle_override,))
                self.mav.mav.rc_channels_override_send(
                    1, # target system
                    1, # targe component
                    65535, # chan1_raw
                    65535, # chan2_raw
                    throttle_override, # chan3_raw
                    65535, # chan4_raw
                    65535, # chan5_raw
                    65535, # chan6_raw
                    65535, # chan7_raw
                    65535) # chan8_raw

                m = self.mav.recv_match(type='VFR_HUD', blocking=True)
                want_speed = 5.0
                self.progress("Speed=%f want=>%f" % (m.groundspeed, want_speed))

                if m.groundspeed > want_speed:
                    break

            # re-enable RC overrides
            self.set_rc(12, 2000)

            # check we revert to normal RC inputs when gcs overrides cease:
            self.progress("Waiting for RC to revert to normal RC input")
            self.wait_rc_channel_value(3, normal_rc_throttle, timeout=10)

            self.start_subtest("Check override time of zero disables overrides")
            old = self.get_parameter("RC_OVERRIDE_TIME")
            ch = 2
            self.set_rc(ch, 1000)
            channels = [65535] * 18
            ch_override_value = 1700
            channels[ch-1] = ch_override_value
            channels[7] = 1234 # that's channel 8!
            self.progress("Sending override message %u" % ch_override_value)
            self.mav.mav.rc_channels_override_send(
                1, # target system
                1, # targe component
                *channels
            )
            # long timeout required here as we may have sent a lot of
            # things via MAVProxy...
            self.wait_rc_channel_value(ch, ch_override_value, timeout=30)
            self.set_parameter("RC_OVERRIDE_TIME", 0)
            self.wait_rc_channel_value(ch, 1000)
            self.set_parameter("RC_OVERRIDE_TIME", old)
            self.wait_rc_channel_value(ch, ch_override_value)

            ch_override_value = 1720
            channels[ch-1] = ch_override_value
            self.progress("Sending override message %u" % ch_override_value)
            self.mav.mav.rc_channels_override_send(
                1, # target system
                1, # targe component
                *channels
            )
            self.wait_rc_channel_value(ch, ch_override_value, timeout=10)
            self.set_parameter("RC_OVERRIDE_TIME", 0)
            self.wait_rc_channel_value(ch, 1000)
            self.set_parameter("RC_OVERRIDE_TIME", old)

            self.progress("Ensuring timeout works")
            self.wait_rc_channel_value(ch, 1000, timeout=5)
            self.delay_sim_time(10)

            self.set_parameter("RC_OVERRIDE_TIME", 10)
            self.progress("Sending override message")

            ch_override_value = 1730
            channels[ch-1] = ch_override_value
            self.progress("Sending override message %u" % ch_override_value)
            self.mav.mav.rc_channels_override_send(
                1, # target system
                1, # targe component
                *channels
            )
            self.wait_rc_channel_value(ch, ch_override_value, timeout=10)
            tstart = self.get_sim_time()
            self.progress("Waiting for channel to revert to 1000 in ~10s")
            self.wait_rc_channel_value(ch, 1000, timeout=15)
            delta = self.get_sim_time() - tstart
            if delta > 12:
                raise NotAchievedException("Took too long to revert RC channel value (delta=%f)" % delta)
            min_delta = 9
            if delta < min_delta:
                raise NotAchievedException("Didn't take long enough to revert RC channel value (delta=%f want>=%f)" %
                                           (delta, min_delta))
            self.progress("Disabling RC override timeout")
            self.set_parameter("RC_OVERRIDE_TIME", -1)
            ch_override_value = 1740
            channels[ch-1] = ch_override_value
            self.progress("Sending override message %u" % ch_override_value)
            self.mav.mav.rc_channels_override_send(
                1, # target system
                1, # targe component
                *channels
            )
            self.wait_rc_channel_value(ch, ch_override_value, timeout=10)
            tstart = self.get_sim_time()
            while True:
                # warning: this is get_sim_time() and can slurp messages on you!
                delta = self.get_sim_time() - tstart
                if delta > 20:
                    break
                m = self.assert_receive_message('RC_CHANNELS', timeout=1)
                channel_field = "chan%u_raw" % ch
                m_value = getattr(m, channel_field)
                if m_value != ch_override_value:
                    raise NotAchievedException("Value reverted after %f seconds when it should not have (got=%u) (want=%u)" % (delta, m_value, ch_override_value))  # noqa
            self.set_parameter("RC_OVERRIDE_TIME", old)

            self.delay_sim_time(10)

            self.start_subtest("Checking higher-channel semantics")
            self.context_push()
            self.set_parameter("RC_OVERRIDE_TIME", 30)

            ch = 11
            rc_value = 1010
            self.set_rc(ch, rc_value)

            channels = [65535] * 18
            ch_override_value = 1234
            channels[ch-1] = ch_override_value
            self.progress("Sending override message ch%u=%u" % (ch, ch_override_value))
            self.mav.mav.rc_channels_override_send(
                1, # target system
                1, # targe component
                *channels
            )
            self.progress("Wait for override value")
            self.wait_rc_channel_value(ch, ch_override_value, timeout=10)

            self.progress("Sending return-to-RC-input value")
            channels[ch-1] = 65534
            self.mav.mav.rc_channels_override_send(
                1, # target system
                1, # targe component
                *channels
            )
            self.wait_rc_channel_value(ch, rc_value, timeout=10)

            channels[ch-1] = ch_override_value
            self.progress("Sending override message ch%u=%u" % (ch, ch_override_value))
            self.mav.mav.rc_channels_override_send(
                1, # target system
                1, # targe component
                *channels
            )
            self.progress("Wait for override value")
            self.wait_rc_channel_value(ch, ch_override_value, timeout=10)

            # make we keep the override vaue for at least 10 seconds:
            tstart = self.get_sim_time()
            while True:
                if self.get_sim_time_cached() - tstart > 10:
                    break
                # try both ignore values:
                ignore_value = 0
                if self.get_sim_time_cached() - tstart > 5:
                    ignore_value = 65535
                self.progress("Sending ignore value %u" % ignore_value)
                channels[ch-1] = ignore_value
                self.mav.mav.rc_channels_override_send(
                    1, # target system
                    1, # targe component
                    *channels
                )
                if self.get_rc_channel_value(ch) != ch_override_value:
                    raise NotAchievedException("Did not maintain value")

            self.context_pop()

            self.end_subtest("Checking higher-channel semantics")

        except Exception as e:
            self.print_exception_caught(e)
            ex = e

        self.context_pop()
        self.disarm_vehicle()
        self.reboot_sitl()

        if ex is not None:
            raise ex

    def MANUAL_CONTROL(self):
        '''Test mavlink MANUAL_CONTROL'''
        self.context_push()
        self.set_parameter("SYSID_MYGCS", self.mav.source_system)
        ex = None
        try:
            self.set_parameter("RC12_OPTION", 46) # enable/disable rc overrides
            self.reboot_sitl()

            self.change_mode("MANUAL")
            self.wait_ready_to_arm()
            self.zero_throttle()
            self.arm_vehicle()
            self.progress("start moving forward a little")
            normal_rc_throttle = 1700
            self.set_rc(3, normal_rc_throttle)
            self.wait_groundspeed(5, 100)

            self.progress("allow overrides")
            self.set_rc(12, 2000)

            self.progress("now override to stop")
            throttle_override_normalized = 0
            expected_throttle = 0 # in VFR_HUD

            tstart = self.get_sim_time_cached()
            while True:
                if self.get_sim_time_cached() - tstart > 10:
                    raise AutoTestTimeoutException("Did not reach speed")
                self.progress("Sending normalized throttle of %d" % (throttle_override_normalized,))
                self.mav.mav.manual_control_send(
                    1, # target system
                    32767, # x (pitch)
                    32767, # y (roll)
                    throttle_override_normalized, # z (thrust)
                    32767, # r (yaw)
                    0) # button mask

                m = self.mav.recv_match(type='VFR_HUD', blocking=True)
                want_speed = 2.0
                self.progress("Speed=%f want=<%f  throttle=%u want=%u" %
                              (m.groundspeed, want_speed, m.throttle, expected_throttle))
                if m.groundspeed < want_speed and m.throttle == expected_throttle:
                    break

            self.progress("now override to stop - but set the switch on the RC transmitter to deny overrides; this should send the speed back up to 5 metres/second")  # noqa
            self.set_rc(12, 1000)

            throttle_override_normalized = 500
            expected_throttle = 36 # in VFR_HUD, corresponding to normal_rc_throttle adjusted for channel min/max

            tstart = self.get_sim_time_cached()
            while True:
                if self.get_sim_time_cached() - tstart > 10:
                    raise AutoTestTimeoutException("Did not stop")
                self.progress("Sending normalized throttle of %u" % (throttle_override_normalized,))
                self.mav.mav.manual_control_send(
                    1, # target system
                    32767, # x (pitch)
                    32767, # y (roll)
                    throttle_override_normalized, # z (thrust)
                    32767, # r (yaw)
                    0) # button mask

                m = self.mav.recv_match(type='VFR_HUD', blocking=True)
                want_speed = 5.0

                self.progress("Speed=%f want=>%f  throttle=%u want=%u" %
                              (m.groundspeed, want_speed, m.throttle, expected_throttle))
                if m.groundspeed > want_speed and m.throttle == expected_throttle:
                    break

            # re-enable RC overrides
            self.set_rc(12, 2000)

            # check we revert to normal RC inputs when gcs overrides cease:
            self.progress("Waiting for RC to revert to normal RC input")
            self.wait_rc_channel_value(3, normal_rc_throttle, timeout=10)

        except Exception as e:
            self.print_exception_caught(e)
            ex = e

        self.context_pop()
        self.disarm_vehicle()
        self.reboot_sitl()

        if ex is not None:
            raise ex

    def CameraMission(self):
        '''Test Camera Mission Items'''
        self.set_parameter("CAM1_TYPE", 1) # Camera with servo trigger
        self.reboot_sitl() # needed for CAM1_TYPE to take effect
        self.load_mission("rover-camera-mission.txt")
        self.wait_ready_to_arm()
        self.change_mode("AUTO")
        self.wait_ready_to_arm()
        self.arm_vehicle()
        prev_cf = None
        while True:
            cf = self.mav.recv_match(type='CAMERA_FEEDBACK', blocking=True)
            if prev_cf is None:
                prev_cf = cf
                continue
            dist_travelled = self.get_distance_int(prev_cf, cf)
            prev_cf = cf
            mc = self.mav.messages.get("MISSION_CURRENT", None)
            if mc is None:
                continue
            elif mc.seq == 2:
                expected_distance = 2
            elif mc.seq == 4:
                expected_distance = 5
            elif mc.seq == 5:
                break
            else:
                continue
            self.progress("Expected distance %f got %f" %
                          (expected_distance, dist_travelled))
            error = abs(expected_distance - dist_travelled)
            # Rover moves at ~5m/s; we appear to do something at
            # 5Hz, so we do see over a meter of error!
            max_error = 1.5
            if error > max_error:
                raise NotAchievedException("Camera distance error: %f (%f)" %
                                           (error, max_error))

        self.disarm_vehicle()

    def DO_SET_MODE(self):
        '''Set mode via MAV_COMMAND_DO_SET_MODE'''
        self.do_set_mode_via_command_long("HOLD")
        self.do_set_mode_via_command_long("MANUAL")

    def InitialMode(self):
        '''test INITIAL_MODE parameter works'''
        # from mavproxy_rc.py
        self.wait_ready_to_arm()
        mapping = [0, 1165, 1295, 1425, 1555, 1685, 1815]
        mode_ch = 8
        throttle_ch = 3
        self.set_parameter('MODE5', 3)
        self.set_rc(mode_ch, mapping[5])
        self.wait_mode('STEERING')
        self.set_rc(mode_ch, mapping[6])
        self.wait_mode('MANUAL')
        self.set_parameter("INITIAL_MODE", 1) # acro
        # stop the vehicle polling the mode switch at boot:
        self.set_parameter('FS_ACTION', 0)  # do nothing when radio fails
        self.set_rc(throttle_ch, 900)  # RC fail
        self.reboot_sitl()
        self.wait_mode(1)
        self.progress("Make sure we stay in this mode")
        self.delay_sim_time(5)
        self.wait_mode(1)
        # now change modes with a switch:
        self.set_rc(throttle_ch, 1100)
        self.delay_sim_time(3)
        self.set_rc(mode_ch, mapping[5])
        self.wait_mode('STEERING')

    def MAVProxy_DO_SET_MODE(self):
        '''Set mode using MAVProxy commandline DO_SET_MODE'''
        mavproxy = self.start_mavproxy()
        self.mavproxy_do_set_mode_via_command_long(mavproxy, "HOLD")
        self.mavproxy_do_set_mode_via_command_long(mavproxy, "MANUAL")
        self.stop_mavproxy(mavproxy)

    def SYSID_ENFORCE(self):
        '''Test enforcement of SYSID_MYGCS'''
        '''Run the same arming code with correct then incorrect SYSID'''

        if self.mav.source_system != self.mav.mav.srcSystem:
            raise PreconditionFailedException("Expected mav.source_system and mav.srcSystem to match")

        self.context_push()
        old_srcSystem = self.mav.mav.srcSystem
        ex = None
        try:
            self.set_parameter("SYSID_MYGCS", self.mav.source_system)
            self.set_parameter("SYSID_ENFORCE", 1, add_to_context=False)

            self.change_mode('MANUAL')

            self.progress("make sure I can arm ATM")
            self.wait_ready_to_arm()
            self.arm_vehicle(timeout=5)
            self.disarm_vehicle()

            self.do_timesync_roundtrip()

            # should not be able to arm from a system id which is not MY_SYSID
            self.progress("Attempting to arm vehicle from bad system-id")
            success = None
            try:
                # temporarily set a different system ID than normal:
                self.mav.mav.srcSystem = 72
                self.arm_vehicle(timeout=5)
                self.disarm_vehicle()
                success = False
            except AutoTestTimeoutException:
                success = True
            self.mav.mav.srcSystem = old_srcSystem
            if not success:
                raise NotAchievedException("Managed to arm with SYSID_ENFORCE set")

            # should be able to arm from the vehicle's own components:
            self.progress("Attempting to arm vehicle from vehicle component")
            comp_arm_exception = None
            try:
                self.mav.mav.srcSystem = 1
                self.arm_vehicle(timeout=5)
                self.disarm_vehicle()
            except Exception as e:
                comp_arm_exception = e
            self.mav.mav.srcSystem = old_srcSystem
            if comp_arm_exception is not None:
                raise comp_arm_exception

        except Exception as e:
            self.print_exception_caught(e)
            ex = e
        self.mav.mav.srcSystem = old_srcSystem
        self.set_parameter("SYSID_ENFORCE", 0, add_to_context=False)
        self.context_pop()
        if ex is not None:
            raise ex

    def Rally(self):
        '''Test Rally Points'''
        self.load_rally("rover-test-rally.txt")

        self.wait_ready_to_arm()
        self.arm_vehicle()

        # calculate early to avoid round-trips while vehicle is moving:
        accuracy = self.get_parameter("WP_RADIUS")

        self.reach_heading_manual(10)
        self.reach_distance_manual(50)

        self.change_mode("RTL")

        # location copied in from rover-test-rally.txt:
        loc = mavutil.location(40.071553,
                               -105.229401,
                               0,
                               0)

        self.wait_location(loc, accuracy=accuracy, minimum_duration=10, timeout=45)
        self.disarm_vehicle()

    def fence_with_bad_frame(self, target_system=1, target_component=1):
        return [
            self.mav.mav.mission_item_int_encode(
                target_system,
                target_component,
                0, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
                mavutil.mavlink.MAV_CMD_NAV_FENCE_RETURN_POINT,
                0, # current
                0, # autocontinue
                0, # p1
                0, # p2
                0, # p3
                0, # p4
                int(1.0017 * 1e7), # latitude
                int(1.0017 * 1e7), # longitude
                31.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_FENCE),
        ]

    def fence_with_zero_vertex_count(self, target_system=1, target_component=1):
        return [
            self.mav.mav.mission_item_int_encode(
                target_system,
                target_component,
                0, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION,
                0, # current
                0, # autocontinue
                0, # p1
                0, # p2
                0, # p3
                0, # p4
                int(1.0017 * 1e7), # latitude
                int(1.0017 * 1e7), # longitude
                31.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_FENCE),
        ]

    def fence_with_wrong_vertex_count(self, target_system=1, target_component=1):
        return [
            self.mav.mav.mission_item_int_encode(
                target_system,
                target_component,
                0, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION,
                0, # current
                0, # autocontinue
                2, # p1
                0, # p2
                0, # p3
                0, # p4
                int(1.0017 * 1e7), # latitude
                int(1.0017 * 1e7), # longitude
                31.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_FENCE),
        ]

    def fence_with_multiple_return_points(self, target_system=1, target_component=1):
        return [
            self.mav.mav.mission_item_int_encode(
                target_system,
                target_component,
                0, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                mavutil.mavlink.MAV_CMD_NAV_FENCE_RETURN_POINT,
                0, # current
                0, # autocontinue
                0, # p1
                0, # p2
                0, # p3
                0, # p4
                int(1.0017 * 1e7), # latitude
                int(1.0017 * 1e7), # longitude
                31.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_FENCE),
            self.mav.mav.mission_item_int_encode(
                target_system,
                target_component,
                1, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                mavutil.mavlink.MAV_CMD_NAV_FENCE_RETURN_POINT,
                0, # current
                0, # autocontinue
                0, # p1
                0, # p2
                0, # p3
                0, # p4
                int(1.0017 * 1e7), # latitude
                int(1.0017 * 1e7), # longitude
                31.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_FENCE),
        ]

    def fence_with_invalid_latlon(self, target_system=1, target_component=1):
        return [
            self.mav.mav.mission_item_int_encode(
                target_system,
                target_component,
                0, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                mavutil.mavlink.MAV_CMD_NAV_FENCE_RETURN_POINT,
                0, # current
                0, # autocontinue
                0, # p1
                0, # p2
                0, # p3
                0, # p4
                int(100 * 1e7), # bad latitude. bad.
                int(1.0017 * 1e7), # longitude
                31.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_FENCE),
        ]

    def fence_with_multiple_return_points_with_bad_sequence_numbers(self, target_system=1, target_component=1):
        return [
            self.mav.mav.mission_item_int_encode(
                target_system,
                target_component,
                0, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                mavutil.mavlink.MAV_CMD_NAV_FENCE_RETURN_POINT,
                0, # current
                0, # autocontinue
                0, # p1
                0, # p2
                0, # p3
                0, # p4
                int(1.0 * 1e7), # latitude
                int(1.0017 * 1e7), # longitude
                31.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_FENCE),
            self.mav.mav.mission_item_int_encode(
                target_system,
                target_component,
                0, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                mavutil.mavlink.MAV_CMD_NAV_FENCE_RETURN_POINT,
                0, # current
                0, # autocontinue
                0, # p1
                0, # p2
                0, # p3
                0, # p4
                int(2.0 * 1e7), # latitude
                int(2.0017 * 1e7), # longitude
                31.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_FENCE),
        ]

    def fence_which_exceeds_storage_space(self, target_system=1, target_component=1):
        ret = []
        for i in range(0, 60):
            ret.append(self.mav.mav.mission_item_int_encode(
                target_system,
                target_component,
                i, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION,
                0, # current
                0, # autocontinue
                10, # p1
                0, # p2
                0, # p3
                0, # p4
                int(1.0 * 1e7), # latitude
                int(1.0017 * 1e7), # longitude
                31.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_FENCE),
            )
        return ret

    def fences_which_should_not_upload(self, target_system=1, target_component=1):
        return [
            ("Bad Frame", self.fence_with_bad_frame(
                target_system=target_system,
                target_component=target_component)),
            ("Zero Vertex Count", self.fence_with_zero_vertex_count(
                target_system=target_system,
                target_component=target_component)),
            ("Wrong Vertex Count", self.fence_with_wrong_vertex_count(
                target_system=target_system,
                target_component=target_component)),
            ("Multiple return points", self.fence_with_multiple_return_points(
                target_system=target_system,
                target_component=target_component)),
            ("Invalid lat/lon", self.fence_with_invalid_latlon(
                target_system=target_system,
                target_component=target_component)),
            ("Multiple Return points with bad sequence numbers",
                 self.fence_with_multiple_return_points_with_bad_sequence_numbers(  # noqa
                     target_system=target_system,
                     target_component=target_component)),
            ("Fence which exceeds storage space",
             self.fence_which_exceeds_storage_space(
                 target_system=target_system,
                 target_component=target_component)),
        ]

    def fence_with_single_return_point(self, target_system=1, target_component=1):
        return [
            self.mav.mav.mission_item_int_encode(
                target_system,
                target_component,
                0, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                mavutil.mavlink.MAV_CMD_NAV_FENCE_RETURN_POINT,
                0, # current
                0, # autocontinue
                0, # p1
                0, # p2
                0, # p3
                0, # p4
                int(1.0017 * 1e7), # latitude
                int(1.0017 * 1e7), # longitude
                31.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_FENCE),
        ]

    def fence_with_single_return_point_and_5_vertex_inclusion(self,
                                                              target_system=1,
                                                              target_component=1):
        return [
            self.mav.mav.mission_item_int_encode(
                target_system,
                target_component,
                0, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                mavutil.mavlink.MAV_CMD_NAV_FENCE_RETURN_POINT,
                0, # current
                0, # autocontinue
                0, # p1
                0, # p2
                0, # p3
                0, # p4
                int(1.0017 * 1e7), # latitude
                int(1.0017 * 1e7), # longitude
                31.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_FENCE),
            self.mav.mav.mission_item_int_encode(
                target_system,
                target_component,
                1, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION,
                0, # current
                0, # autocontinue
                5, # p1
                0, # p2
                0, # p3
                0, # p4
                int(1.0000 * 1e7), # latitude
                int(1.0000 * 1e7), # longitude
                31.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_FENCE),
            self.mav.mav.mission_item_int_encode(
                target_system,
                target_component,
                2, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION,
                0, # current
                0, # autocontinue
                5, # p1
                0, # p2
                0, # p3
                0, # p4
                int(1.0001 * 1e7), # latitude
                int(1.0000 * 1e7), # longitude
                32.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_FENCE),
            self.mav.mav.mission_item_int_encode(
                target_system,
                target_component,
                3, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION,
                0, # current
                0, # autocontinue
                5, # p1
                0, # p2
                0, # p3
                0, # p4
                int(1.0001 * 1e7), # latitude
                int(1.0001 * 1e7), # longitude
                33.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_FENCE),
            self.mav.mav.mission_item_int_encode(
                target_system,
                target_component,
                4, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION,
                0, # current
                0, # autocontinue
                5, # p1
                0, # p2
                0, # p3
                0, # p4
                int(1.0002 * 1e7), # latitude
                int(1.0002 * 1e7), # longitude
                33.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_FENCE),
            self.mav.mav.mission_item_int_encode(
                target_system,
                target_component,
                5, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION,
                0, # current
                0, # autocontinue
                5, # p1
                0, # p2
                0, # p3
                0, # p4
                int(1.0002 * 1e7), # latitude
                int(1.0003 * 1e7), # longitude
                33.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_FENCE),
        ]

    def fence_with_many_exclusion_circles(self, count=50, target_system=1, target_component=1):
        ret = []
        for i in range(0, count):
            lat_deg = 1.0003 + count/10
            lng_deg = 1.0002 + count/10
            item = self.mav.mav.mission_item_int_encode(
                target_system,
                target_component,
                i, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION,
                0, # current
                0, # autocontinue
                count, # p1
                0, # p2
                0, # p3
                0, # p4
                int(lat_deg * 1e7), # latitude
                int(lng_deg * 1e7), # longitude
                33.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_FENCE)
            ret.append(item)
        return ret

    def fence_with_many_exclusion_polyfences(self, target_system=1, target_component=1):
        ret = []
        seq = 0
        for fencenum in range(0, 4):
            pointcount = fencenum + 6
            for p in range(0, pointcount):
                lat_deg = 1.0003 + p/10 + fencenum/100
                lng_deg = 1.0002 + p/10 + fencenum/100
                item = self.mav.mav.mission_item_int_encode(
                    target_system,
                    target_component,
                    seq, # seq
                    mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                    mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION,
                    0, # current
                    0, # autocontinue
                    pointcount, # p1
                    0, # p2
                    0, # p3
                    0, # p4
                    int(lat_deg * 1e7), # latitude
                    int(lng_deg * 1e7), # longitude
                    33.0000, # altitude
                    mavutil.mavlink.MAV_MISSION_TYPE_FENCE)
                ret.append(item)
                seq += 1
        return ret

    def fences_which_should_upload(self, target_system=1, target_component=1):
        return [
            ("Single Return Point",
             self.fence_with_single_return_point(
                 target_system=target_system,
                 target_component=target_component)),
            ("Return and 5-vertex-inclusion",
             self.fence_with_single_return_point_and_5_vertex_inclusion(
                 target_system=target_system,
                 target_component=target_component)),
            ("Many exclusion circles",
             self.fence_with_many_exclusion_circles(
                 target_system=target_system,
                 target_component=target_component)),
            ("Many exclusion polyfences",
             self.fence_with_many_exclusion_polyfences(
                 target_system=target_system,
                 target_component=target_component)),
            ("Empty fence", []),
        ]

    def assert_fence_does_not_upload(self, fence, target_system=1, target_component=1):
        self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_FENCE,
                           target_system=target_system,
                           target_component=target_component)
        # upload single item using mission item protocol:
        upload_failed = False
        try:
            self.upload_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE,
                                               fence)
        except NotAchievedException:
            # TODO: make sure we failed for correct reason
            upload_failed = True
        if not upload_failed:
            raise NotAchievedException("Uploaded fence when should not be possible")
        self.progress("Fence rightfully bounced")

    def send_fencepoint_expect_statustext(self,
                                          offset,
                                          count,
                                          lat,
                                          lng,
                                          statustext_fragment,
                                          target_system=1,
                                          target_component=1,
                                          timeout=10):
        self.mav.mav.fence_point_send(target_system,
                                      target_component,
                                      offset,
                                      count,
                                      lat,
                                      lng)

        tstart = self.get_sim_time_cached()
        while True:
            if self.get_sim_time_cached() - tstart > timeout:
                raise NotAchievedException("Did not get error message back")
            m = self.mav.recv_match(type='STATUSTEXT', blocking=True, timeout=1)
            self.progress("statustext: %s (want='%s')" %
                          (str(m), statustext_fragment))
            if m is None:
                continue
            if statustext_fragment in m.text:
                break

    def GCSFailsafe(self, side=60, timeout=360):
        """Test GCS Failsafe"""
        try:
            self.test_gcs_failsafe(side=side, timeout=timeout)
        except Exception as ex:
            self.setGCSfailsafe(0)
            self.set_parameter('FS_ACTION', 0)
            self.disarm_vehicle(force=True)
            self.reboot_sitl()
            raise ex

    def test_gcs_failsafe(self, side=60, timeout=360):
        self.set_parameter("SYSID_MYGCS", self.mav.source_system)
        self.set_parameter("FS_ACTION", 1)
        self.set_parameter("FS_THR_ENABLE", 0)  # disable radio FS as it inhibt GCS one's

        def go_somewhere():
            self.change_mode("MANUAL")
            self.wait_ready_to_arm()
            self.arm_vehicle()
            self.set_rc(3, 2000)
            self.delay_sim_time(5)
            self.set_rc(3, 1500)
        # Trigger telemetry loss with failsafe disabled. Verify no action taken.
        self.start_subtest("GCS failsafe disabled test: FS_GCS_ENABLE=0 should take no failsafe action")
        self.setGCSfailsafe(0)
        go_somewhere()
        self.set_heartbeat_rate(0)
        self.delay_sim_time(5)
        self.wait_mode("MANUAL")
        self.set_heartbeat_rate(self.speedup)
        self.delay_sim_time(5)
        self.wait_mode("MANUAL")
        self.end_subtest("Completed GCS failsafe disabled test")

        # Trigger telemetry loss with failsafe enabled. Verify
        # failsafe triggers to RTL. Restore telemetry, verify failsafe
        # clears, and change modes.
        # TODO not implemented
        # self.start_subtest("GCS failsafe recovery test: FS_GCS_ENABLE=1")
        # self.setGCSfailsafe(1)
        # self.set_heartbeat_rate(0)
        # self.wait_mode("RTL")
        # self.set_heartbeat_rate(self.speedup)
        # self.wait_statustext("GCS Failsafe Cleared", timeout=60)
        # self.change_mode("MANUAL")
        # self.end_subtest("Completed GCS failsafe recovery test")

        # Trigger telemetry loss with failsafe enabled. Verify failsafe triggers and RTL completes
        self.start_subtest("GCS failsafe RTL with no options test: FS_GCS_ENABLE=1")
        self.setGCSfailsafe(1)
        self.set_heartbeat_rate(0)
        self.wait_mode("RTL")
        self.wait_statustext("Reached destination", timeout=60)
        self.set_heartbeat_rate(self.speedup)
        self.wait_statustext("GCS Failsafe Cleared", timeout=60)
        self.end_subtest("Completed GCS failsafe RTL")

        # Trigger telemetry loss with an invalid failsafe value. Verify failsafe triggers and RTL completes
        self.start_subtest("GCS failsafe invalid value with no options test: FS_GCS_ENABLE=99")
        self.setGCSfailsafe(99)
        go_somewhere()
        self.set_heartbeat_rate(0)
        self.wait_mode("RTL")
        self.wait_statustext("Reached destination", timeout=60)
        self.set_heartbeat_rate(self.speedup)
        self.wait_statustext("GCS Failsafe Cleared", timeout=60)
        self.end_subtest("Completed GCS failsafe invalid value")

        self.start_subtest("Testing continue in auto mission")
        self.disarm_vehicle()
        self.setGCSfailsafe(2)
        self.load_mission("test_arming.txt")
        self.change_mode("AUTO")
        self.delay_sim_time(5)
        self.set_heartbeat_rate(0)
        self.wait_statustext("Failsafe - Continuing Auto Mode", timeout=60)
        self.delay_sim_time(5)
        self.wait_mode("AUTO")
        self.set_heartbeat_rate(self.speedup)
        self.wait_statustext("GCS Failsafe Cleared", timeout=60)

        self.start_subtest("GCS failsafe RTL with no options test: FS_GCS_ENABLE=1 and FS_GCS_TIMEOUT=10")
        self.setGCSfailsafe(1)
        old_gcs_timeout = self.get_parameter("FS_GCS_TIMEOUT")
        new_gcs_timeout = old_gcs_timeout * 2
        self.set_parameter("FS_GCS_TIMEOUT", new_gcs_timeout)
        go_somewhere()
        self.set_heartbeat_rate(0)
        self.delay_sim_time(old_gcs_timeout + (new_gcs_timeout - old_gcs_timeout) / 2)
        self.assert_mode("MANUAL")
        self.wait_mode("RTL")
        self.wait_statustext("Reached destination", timeout=60)
        self.set_heartbeat_rate(self.speedup)
        self.wait_statustext("GCS Failsafe Cleared", timeout=60)
        self.disarm_vehicle()
        self.end_subtest("Completed GCS failsafe RTL")

        self.setGCSfailsafe(0)
        self.progress("All GCS failsafe tests complete")

    def test_gcs_fence_centroid(self, target_system=1, target_component=1):
        self.start_subtest("Ensuring if we don't have a centroid it gets calculated")
        items = self.test_gcs_fence_need_centroid(
            target_system=target_system,
            target_component=target_component)
        self.upload_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE,
                                           items)
        centroid = self.get_fence_point(0)
        want_lat = 1.0001
        want_lng = 1.00005
        if abs(centroid.lat - want_lat) > 0.000001:
            raise NotAchievedException("Centroid lat not as expected (want=%f got=%f)" % (want_lat, centroid.lat))
        if abs(centroid.lng - want_lng) > 0.000001:
            raise NotAchievedException("Centroid lng not as expected (want=%f got=%f)" % (want_lng, centroid.lng))

    def test_gcs_fence_update_fencepoint(self, target_system=1, target_component=1):
        self.start_subtest("Ensuring we can move a fencepoint")
        items = self.test_gcs_fence_boring_triangle(
            target_system=target_system,
            target_component=target_component)
        self.upload_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE,
                                           items)
#        downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE)
        item_seq = 2
        item = items[item_seq]
        print("item is (%s)" % str(item))
        self.progress("original x=%d" % item.x)
        item.x += int(0.1 * 1e7)
        self.progress("new x=%d" % item.x)
        self.progress("try to overwrite item %u" % item_seq)
        self.mav.mav.mission_write_partial_list_send(
            target_system,
            target_component,
            item_seq,
            item_seq,
            mavutil.mavlink.MAV_MISSION_TYPE_FENCE)
        self.assert_receive_mission_item_request(mavutil.mavlink.MAV_MISSION_TYPE_FENCE, item_seq)
        item.pack(self.mav.mav)
        self.mav.mav.send(item)
        self.progress("Answered request for fence point %u" % item_seq)

        self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_FENCE)
        downloaded_items2 = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE)
        if downloaded_items2[item_seq].x != item.x:
            raise NotAchievedException("Item did not update")
        self.check_fence_items_same([items[0], items[1], item, items[3]], downloaded_items2)

    def test_gcs_fence_boring_triangle(self, target_system=1, target_component=1):
        return copy.copy([
            self.mav.mav.mission_item_int_encode(
                target_system,
                target_component,
                0, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION,
                0, # current
                0, # autocontinue
                3, # p1
                0, # p2
                0, # p3
                0, # p4
                int(1.0000 * 1e7), # latitude
                int(1.0000 * 1e7), # longitude
                31.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_FENCE),
            self.mav.mav.mission_item_int_encode(
                target_system,
                target_component,
                1, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION,
                0, # current
                0, # autocontinue
                3, # p1
                0, # p2
                0, # p3
                0, # p4
                int(1.0001 * 1e7), # latitude
                int(1.0000 * 1e7), # longitude
                32.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_FENCE),
            self.mav.mav.mission_item_int_encode(
                target_system,
                target_component,
                2, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION,
                0, # current
                0, # autocontinue
                3, # p1
                0, # p2
                0, # p3
                0, # p4
                int(1.0001 * 1e7), # latitude
                int(1.0001 * 1e7), # longitude
                33.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_FENCE),
            self.mav.mav.mission_item_int_encode(
                target_system,
                target_component,
                3, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                mavutil.mavlink.MAV_CMD_NAV_FENCE_RETURN_POINT,
                0, # current
                0, # autocontinue
                0, # p1
                0, # p2
                0, # p3
                0, # p4
                int(1.00015 * 1e7), # latitude
                int(1.00015 * 1e7), # longitude
                33.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_FENCE),
        ])

    def test_gcs_fence_need_centroid(self, target_system=1, target_component=1):
        return copy.copy([
            self.mav.mav.mission_item_int_encode(
                target_system,
                target_component,
                0, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION,
                0, # current
                0, # autocontinue
                4, # p1
                0, # p2
                0, # p3
                0, # p4
                int(1.0000 * 1e7), # latitude
                int(1.0000 * 1e7), # longitude
                31.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_FENCE),
            self.mav.mav.mission_item_int_encode(
                target_system,
                target_component,
                1, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION,
                0, # current
                0, # autocontinue
                4, # p1
                0, # p2
                0, # p3
                0, # p4
                int(1.0002 * 1e7), # latitude
                int(1.0000 * 1e7), # longitude
                32.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_FENCE),
            self.mav.mav.mission_item_int_encode(
                target_system,
                target_component,
                2, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION,
                0, # current
                0, # autocontinue
                4, # p1
                0, # p2
                0, # p3
                0, # p4
                int(1.0002 * 1e7), # latitude
                int(1.0001 * 1e7), # longitude
                33.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_FENCE),
            self.mav.mav.mission_item_int_encode(
                target_system,
                target_component,
                3, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION,
                0, # current
                0, # autocontinue
                4, # p1
                0, # p2
                0, # p3
                0, # p4
                int(1.0000 * 1e7), # latitude
                int(1.0001 * 1e7), # longitude
                33.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_FENCE),
        ])

    def click_location_from_item(self, mavproxy, item):
        mavproxy.send("click %f %f\n" % (item.x*1e-7, item.y*1e-7))

    def test_gcs_fence_via_mavproxy(self, target_system=1, target_component=1):
        self.start_subtest("Fence via MAVProxy")
        if not self.mavproxy_can_do_mision_item_protocols():
            return
        mavproxy = self.start_mavproxy()
        self.start_subsubtest("fence addcircle")
        self.clear_fence_using_mavproxy(mavproxy)
        self.delay_sim_time(1)
        radius = 20
        item = self.mav.mav.mission_item_int_encode(
            target_system,
            target_component,
            0, # seq
            mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
            mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION,
            0, # current
            0, # autocontinue
            radius, # p1
            0, # p2
            0, # p3
            0, # p4
            int(1.0017 * 1e7), # latitude
            int(1.0017 * 1e7), # longitude
            0.0000, # altitude
            mavutil.mavlink.MAV_MISSION_TYPE_FENCE)
        print("item is (%s)" % str(item))
        self.click_location_from_item(mavproxy, item)
        mavproxy.send("fence addcircle inc %u\n" % radius)
        self.delay_sim_time(1)
        downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE)
        print("downloaded items: %s" % str(downloaded_items))
        self.check_fence_items_same([item], downloaded_items)

        radius_exc = 57.3
        item2 = self.mav.mav.mission_item_int_encode(
            target_system,
            target_component,
            0, # seq
            mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
            mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION,
            0, # current
            0, # autocontinue
            radius_exc, # p1
            0, # p2
            0, # p3
            0, # p4
            int(1.0017 * 1e7), # latitude
            int(1.0017 * 1e7), # longitude
            0.0000, # altitude
            mavutil.mavlink.MAV_MISSION_TYPE_FENCE)
        self.click_location_from_item(mavproxy, item2)
        mavproxy.send("fence addcircle exc %f\n" % radius_exc)
        self.delay_sim_time(1)
        downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE)
        print("downloaded items: %s" % str(downloaded_items))
        self.check_fence_items_same([item, item2], downloaded_items)
        self.end_subsubtest("fence addcircle")

        self.start_subsubtest("fence addpoly")
        self.clear_fence_using_mavproxy(mavproxy)
        self.delay_sim_time(1)
        pointcount = 7
        mavproxy.send("fence addpoly inc 20 %u 37.2\n" % pointcount) # radius, pointcount, rotaiton
        self.delay_sim_time(5)
        downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE)
        if len(downloaded_items) != pointcount:
            raise NotAchievedException("Did not get expected number of points returned (want=%u got=%u)" %
                                       (pointcount, len(downloaded_items)))
        self.end_subsubtest("fence addpoly")

        self.start_subsubtest("fence movepolypoint")
        self.clear_fence_using_mavproxy(mavproxy)
        self.delay_sim_time(1)
        triangle = self.test_gcs_fence_boring_triangle(
            target_system=target_system,
            target_component=target_component)
        self.upload_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE,
                                           triangle)
        mavproxy.send("fence list\n")
        self.delay_sim_time(1)
        triangle[2].x += 500
        triangle[2].y += 700
        self.click_location_from_item(mavproxy, triangle[2])
        mavproxy.send("fence movepolypoint 0 2\n")
        self.delay_sim_time(10)
        downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE)
        self.check_fence_items_same(triangle, downloaded_items)
        self.end_subsubtest("fence movepolypoint")

        self.start_subsubtest("fence enable and disable")
        mavproxy.send("fence enable\n")
        mavproxy.expect("fence enabled")
        mavproxy.send("fence disable\n")
        mavproxy.expect("fence disabled")
        self.end_subsubtest("fence enable and disable")

        self.stop_mavproxy(mavproxy)

#        MANUAL> usage: fence <addcircle|addpoly|changealt|clear|disable|draw|enable|list|load|move|movemulti|movepolypoint|param|remove|save|savecsv|savelocal|show|status|undo|update>  # noqa

    def GCSFence(self):
        '''Upload and download of fence'''
        target_system = 1
        target_component = 1

        self.progress("Testing FENCE_POINT protocol")

        self.start_subtest("FENCE_TOTAL manipulation")
        self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_FENCE)
        self.assert_parameter_value("FENCE_TOTAL", 0)

        self.set_parameter("FENCE_TOTAL", 5)
        self.assert_parameter_value("FENCE_TOTAL", 5)

        self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_FENCE)
        self.assert_parameter_value("FENCE_TOTAL", 0)

        self.progress("sending out-of-range fencepoint")
        self.send_fencepoint_expect_statustext(0,
                                               0,
                                               1.2345,
                                               5.4321,
                                               "index past total",
                                               target_system=target_component,
                                               target_component=target_component)

        self.progress("sending another out-of-range fencepoint")
        self.send_fencepoint_expect_statustext(0,
                                               1,
                                               1.2345,
                                               5.4321,
                                               "bad count",
                                               target_system=target_component,
                                               target_component=target_component)

        self.set_parameter("FENCE_TOTAL", 1)
        self.assert_parameter_value("FENCE_TOTAL", 1)

        self.send_fencepoint_expect_statustext(0,
                                               1,
                                               1.2345,
                                               5.4321,
                                               "Invalid FENCE_TOTAL",
                                               target_system=target_component,
                                               target_component=target_component)

        self.set_parameter("FENCE_TOTAL", 5)
        self.progress("Checking default points")
        for i in range(5):
            m = self.get_fence_point(i)
            if m.count != 5:
                raise NotAchievedException("Unexpected count in fence point (want=%u got=%u" %
                                           (5, m.count))
            if m.lat != 0 or m.lng != 0:
                raise NotAchievedException("Unexpected lat/lon in fencepoint")

        self.progress("Storing a return point")
        self.roundtrip_fencepoint_protocol(0,
                                           5,
                                           1.2345,
                                           5.4321,
                                           target_system=target_system,
                                           target_component=target_component)

        lat = 2.345
        lng = 4.321
        self.roundtrip_fencepoint_protocol(0,
                                           5,
                                           lat,
                                           lng,
                                           target_system=target_system,
                                           target_component=target_component)

        if not self.mavproxy_can_do_mision_item_protocols():
            self.progress("MAVProxy too old to do fence point protocols")
            return

        self.progress("Download with new protocol")
        items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE)
        if len(items) != 1:
            raise NotAchievedException("Unexpected fencepoint count (want=%u got=%u)" % (1, len(items)))
        if items[0].command != mavutil.mavlink.MAV_CMD_NAV_FENCE_RETURN_POINT:
            raise NotAchievedException(
                "Fence return point not of correct type expected (%u) got %u" %
                (items[0].command,
                 mavutil.mavlink.MAV_CMD_NAV_FENCE_RETURN_POINT))
        if items[0].frame != mavutil.mavlink.MAV_FRAME_GLOBAL:
            raise NotAchievedException(
                "Unexpected frame want=%s got=%s," %
                (self.string_for_frame(mavutil.mavlink.MAV_FRAME_GLOBAL),
                 self.string_for_frame(items[0].frame)))
        got_lat = items[0].x
        want_lat = lat * 1e7
        if abs(got_lat - want_lat) > 1:
            raise NotAchievedException("Disagree in lat (got=%f want=%f)" % (got_lat, want_lat))
        if abs(items[0].y - lng * 1e7) > 1:
            raise NotAchievedException("Disagree in lng")
        if items[0].seq != 0:
            raise NotAchievedException("Disagree in offset")
        self.progress("Downloaded with new protocol OK")

        # upload using mission protocol:
        items = self.test_gcs_fence_boring_triangle(
            target_system=target_system,
            target_component=target_component)
        self.upload_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE,
                                           items)

        self.progress("Download with new protocol")
        downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE)
        if len(downloaded_items) != len(items):
            raise NotAchievedException("Did not download expected number of items (wanted=%u got=%u)" %
                                       (len(items), len(downloaded_items)))
        self.assert_parameter_value("FENCE_TOTAL", len(items) + 1)  # +1 for closing
        self.progress("Ensuring fence items match what we sent up")
        self.check_fence_items_same(items, downloaded_items)

        # now check centroid
        self.progress("Requesting fence return point")
        self.mav.mav.fence_fetch_point_send(target_system,
                                            target_component,
                                            0)
        m = self.mav.recv_match(type="FENCE_POINT", blocking=True, timeout=1)
        print("m: %s" % str(m))

        self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_FENCE,
                           target_system=target_system,
                           target_component=target_component)
        self.progress("Checking count post-nuke")
        self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_FENCE,
                           target_system=target_system,
                           target_component=target_component)
        self.assert_mission_count_on_link(self.mav,
                                          0,
                                          target_system,
                                          target_component,
                                          mavutil.mavlink.MAV_MISSION_TYPE_FENCE)

        self.start_subtest("Ensuring bad fences get bounced")
        for fence in self.fences_which_should_not_upload(target_system=target_system, target_component=target_component):
            (name, items) = fence
            self.progress("Ensuring (%s) gets bounced" % (name,))
            self.assert_fence_does_not_upload(items)

        self.start_subtest("Ensuring good fences don't get bounced")
        for fence in self.fences_which_should_upload(target_system=target_system, target_component=target_component):
            (name, items) = fence
            self.progress("Ensuring (%s) gets uploaded" % (name,))
            self.check_fence_upload_download(items)
            self.progress("(%s) uploaded just fine" % (name,))

        self.test_gcs_fence_update_fencepoint(target_system=target_system,
                                              target_component=target_component)

        self.test_gcs_fence_centroid(target_system=target_system,
                                     target_component=target_component)

        self.test_gcs_fence_via_mavproxy(target_system=target_system,
                                         target_component=target_component)

    # explode the write_type_to_storage method
    # FIXME: test converting invalid fences / minimally valid fences / normal fences
    # FIXME: show that uploading smaller items take up less space
    # FIXME: add test for consecutive breaches within the manual recovery period
    # FIXME: ensure truncation does the right thing by fence_total

    # FIXME: test vehicle escape from outside inclusion zones to
    # inside inclusion zones (and inside exclusion zones to outside
    # exclusion zones)
    # FIXME: add test that a fence with edges that cross can't be uploaded
    # FIXME: add a test that fences enclose an area (e.g. all the points aren't the same value!
    def Offboard(self, timeout=90):
        '''Test Offboard Control'''
        self.load_mission("rover-guided-mission.txt")
        self.wait_ready_to_arm(require_absolute=True)
        self.arm_vehicle()
        self.change_mode("AUTO")

        offboard_expected_duration = 10 # see mission file

        if self.mav.messages.get("SET_POSITION_TARGET_GLOBAL_INT", None):
            raise PreconditionFailedException("Already have SET_POSITION_TARGET_GLOBAL_INT")

        tstart = self.get_sim_time_cached()
        last_heartbeat_sent = 0
        got_ptgi = False
        magic_waypoint_tstart = 0
        magic_waypoint_tstop = 0
        while True:
            now = self.get_sim_time_cached()
            if now - last_heartbeat_sent > 1:
                last_heartbeat_sent = now
                self.mav.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_ONBOARD_CONTROLLER,
                                            mavutil.mavlink.MAV_AUTOPILOT_INVALID,
                                            0,
                                            0,
                                            0)

            if now - tstart > timeout:
                raise AutoTestTimeoutException("Didn't complete")
            magic_waypoint = 3
            mc = self.mav.recv_match(type=["MISSION_CURRENT", "STATUSTEXT"],
                                     blocking=False)
            if mc is not None:
                print("%s" % str(mc))
                if mc.get_type() == "STATUSTEXT":
                    if "Mission Complete" in mc.text:
                        break
                    continue
                if mc.seq == magic_waypoint:
                    print("At magic waypoint")
                    if magic_waypoint_tstart == 0:
                        magic_waypoint_tstart = self.get_sim_time_cached()
                    ptgi = self.mav.messages.get("POSITION_TARGET_GLOBAL_INT", None)
                    if ptgi is not None:
                        got_ptgi = True
                elif mc.seq > magic_waypoint:
                    if magic_waypoint_tstop == 0:
                        magic_waypoint_tstop = self.get_sim_time_cached()

        self.disarm_vehicle()
        offboard_duration = magic_waypoint_tstop - magic_waypoint_tstart
        if abs(offboard_duration - offboard_expected_duration) > 1:
            raise NotAchievedException("Did not stay in offboard control for correct time (want=%f got=%f)" %
                                       (offboard_expected_duration, offboard_duration))

        if not got_ptgi:
            raise NotAchievedException("Did not get ptgi message")
        print("pgti: %s" % str(ptgi))

    def assert_mission_count_on_link(self, mav, expected_count, target_system, target_component, mission_type):
        self.drain_mav_unparsed(mav=mav, freshen_sim_time=True)
        self.progress("waiting for a message - any message....")
        m = mav.recv_match(blocking=True, timeout=1)
        self.progress("Received (%s)" % str(m))

        if not mav.mavlink20():
            raise NotAchievedException("Not doing mavlink2")
        mav.mav.mission_request_list_send(target_system,
                                          target_component,
                                          mission_type)
        self.assert_receive_mission_count_on_link(mav,
                                                  expected_count,
                                                  target_system,
                                                  target_component,
                                                  mission_type)

    def assert_receive_mission_count_on_link(self,
                                             mav,
                                             expected_count,
                                             target_system,
                                             target_component,
                                             mission_type,
                                             expected_target_system=None,
                                             expected_target_component=None,
                                             timeout=120):
        if expected_target_system is None:
            expected_target_system = mav.mav.srcSystem
        if expected_target_component is None:
            expected_target_component = mav.mav.srcComponent
        self.progress("Waiting for mission count of (%u) from (%u:%u) to (%u:%u)" %
                      (expected_count, target_system, target_component, expected_target_system, expected_target_component))

        tstart = self.get_sim_time_cached()
        while True:
            delta = self.get_sim_time_cached() - tstart
            if delta > timeout:
                raise NotAchievedException("Did not receive MISSION_COUNT on link after %fs" % delta)
            m = mav.recv_match(blocking=True, timeout=1)
            if m is None:
                self.progress("No messages")
                continue
#            self.progress("Received (%s)" % str(m))
            if m.get_type() == "MISSION_ACK":
                if m.type != mavutil.mavlink.MAV_MISSION_ACCEPTED:
                    raise NotAchievedException("Expected MAV_MISSION_ACCEPTED, got (%s)" % m)
            if m.get_type() == "MISSION_COUNT":
                break
        if m.target_system != expected_target_system:
            raise NotAchievedException("Incorrect target system in MISSION_COUNT (want=%u got=%u)" %
                                       (expected_target_system, m.target_system))
        if m.target_component != expected_target_component:
            raise NotAchievedException("Incorrect target component in MISSION_COUNT")
        if m.mission_type != mission_type:
            raise NotAchievedException("Did not get expected mission type (want=%u got=%u)" % (mission_type, m.mission_type))
        if m.count != expected_count:
            raise NotAchievedException("Bad count received (want=%u got=%u)" %
                                       (expected_count, m.count))
        self.progress("Asserted mission count (type=%u) is %u after %fs" % (
            (mission_type, m.count, delta)))

    def get_mission_item_int_on_link(self, item, mav, target_system, target_component, mission_type, delay_fn=None):
        self.drain_mav(mav=mav, unparsed=True)
        mav.mav.mission_request_int_send(target_system,
                                         target_component,
                                         item,
                                         mission_type)
        m = self.assert_receive_message(
            'MISSION_ITEM_INT',
            timeout=60,
            condition='MISSION_ITEM_INT.mission_type==%u' % mission_type,
            delay_fn=delay_fn)
        if m is None:
            raise NotAchievedException("Did not receive MISSION_ITEM_INT")
        if m.mission_type != mission_type:
            raise NotAchievedException("Mission item of incorrect type")
        if m.target_system != mav.mav.srcSystem:
            raise NotAchievedException("Unexpected target system %u want=%u" %
                                       (m.target_system, mav.mav.srcSystem))
        if m.seq != item:
            raise NotAchievedException(
                "Incorrect sequence number on received item got=%u want=%u" %
                (m.seq, item))
        if m.mission_type != mission_type:
            raise NotAchievedException(
                "Mission type incorrect on received item (want=%u got=%u)" %
                (mission_type, m.mission_type))
        if m.target_component != mav.mav.srcComponent:
            raise NotAchievedException(
                "Unexpected target component %u want=%u" %
                (m.target_component, mav.mav.srcComponent))
        return m

    def get_mission_item_on_link(self, item, mav, target_system, target_component, mission_type):
        self.drain_mav(mav=mav, unparsed=True)
        mav.mav.mission_request_send(target_system,
                                     target_component,
                                     item,
                                     mission_type)
        m = mav.recv_match(type='MISSION_ITEM',
                           blocking=True,
                           timeout=60)
        if m is None:
            raise NotAchievedException("Did not receive MISSION_ITEM")
        if m.target_system != mav.mav.srcSystem:
            raise NotAchievedException("Unexpected target system %u want=%u" %
                                       (m.target_system, mav.mav.srcSystem))
        if m.seq != item:
            raise NotAchievedException("Incorrect sequence number on received item_int got=%u want=%u" %
                                       (m.seq, item))
        if m.mission_type != mission_type:
            raise NotAchievedException("Mission type incorrect on received item_int (want=%u got=%u)" %
                                       (mission_type, m.mission_type))
        if m.target_component != mav.mav.srcComponent:
            raise NotAchievedException("Unexpected target component %u want=%u" %
                                       (m.target_component, mav.mav.srcComponent))
        return m

    def assert_receive_mission_item_request(self, mission_type, seq):
        self.progress("Expecting request for item %u" % seq)
        m = self.assert_receive_message('MISSION_REQUEST', timeout=1)
        if m.mission_type != mission_type:
            raise NotAchievedException("Incorrect mission type (wanted=%u got=%u)" %
                                       (mission_type, m.mission_type))
        if m.seq != seq:
            raise NotAchievedException("Unexpected sequence number (want=%u got=%u)" % (seq, m.seq))
        self.progress("Received item request OK")

    def assert_receive_mission_ack(self, mission_type,
                                   want_type=mavutil.mavlink.MAV_MISSION_ACCEPTED,
                                   target_system=None,
                                   target_component=None,
                                   mav=None):
        if mav is None:
            mav = self.mav
        if target_system is None:
            target_system = mav.mav.srcSystem
        if target_component is None:
            target_component = mav.mav.srcComponent
        self.progress("Expecting mission ack")
        m = mav.recv_match(type='MISSION_ACK',
                           blocking=True,
                           timeout=5)
        self.progress("Received ACK (%s)" % str(m))
        if m is None:
            raise NotAchievedException("Expected mission ACK")
        if m.target_system != target_system:
            raise NotAchievedException("ACK not targetted at correct system want=%u got=%u" %
                                       (target_system, m.target_system))
        if m.target_component != target_component:
            raise NotAchievedException("ACK not targetted at correct component")
        if m.mission_type != mission_type:
            raise NotAchievedException("Unexpected mission type %u want=%u" %
                                       (m.mission_type, mission_type))
        if m.type != want_type:
            raise NotAchievedException("Expected ack type got %u got %u" %
                                       (want_type, m.type))

    def assert_filepath_content(self, filepath, want):
        with open(filepath) as f:
            got = f.read()
        if want != got:
            raise NotAchievedException("Did not get expected file content (want=%s) (got=%s)" % (want, got))

    def mavproxy_can_do_mision_item_protocols(self):
        return False
        if not self.mavproxy_version_gt(1, 8, 12):
            self.progress("MAVProxy is too old; skipping tests")
            return False
        return True

    def check_rally_items_same(self, want, got, epsilon=None):
        check_atts = ['mission_type', 'command', 'x', 'y', 'z', 'seq', 'param1']
        return self.check_mission_items_same(check_atts, want, got, epsilon=epsilon)

    def click_three_in(self, mavproxy, target_system=1, target_component=1):
        mavproxy.send('rally clear\n')
        self.drain_mav()
        # there are race conditions in MAVProxy.  Beware.
        mavproxy.send("click 1.0 1.0\n")
        mavproxy.send("rally add\n")
        self.delay_sim_time(1)
        mavproxy.send("click 2.0 2.0\n")
        mavproxy.send("rally add\n")
        self.delay_sim_time(1)
        mavproxy.send("click 3.0 3.0\n")
        mavproxy.send("rally add\n")
        self.delay_sim_time(10)
        self.assert_mission_count_on_link(
            self.mav,
            3,
            target_system,
            target_component,
            mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
        )

    def GCSRally(self, target_system=1, target_component=1):
        '''Upload and download of rally using MAVProxy'''
        self.start_subtest("Testing mavproxy CLI for rally points")
        if not self.mavproxy_can_do_mision_item_protocols():
            return

        mavproxy = self.start_mavproxy()

        mavproxy.send('rally clear\n')

        self.start_subsubtest("rally add")
        mavproxy.send('rally clear\n')
        lat_s = "-5.6789"
        lng_s = "98.2341"
        lat = float(lat_s)
        lng = float(lng_s)
        mavproxy.send('click %s %s\n' % (lat_s, lng_s))
        self.drain_mav()
        mavproxy.send('rally add\n')
        self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
                                        target_system=255,
                                        target_component=0)
        self.delay_sim_time(5)
        downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
        if len(downloaded_items) != 1:
            raise NotAchievedException("Unexpected count (got=%u want=1)" %
                                       (len(downloaded_items), ))
        if (downloaded_items[0].x - int(lat * 1e7)) > 1:
            raise NotAchievedException("Bad rally lat.  Want=%d got=%d" %
                                       (int(lat * 1e7), downloaded_items[0].x))
        if (downloaded_items[0].y - int(lng * 1e7)) > 1:
            raise NotAchievedException("Bad rally lng.  Want=%d got=%d" %
                                       (int(lng * 1e7), downloaded_items[0].y))
        if (downloaded_items[0].z - int(90)) > 1:
            raise NotAchievedException("Bad rally alt.  Want=90 got=%d" %
                                       (downloaded_items[0].y))
        self.end_subsubtest("rally add")

        self.start_subsubtest("rally list")
        util.pexpect_drain(mavproxy)
        mavproxy.send('rally list\n')
        mavproxy.expect(r"Saved 1 rally items to ([^\s]*)\s")
        filename = mavproxy.match.group(1)
        self.assert_rally_filepath_content(filename, '''QGC WPL 110
0	0	3	5100	0.000000	0.000000	0.000000	0.000000	-5.678900	98.234100	90.000000	0
''')
        self.end_subsubtest("rally list")

        self.start_subsubtest("rally save")
        util.pexpect_drain(mavproxy)
        save_tmppath = self.buildlogs_path("rally-testing-tmp.txt")
        mavproxy.send('rally save %s\n' % save_tmppath)
        mavproxy.expect(r"Saved 1 rally items to ([^\s]*)\s")
        filename = mavproxy.match.group(1)
        if filename != save_tmppath:
            raise NotAchievedException("Bad save filepath; want=%s got=%s" % (save_tmppath, filename))
        self.assert_rally_filepath_content(filename, '''QGC WPL 110
0	0	3	5100	0.000000	0.000000	0.000000	0.000000	-5.678900	98.234100	90.000000	0
''')
        self.end_subsubtest("rally save")

        self.start_subsubtest("rally savecsv")
        util.pexpect_drain(mavproxy)
        csvpath = self.buildlogs_path("rally-testing-tmp.csv")
        mavproxy.send('rally savecsv %s\n' % csvpath)
        mavproxy.expect('"Seq","Frame"')
        expected_content = '''"Seq","Frame","Cmd","P1","P2","P3","P4","X","Y","Z"
"0","Rel","NAV_RALLY_POINT","0.0","0.0","0.0","0.0","-5.67890024185","98.2341003418","90.0"
'''
        if sys.version_info[0] >= 3:
            # greater precision output by default
            expected_content = '''"Seq","Frame","Cmd","P1","P2","P3","P4","X","Y","Z"
"0","Rel","NAV_RALLY_POINT","0.0","0.0","0.0","0.0","-5.678900241851807","98.23410034179688","90.0"
'''
        self.assert_filepath_content(csvpath, expected_content)

        self.end_subsubtest("rally savecsv")

        self.start_subsubtest("rally load")
        self.drain_mav()
        mavproxy.send('rally clear\n')
        self.assert_mission_count_on_link(self.mav,
                                          0,
                                          target_system,
                                          target_component,
                                          mavutil.mavlink.MAV_MISSION_TYPE_RALLY)

        # warning: uses file saved from previous test
        self.start_subtest("Check rally load from filepath")
        mavproxy.send('rally load %s\n' % save_tmppath)
        mavproxy.expect(r"Loaded 1 rally items from ([^\s]*)\s")
        mavproxy.expect("Sent all .* rally items") # notional race condition here
        downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
        if len(downloaded_items) != 1:
            raise NotAchievedException("Unexpected item count (%u)" % len(downloaded_items))
        if abs(int(downloaded_items[0].x) - int(lat * 1e7)) > 3:
            raise NotAchievedException("Expected lat=%d got=%d" %
                                       (lat * 1e7, downloaded_items[0].x))
        if abs(int(downloaded_items[0].y) - int(lng * 1e7)) > 10:
            raise NotAchievedException("Expected lng=%d got=%d" %
                                       (lng * 1e7, downloaded_items[0].y))
        self.end_subsubtest("rally load")

        self.start_subsubtest("rally changealt")
        mavproxy.send('rally clear\n')
        mavproxy.send("click 1.0 1.0\n")
        mavproxy.send("rally add\n")
        mavproxy.send("click 2.0 2.0\n")
        mavproxy.send("rally add\n")
        self.delay_sim_time(10)
        self.assert_mission_count_on_link(
            self.mav,
            2,
            target_system,
            target_component,
            mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
        )
        self.drain_mav()
        mavproxy.send("rally changealt 1 17.6\n")
        self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
                                        target_system=255,
                                        target_component=0)
        self.delay_sim_time(10)
        mavproxy.send("rally changealt 2 19.1\n")
        self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
                                        target_system=255,
                                        target_component=0)
        self.delay_sim_time(10)
        downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
        if len(downloaded_items) != 2:
            raise NotAchievedException("Unexpected item count (%u)" % len(downloaded_items))
        if abs(int(downloaded_items[0].x) - int(1 * 1e7)) > 3:
            raise NotAchievedException("Expected lat=%d got=%d" % (1 * 1e7, downloaded_items[0].x))
        if abs(int(downloaded_items[0].y) - int(1 * 1e7)) > 10:
            raise NotAchievedException("Expected lng=%d got=%d" % (1 * 1e7, downloaded_items[0].y))
        # at some stage ArduPilot will stop rounding altitude.  This
        # will break then.
        if abs(int(downloaded_items[0].z) - int(17.6)) > 0.0001:
            raise NotAchievedException("Expected alt=%f got=%f" % (17.6, downloaded_items[0].z))

        if abs(int(downloaded_items[1].x) - int(2 * 1e7)) > 3:
            raise NotAchievedException("Expected lat=%d got=%d" % (2 * 1e7, downloaded_items[0].x))
        if abs(int(downloaded_items[1].y) - int(2 * 1e7)) > 10:
            raise NotAchievedException("Expected lng=%d got=%d" % (2 * 1e7, downloaded_items[0].y))
        # at some stage ArduPilot will stop rounding altitude.  This
        # will break then.
        if abs(int(downloaded_items[1].z) - int(19.1)) > 0.0001:
            raise NotAchievedException("Expected alt=%f got=%f" % (19.1, downloaded_items[1].z))

        self.progress("Now change two at once")
        mavproxy.send("rally changealt 1 17.3 2\n")
        self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
                                        target_system=255,
                                        target_component=0)
        downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
        if len(downloaded_items) != 2:
            raise NotAchievedException("Unexpected item count (%u)" % len(downloaded_items))

        if abs(int(downloaded_items[0].x) - int(1 * 1e7)) > 3:
            raise NotAchievedException("Expected lat=%d got=%d" % (1 * 1e7, downloaded_items[0].x))
        if abs(int(downloaded_items[0].y) - int(1 * 1e7)) > 10:
            raise NotAchievedException("Expected lng=%d got=%d" % (1 * 1e7, downloaded_items[0].y))
        # at some stage ArduPilot will stop rounding altitude.  This
        # will break then.
        if abs(int(downloaded_items[0].z) - int(17.3)) > 0.0001:
            raise NotAchievedException("Expected alt=%f got=%f" % (17.3, downloaded_items[0].z))

        if abs(int(downloaded_items[1].x) - int(2 * 1e7)) > 3:
            raise NotAchievedException("Expected lat=%d got=%d" % (2 * 1e7, downloaded_items[0].x))
        if abs(int(downloaded_items[1].y) - int(2 * 1e7)) > 10:
            raise NotAchievedException("Expected lng=%d got=%d" % (2 * 1e7, downloaded_items[0].y))
        # at some stage ArduPilot will stop rounding altitude.  This
        # will break then.
        if abs(int(downloaded_items[1].z) - int(17.3)) > 0.0001:
            raise NotAchievedException("Expected alt=%f got=%f" % (17.3, downloaded_items[0].z))

        self.end_subsubtest("rally changealt")

        self.start_subsubtest("rally move")
        mavproxy.send('rally clear\n')
        mavproxy.send("click 1.0 1.0\n")
        mavproxy.send("rally add\n")
        mavproxy.send("click 2.0 2.0\n")
        mavproxy.send("rally add\n")
        self.delay_sim_time(5)
        self.assert_mission_count_on_link(
            self.mav,
            2,
            target_system,
            target_component,
            mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
        )
        mavproxy.send("click 3.0 3.0\n")
        mavproxy.send("rally move 2\n")
        self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
                                        target_system=255,
                                        target_component=0)
        mavproxy.send("click 4.12345 4.987654\n")
        mavproxy.send("rally move 1\n")
        self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
                                        target_system=255,
                                        target_component=0)

        downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
        if len(downloaded_items) != 2:
            raise NotAchievedException("Unexpected item count (%u)" % len(downloaded_items))
        if downloaded_items[0].x != 41234500:
            raise NotAchievedException("Bad latitude")
        if downloaded_items[0].y != 49876540:
            raise NotAchievedException("Bad longitude")
        if downloaded_items[0].z != 90:
            raise NotAchievedException("Bad altitude (want=%u got=%u)" %
                                       (90, downloaded_items[0].z))

        if downloaded_items[1].x != 30000000:
            raise NotAchievedException("Bad latitude")
        if downloaded_items[1].y != 30000000:
            raise NotAchievedException("Bad longitude")
        if downloaded_items[1].z != 90:
            raise NotAchievedException("Bad altitude (want=%u got=%u)" %
                                       (90, downloaded_items[1].z))
        self.end_subsubtest("rally move")

        self.start_subsubtest("rally movemulti")
        self.drain_mav()
        mavproxy.send('rally clear\n')
        self.drain_mav()
        # there are race conditions in MAVProxy.  Beware.
        mavproxy.send("click 1.0 1.0\n")
        mavproxy.send("rally add\n")
        mavproxy.send("click 2.0 2.0\n")
        mavproxy.send("rally add\n")
        mavproxy.send("click 3.0 3.0\n")
        mavproxy.send("rally add\n")
        self.delay_sim_time(10)
        self.assert_mission_count_on_link(
            self.mav,
            3,
            target_system,
            target_component,
            mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
        )
        click_lat = 2.0
        click_lon = 3.0
        unmoved_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
        if len(unmoved_items) != 3:
            raise NotAchievedException("Unexpected item count")
        mavproxy.send("click %f %f\n" % (click_lat, click_lon))
        mavproxy.send("rally movemulti 2 1 3\n")
        # MAVProxy currently sends three separate items up.  That's
        # not great and I don't want to lock that behaviour in here.
        self.delay_sim_time(10)
        expected_moved_items = copy.copy(unmoved_items)
        expected_moved_items[0].x = 1.0 * 1e7
        expected_moved_items[0].y = 2.0 * 1e7
        expected_moved_items[1].x = 2.0 * 1e7
        expected_moved_items[1].y = 3.0 * 1e7
        expected_moved_items[2].x = 3.0 * 1e7
        expected_moved_items[2].y = 4.0 * 1e7
        moved_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
        # we're moving an entire degree in latitude; quite an epsilon required...
        self.check_rally_items_same(expected_moved_items, moved_items, epsilon=10000)

        self.progress("now move back and rotate through 90 degrees")
        mavproxy.send("click %f %f\n" % (2, 2))
        mavproxy.send("rally movemulti 2 1 3 90\n")

        # MAVProxy currently sends three separate items up.  That's
        # not great and I don't want to lock that behaviour in here.
        self.delay_sim_time(10)
        expected_moved_items = copy.copy(unmoved_items)
        expected_moved_items[0].x = 3.0 * 1e7
        expected_moved_items[0].y = 1.0 * 1e7
        expected_moved_items[1].x = 2.0 * 1e7
        expected_moved_items[1].y = 2.0 * 1e7
        expected_moved_items[2].x = 1.0 * 1e7
        expected_moved_items[2].y = 3.0 * 1e7
        moved_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
        # we're moving an entire degree in latitude; quite an epsilon required...
        self.check_rally_items_same(expected_moved_items, moved_items, epsilon=12000)
        self.end_subsubtest("rally movemulti")

        self.start_subsubtest("rally param")
        mavproxy.send("rally param 3 2 5\n")
        mavproxy.expect("Set param 2 for 3 to 5.000000")
        self.end_subsubtest("rally param")

        self.start_subsubtest("rally remove")
        self.click_three_in(target_system=target_system, target_component=target_component)
        pure_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
        self.progress("Removing last in list")
        mavproxy.send("rally remove 3\n")
        self.delay_sim_time(10)
        self.assert_mission_count_on_link(
            self.mav,
            2,
            target_system,
            target_component,
            mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
        )
        fewer_downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
        if len(fewer_downloaded_items) != 2:
            raise NotAchievedException("Unexpected download list length")
        shorter_items = copy.copy(pure_items)
        shorter_items = shorter_items[0:2]
        self.check_rally_items_same(shorter_items, fewer_downloaded_items)

        self.progress("Removing first in list")
        mavproxy.send("rally remove 1\n")
        self.delay_sim_time(5)
        self.assert_mission_count_on_link(
            self.mav,
            1,
            target_system,
            target_component,
            mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
        )
        fewer_downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
        if len(fewer_downloaded_items) != 1:
            raise NotAchievedException("Unexpected download list length")
        shorter_items = shorter_items[1:]
        self.check_rally_items_same(shorter_items, fewer_downloaded_items)

        self.progress("Removing remaining item")
        mavproxy.send("rally remove 1\n")
        self.delay_sim_time(5)
        self.assert_mission_count_on_link(
            self.mav,
            0,
            target_system,
            target_component,
            mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
        )
        self.end_subsubtest("rally remove")

        self.start_subsubtest("rally show")
        # what can we test here?
        mavproxy.send("rally show %s\n" % save_tmppath)
        self.end_subsubtest("rally show")

        # savelocal must be run immediately after show!
        self.start_subsubtest("rally savelocal")
        util.pexpect_drain(mavproxy)
        savelocal_path = self.buildlogs_path("rally-testing-tmp-local.txt")
        mavproxy.send('rally savelocal %s\n' % savelocal_path)
        self.delay_sim_time(5)
        self.assert_rally_filepath_content(savelocal_path, '''QGC WPL 110
0	0	3	5100	0.000000	0.000000	0.000000	0.000000	-5.678900	98.234100	90.000000	0
''')
        self.end_subsubtest("rally savelocal")

        self.start_subsubtest("rally status")
        self.click_three_in(target_system=target_system, target_component=target_component)
        mavproxy.send("rally status\n")
        mavproxy.expect("Have 3 of 3 rally items")
        mavproxy.send("rally clear\n")
        mavproxy.send("rally status\n")
        mavproxy.expect("Have 0 of 0 rally items")
        self.end_subsubtest("rally status")

        self.start_subsubtest("rally undo")
        self.progress("Testing undo-remove")
        self.click_three_in(target_system=target_system, target_component=target_component)
        pure_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
        self.progress("Removing first in list")
        mavproxy.send("rally remove 1\n")
        self.delay_sim_time(5)
        self.assert_mission_count_on_link(
            self.mav,
            2,
            target_system,
            target_component,
            mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
        )
        mavproxy.send("rally undo\n")
        self.delay_sim_time(5)
        undone_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
        self.check_rally_items_same(pure_items, undone_items)

        self.progress("Testing undo-move")

        pure_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
        mavproxy.send("click 4.12345 4.987654\n")
        mavproxy.send("rally move 1\n")
        # move has already been tested, assume it works...
        self.delay_sim_time(5)
        mavproxy.send("rally undo\n")
        self.delay_sim_time(5)
        undone_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
        self.check_rally_items_same(pure_items, undone_items)

        self.end_subsubtest("rally undo")

        self.start_subsubtest("rally update")
        self.click_three_in(target_system=target_system, target_component=target_component)
        pure_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
        rally_update_tmpfilepath = self.buildlogs_path("rally-tmp-update.txt")
        mavproxy.send("rally save %s\n" % rally_update_tmpfilepath)
        self.delay_sim_time(5)
        self.progress("Moving waypoint")
        mavproxy.send("click 13.0 13.0\n")
        mavproxy.send("rally move 1\n")
        self.delay_sim_time(5)
        self.progress("Reverting to original")
        mavproxy.send("rally update %s\n" % rally_update_tmpfilepath)
        self.delay_sim_time(5)
        reverted_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
        self.check_rally_items_same(pure_items, reverted_items)

        self.progress("Making sure specifying a waypoint to be updated works")
        mavproxy.send("click 13.0 13.0\n")
        mavproxy.send("rally move 1\n")
        self.delay_sim_time(5)
        mavproxy.send("click 17.0 17.0\n")
        mavproxy.send("rally move 2\n")
        self.delay_sim_time(5)
        self.progress("Reverting to original item 2")
        mavproxy.send("rally update %s 2\n" % rally_update_tmpfilepath)
        self.delay_sim_time(5)
        reverted_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
        if reverted_items[0].x != 130000000:
            raise NotAchievedException("Expected item1 x to stay changed (got=%u want=%u)" % (reverted_items[0].x, 130000000))
        if reverted_items[1].x == 170000000:
            raise NotAchievedException("Expected item2 x to revert")

        self.end_subsubtest("rally update")
        self.delay_sim_time(1)
        if self.get_parameter("RALLY_TOTAL") != 0:
            raise NotAchievedException("Failed to clear rally points")

        self.stop_mavproxy(mavproxy)

# MANUAL> usage: rally <add|alt|changealt|clear|list|load|move|movemulti|param|remove|save|savecsv|savelocal|show|status|undo|update>  # noqa

    def RallyUploadDownload(self, target_system=1, target_component=1):
        '''Upload and download of rally'''
        old_srcSystem = self.mav.mav.srcSystem

        self.drain_mav()
        try:
            item1_lat = int(2.0000 * 1e7)
            items = [
                self.mav.mav.mission_item_int_encode(
                    target_system,
                    target_component,
                    0, # seq
                    mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
                    mavutil.mavlink.MAV_CMD_NAV_RALLY_POINT,
                    0, # current
                    0, # autocontinue
                    0, # p1
                    0, # p2
                    0, # p3
                    0, # p4
                    int(1.0000 * 1e7), # latitude
                    int(1.0000 * 1e7), # longitude
                    31.0000, # altitude
                    mavutil.mavlink.MAV_MISSION_TYPE_RALLY),
                self.mav.mav.mission_item_int_encode(
                    target_system,
                    target_component,
                    1, # seq
                    mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
                    mavutil.mavlink.MAV_CMD_NAV_RALLY_POINT,
                    0, # current
                    0, # autocontinue
                    0, # p1
                    0, # p2
                    0, # p3
                    0, # p4
                    item1_lat, # latitude
                    int(2.0000 * 1e7), # longitude
                    32.0000, # altitude
                    mavutil.mavlink.MAV_MISSION_TYPE_RALLY),
                self.mav.mav.mission_item_int_encode(
                    target_system,
                    target_component,
                    2, # seq
                    mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
                    mavutil.mavlink.MAV_CMD_NAV_RALLY_POINT,
                    0, # current
                    0, # autocontinue
                    0, # p1
                    0, # p2
                    0, # p3
                    0, # p4
                    int(3.0000 * 1e7), # latitude
                    int(3.0000 * 1e7), # longitude
                    33.0000, # altitude
                    mavutil.mavlink.MAV_MISSION_TYPE_RALLY),
            ]
            self.upload_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
                                               items)
            downloaded = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
            print("Got items (%s)" % str(items))
            if len(downloaded) != len(items):
                raise NotAchievedException(
                    "Did not download correct number of items want=%u got=%u" %
                    (len(downloaded), len(items)))

            rally_total = self.get_parameter("RALLY_TOTAL")
            if rally_total != len(downloaded):
                raise NotAchievedException(
                    "Unexpected rally point count: want=%u got=%u" %
                    (len(items), rally_total))

            self.progress("Pruning count by setting parameter (urgh)")
            self.set_parameter("RALLY_TOTAL", 2)
            downloaded = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
            if len(downloaded) != 2:
                raise NotAchievedException(
                    "Failed to prune rally points by setting parameter.  want=%u got=%u" %
                    (2, len(downloaded)))

            self.progress("Uploading a third item using old protocol")
            new_item2_lat = int(6.0 * 1e7)
            self.set_parameter("RALLY_TOTAL", 3)
            self.mav.mav.rally_point_send(target_system,
                                          target_component,
                                          2, # sequence number
                                          3, # total count
                                          new_item2_lat,
                                          int(7.0 * 1e7),
                                          15,
                                          0, # "break" alt?!
                                          0, # "land dir"
                                          0) # flags
            downloaded = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
            if len(downloaded) != 3:
                raise NotAchievedException(
                    "resetting rally point count didn't change items returned")
            if downloaded[2].x != new_item2_lat:
                raise NotAchievedException(
                    "Bad lattitude in downloaded item: want=%u got=%u" %
                    (new_item2_lat, downloaded[2].x))

            self.progress("Grabbing original item 1 using original protocol")
            self.mav.mav.rally_fetch_point_send(target_system,
                                                target_component,
                                                1)
            m = self.mav.recv_match(type="RALLY_POINT", blocking=True, timeout=1)
            if m.target_system != self.mav.source_system:
                raise NotAchievedException(
                    "Bad target_system on received rally point (want=%u got=%u)" %
                    (255, m.target_system))
            if m.target_component != self.mav.source_component: # autotest's component ID
                raise NotAchievedException("Bad target_component on received rally point")
            if m.lat != item1_lat:
                raise NotAchievedException("Bad latitude on received rally point")

            self.start_subtest("Test upload lockout and timeout")
            self.progress("Starting upload from normal sysid")
            self.mav.mav.mission_count_send(target_system,
                                            target_component,
                                            len(items),
                                            mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
            self.drain_mav() # throw away requests for items
            self.mav.mav.srcSystem = 243

            self.progress("Attempting upload from sysid=%u" %
                          (self.mav.mav.srcSystem,))
            self.mav.mav.mission_count_send(target_system,
                                            target_component,
                                            len(items),
                                            mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
            self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
                                            want_type=mavutil.mavlink.MAV_MISSION_DENIED)

            self.progress("Attempting download from sysid=%u" %
                          (self.mav.mav.srcSystem,))
            self.mav.mav.mission_request_list_send(target_system,
                                                   target_component,
                                                   mavutil.mavlink.MAV_MISSION_TYPE_RALLY)

            self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
                                            want_type=mavutil.mavlink.MAV_MISSION_DENIED)

            # wait for the upload from sysid=1 to time out:
            tstart = self.get_sim_time()
            got_statustext = False
            got_ack = False
            while True:
                if got_statustext and got_ack:
                    self.progress("Got both ack and statustext")
                    break
                if self.get_sim_time_cached() - tstart > 100:
                    raise NotAchievedException("Did not get both ack and statustext")
                m = self.mav.recv_match(type=['STATUSTEXT', 'MISSION_ACK'],
                                        blocking=True,
                                        timeout=1)
                if m is None:
                    continue
                self.progress("Got (%s)" % str(m))
                if m.get_type() == 'STATUSTEXT':
                    if "upload timeout" in m.text:
                        got_statustext = True
                        self.progress("Received desired statustext")
                    continue
                if m.get_type() == 'MISSION_ACK':
                    if m.target_system != old_srcSystem:
                        raise NotAchievedException("Incorrect sourcesystem")
                    if m.type != mavutil.mavlink.MAV_MISSION_OPERATION_CANCELLED:
                        raise NotAchievedException("Incorrect result")
                    if m.mission_type != mavutil.mavlink.MAV_MISSION_TYPE_RALLY:
                        raise NotAchievedException("Incorrect mission_type")
                    got_ack = True
                    self.progress("Received desired ACK")
                    continue
                raise NotAchievedException("Huh?")

            self.progress("Now trying to upload empty mission after timeout")
            self.mav.mav.mission_count_send(target_system,
                                            target_component,
                                            0,
                                            mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
            self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)

            self.drain_mav()
            self.start_subtest("Check rally upload/download across separate links")
            self.upload_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
                                               items)
            self.progress("ensure a mavlink1 connection can't do anything useful with new item types")
            self.set_parameter("SERIAL2_PROTOCOL", 1)
            self.reboot_sitl()
            mav2 = mavutil.mavlink_connection("tcp:localhost:5763",
                                              robust_parsing=True,
                                              source_system=7,
                                              source_component=7)
            mav2.mav.mission_request_list_send(target_system,
                                               target_component,
                                               mission_type=mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
            # so this looks a bit odd; the other end isn't sending
            # mavlink2 so can't fill in the extension here.
            self.assert_receive_mission_ack(
                mavutil.mavlink.MAV_MISSION_TYPE_MISSION,
                want_type=mavutil.mavlink.MAV_MISSION_UNSUPPORTED,
                mav=mav2,
            )
            # this relies on magic upgrade to serial2:
            self.set_parameter("SERIAL2_PROTOCOL", 2)
            expected_count = 3
            self.progress("Assert mission count on new link")
            self.assert_mission_count_on_link(
                mav2,
                expected_count,
                target_system,
                target_component,
                mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
            self.progress("Assert mission count on original link")
            self.assert_mission_count_on_link(
                self.mav,
                expected_count,
                target_system,
                target_component,
                mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
            self.progress("Get first item on new link")

            def drain_self_mav_fn():
                self.drain_mav(self.mav)
            m2 = self.get_mission_item_int_on_link(
                2,
                mav2,
                target_system,
                target_component,
                mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
                delay_fn=drain_self_mav_fn)
            self.progress("Get first item on original link")
            m = self.get_mission_item_int_on_link(
                2,
                self.mav,
                target_system,
                target_component,
                mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
            if m2.x != m.x:
                raise NotAchievedException("mission items do not match (%d vs %d)" % (m2.x, m.x))
            self.get_mission_item_on_link(2, self.mav, target_system, target_component, mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
            # ensure we get nacks for bad mission item requests:
            self.mav.mav.mission_request_send(target_system,
                                              target_component,
                                              65,
                                              mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
            self.assert_receive_mission_ack(
                mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
                want_type=mavutil.mavlink.MAV_MISSION_INVALID_SEQUENCE,
            )
            self.mav.mav.mission_request_int_send(target_system,
                                                  target_component,
                                                  65,
                                                  mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
            self.assert_receive_mission_ack(
                mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
                want_type=mavutil.mavlink.MAV_MISSION_INVALID_SEQUENCE,
            )

            self.start_subtest("Should enforce items come from correct GCS")
            self.drain_mav(unparsed=True)
            self.mav.mav.mission_count_send(target_system,
                                            target_component,
                                            1,
                                            mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
            self.assert_receive_mission_item_request(mavutil.mavlink.MAV_MISSION_TYPE_RALLY, 0)
            self.progress("Attempting to upload from bad sysid")
            old_sysid = self.mav.mav.srcSystem
            self.mav.mav.srcSystem = 17
            items[0].pack(self.mav.mav)
            self.drain_mav(unparsed=True)
            self.mav.mav.send(items[0])
            self.mav.mav.srcSystem = old_sysid
            self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
                                            want_type=mavutil.mavlink.MAV_MISSION_DENIED,
                                            target_system=17)
            self.progress("Sending from correct sysid")
            items[0].pack(self.mav.mav)
            self.drain_mav(unparsed=True)
            self.mav.mav.send(items[0])
            self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)

            self.drain_mav()
            self.drain_all_pexpects()

            self.start_subtest("Attempt to send item on different link to that which we are sending requests on")
            self.progress("Sending count")
            self.drain_mav(unparsed=True)
            self.mav.mav.mission_count_send(target_system,
                                            target_component,
                                            2,
                                            mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
            self.assert_receive_mission_item_request(mavutil.mavlink.MAV_MISSION_TYPE_RALLY, 0)
            old_mav2_system = mav2.mav.srcSystem
            old_mav2_component = mav2.mav.srcComponent
            mav2.mav.srcSystem = self.mav.mav.srcSystem
            mav2.mav.srcComponent = self.mav.mav.srcComponent
            self.progress("Sending item on second link")
            # note that the routing table in ArduPilot now will say
            # this sysid/compid is on both links which may cause
            # weirdness...
            items[0].pack(mav2.mav)
            self.drain_mav(mav=self.mav, unparsed=True)
            mav2.mav.send(items[0])
            mav2.mav.srcSystem = old_mav2_system
            mav2.mav.srcComponent = old_mav2_component
            # we continue to receive requests on the original link:
            m = self.assert_receive_message('MISSION_REQUEST', timeout=1)
            if m.mission_type != mavutil.mavlink.MAV_MISSION_TYPE_RALLY:
                raise NotAchievedException("Mission request of incorrect type")
            if m.seq != 1:
                raise NotAchievedException("Unexpected sequence number (expected=%u got=%u)" % (1, m.seq))
            items[1].pack(self.mav.mav)
            self.drain_mav(unparsed=True)
            self.mav.mav.send(items[1])
            self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)

            self.drain_mav()
            self.drain_all_pexpects()

            self.start_subtest("Upload mission and rally points at same time")
            self.progress("Sending rally count")
            self.drain_mav(unparsed=True)
            self.mav.mav.mission_count_send(target_system,
                                            target_component,
                                            3,
                                            mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
            self.assert_receive_mission_item_request(mavutil.mavlink.MAV_MISSION_TYPE_RALLY, 0)

            self.progress("Sending wp count")
            self.mav.mav.mission_count_send(target_system,
                                            target_component,
                                            3,
                                            mavutil.mavlink.MAV_MISSION_TYPE_MISSION)
            self.assert_receive_mission_item_request(mavutil.mavlink.MAV_MISSION_TYPE_MISSION, 0)

            self.progress("Answering request for mission item 0")
            self.drain_mav(mav=self.mav, unparsed=True)
            self.mav.mav.mission_item_int_send(
                target_system,
                target_component,
                0, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
                mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
                0, # current
                0, # autocontinue
                0, # p1
                0, # p2
                0, # p3
                0, # p4
                int(1.1000 * 1e7), # latitude
                int(1.2000 * 1e7), # longitude
                321.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_MISSION),
            self.assert_receive_mission_item_request(mavutil.mavlink.MAV_MISSION_TYPE_MISSION, 1)

            self.progress("Answering request for rally point 0")
            items[0].pack(self.mav.mav)
            self.drain_mav(unparsed=True)
            self.mav.mav.send(items[0])
            self.progress("Expecting request for rally item 1")
            self.assert_receive_mission_item_request(mavutil.mavlink.MAV_MISSION_TYPE_RALLY, 1)
            self.progress("Answering request for rally point 1")
            items[1].pack(self.mav.mav)
            self.drain_mav(unparsed=True)
            self.mav.mav.send(items[1])
            self.progress("Expecting request for rally item 2")
            self.assert_receive_mission_item_request(mavutil.mavlink.MAV_MISSION_TYPE_RALLY, 2)

            self.progress("Answering request for rally point 2")
            items[2].pack(self.mav.mav)
            self.drain_mav(unparsed=True)
            self.mav.mav.send(items[2])
            self.progress("Expecting mission ack for rally")
            self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)

            self.progress("Answering request for waypoints item 1")
            self.drain_mav(unparsed=True)
            self.mav.mav.mission_item_int_send(
                target_system,
                target_component,
                1, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
                mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
                0, # current
                0, # autocontinue
                0, # p1
                0, # p2
                0, # p3
                0, # p4
                int(1.1000 * 1e7), # latitude
                int(1.2000 * 1e7), # longitude
                321.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_MISSION),
            self.assert_receive_mission_item_request(mavutil.mavlink.MAV_MISSION_TYPE_MISSION, 2)

            self.progress("Answering request for waypoints item 2")
            self.drain_mav(unparsed=True)
            self.mav.mav.mission_item_int_send(
                target_system,
                target_component,
                2, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
                mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
                0, # current
                0, # autocontinue
                0, # p1
                0, # p2
                0, # p3
                0, # p4
                int(1.1000 * 1e7), # latitude
                int(1.2000 * 1e7), # longitude
                321.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_MISSION),
            self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_MISSION)

            self.start_subtest("Test write-partial-list")
            self.progress("Clearing rally points using count-send")
            self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
                               target_system=target_system,
                               target_component=target_component)
            self.progress("Should not be able to set items completely past the waypoint count")
            self.upload_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
                                               items)
            self.drain_mav(unparsed=True)
            self.mav.mav.mission_write_partial_list_send(
                target_system,
                target_component,
                17,
                20,
                mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
            self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
                                            want_type=mavutil.mavlink.MAV_MISSION_ERROR)

            self.progress("Should not be able to set items overlapping the waypoint count")
            self.drain_mav(unparsed=True)
            self.mav.mav.mission_write_partial_list_send(
                target_system,
                target_component,
                0,
                20,
                mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
            self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
                                            want_type=mavutil.mavlink.MAV_MISSION_ERROR)

            self.progress("try to overwrite items 1 and 2")
            self.drain_mav(unparsed=True)
            self.mav.mav.mission_write_partial_list_send(
                target_system,
                target_component,
                1,
                2,
                mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
            self.assert_receive_mission_item_request(mavutil.mavlink.MAV_MISSION_TYPE_RALLY, 1)
            self.progress("Try shoving up an incorrectly sequenced item")
            self.drain_mav(unparsed=True)
            self.mav.mav.mission_item_int_send(
                target_system,
                target_component,
                0, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
                mavutil.mavlink.MAV_CMD_NAV_RALLY_POINT,
                0, # current
                0, # autocontinue
                0, # p1
                0, # p2
                0, # p3
                0, # p4
                int(1.1000 * 1e7), # latitude
                int(1.2000 * 1e7), # longitude
                321.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_RALLY),
            self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
                                            want_type=mavutil.mavlink.MAV_MISSION_INVALID_SEQUENCE)

            self.progress("Try shoving up an incorrectly sequenced item (but within band)")
            self.drain_mav(unparsed=True)
            self.mav.mav.mission_item_int_send(
                target_system,
                target_component,
                2, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
                mavutil.mavlink.MAV_CMD_NAV_RALLY_POINT,
                0, # current
                0, # autocontinue
                0, # p1
                0, # p2
                0, # p3
                0, # p4
                int(1.1000 * 1e7), # latitude
                int(1.2000 * 1e7), # longitude
                321.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_RALLY),
            self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
                                            want_type=mavutil.mavlink.MAV_MISSION_INVALID_SEQUENCE)

            self.progress("Now provide correct item")
            item1_latitude = int(1.2345 * 1e7)
            self.drain_mav(unparsed=True)
            self.mav.mav.mission_item_int_send(
                target_system,
                target_component,
                1, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
                mavutil.mavlink.MAV_CMD_NAV_RALLY_POINT,
                0, # current
                0, # autocontinue
                0, # p1
                0, # p2
                0, # p3
                0, # p4
                item1_latitude, # latitude
                int(1.2000 * 1e7), # longitude
                321.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_RALLY),
            self.assert_receive_mission_item_request(mavutil.mavlink.MAV_MISSION_TYPE_RALLY, 2)
            self.progress("Answering request for rally point 2")
            items[2].pack(self.mav.mav)
            self.drain_mav(unparsed=True)
            self.mav.mav.send(items[2])
            self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
            self.progress("TODO: ensure partial mission write was good")

            self.start_subtest("clear mission types")
            self.assert_mission_count_on_link(
                self.mav,
                3,
                target_system,
                target_component,
                mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
            self.assert_mission_count_on_link(
                self.mav,
                3,
                target_system,
                target_component,
                mavutil.mavlink.MAV_MISSION_TYPE_MISSION)
            self.drain_mav(unparsed=True)
            self.mav.mav.mission_clear_all_send(target_system, target_component, mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
            self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
            self.assert_mission_count_on_link(
                self.mav,
                0,
                target_system,
                target_component,
                mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
            self.assert_mission_count_on_link(
                self.mav,
                3,
                target_system,
                target_component,
                mavutil.mavlink.MAV_MISSION_TYPE_MISSION)
            self.drain_mav(unparsed=True)
            self.mav.mav.mission_clear_all_send(target_system, target_component, mavutil.mavlink.MAV_MISSION_TYPE_MISSION)
            self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_MISSION)
            self.assert_mission_count_on_link(
                self.mav,
                0,
                target_system,
                target_component,
                mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
            self.assert_mission_count_on_link(
                self.mav,
                0,
                target_system,
                target_component,
                mavutil.mavlink.MAV_MISSION_TYPE_MISSION)

            self.start_subtest("try sending out-of-range counts")
            self.drain_mav(unparsed=True)
            self.mav.mav.mission_count_send(target_system,
                                            target_component,
                                            1,
                                            230)
            self.assert_receive_mission_ack(230,
                                            want_type=mavutil.mavlink.MAV_MISSION_UNSUPPORTED)
            self.drain_mav(unparsed=True)
            self.mav.mav.mission_count_send(target_system,
                                            target_component,
                                            16000,
                                            mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
            self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
                                            want_type=mavutil.mavlink.MAV_MISSION_NO_SPACE)

        except Exception as e:
            self.progress("Received exception (%s)" % self.get_exception_stacktrace(e))
            self.mav.mav.srcSystem = old_srcSystem
            raise e
        self.reboot_sitl()

    def GCSMission(self):
        '''check MAVProxy's waypoint handling of missions'''
        target_system = 1
        target_component = 1
        mavproxy = self.start_mavproxy()
        mavproxy.send('wp clear\n')
        self.delay_sim_time(1)
        if self.get_parameter("MIS_TOTAL") != 0:
            raise NotAchievedException("Failed to clear mission")
        m = self.assert_receive_message('MISSION_CURRENT', timeout=5)
        if m.seq != 0:
            raise NotAchievedException("Bad mission current")
        self.load_mission_using_mavproxy(mavproxy, "rover-gripper-mission.txt")
        set_wp = 1
        mavproxy.send('wp set %u\n' % set_wp)
        self.wait_current_waypoint(set_wp)

        self.start_subsubtest("wp changealt")
        downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_MISSION)
        changealt_item = 1
#        oldalt = downloaded_items[changealt_item].z
        want_newalt = 37.2
        mavproxy.send('wp changealt %u %f\n' % (changealt_item, want_newalt))
        self.delay_sim_time(5)
        downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_MISSION)
        if abs(downloaded_items[changealt_item].z - want_newalt) > 0.0001:
            raise NotAchievedException(
                "changealt didn't (want=%f got=%f)" %
                (want_newalt, downloaded_items[changealt_item].z))
        self.end_subsubtest("wp changealt")

        self.start_subsubtest("wp sethome")
        new_home_lat = 3.14159
        new_home_lng = 2.71828
        mavproxy.send('click %f %f\n' % (new_home_lat, new_home_lng))
        mavproxy.send('wp sethome\n')
        self.delay_sim_time(5)
        # any way to close the loop on this one?
        # downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_MISSION)
        # if abs(downloaded_items[0].x - new_home_lat) > 0.0001:
        #     raise NotAchievedException("wp sethome didn't work")
        # if abs(downloaded_items[0].y - new_home_lng) > 0.0001:
        #     raise NotAchievedException("wp sethome didn't work")
        self.end_subsubtest("wp sethome")

        self.start_subsubtest("wp slope")
        mavproxy.send('wp slope\n')
        mavproxy.expect("WP3: slope 0.1")
        self.delay_sim_time(5)
        self.end_subsubtest("wp slope")

        if not self.mavproxy_can_do_mision_item_protocols():
            # adding based on click location yet to be merged into MAVProxy
            return

        self.start_subsubtest("wp split")
        mavproxy.send("wp clear\n")
        self.delay_sim_time(5)
        mavproxy.send("wp list\n")
        self.delay_sim_time(5)
        items = [
            None,
            self.mav.mav.mission_item_int_encode(
                target_system,
                target_component,
                1, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
                0, # current
                0, # autocontinue
                0, # p1
                0, # p2
                0, # p3
                0, # p4
                int(1.0 * 1e7), # latitude
                int(1.0 * 1e7), # longitude
                33.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_MISSION),
            self.mav.mav.mission_item_int_encode(
                target_system,
                target_component,
                2, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
                0, # current
                0, # autocontinue
                0, # p1
                0, # p2
                0, # p3
                0, # p4
                int(2.0 * 1e7), # latitude
                int(2.0 * 1e7), # longitude
                33.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_MISSION),
        ]
        mavproxy.send("click 5 5\n") # space for home position
        mavproxy.send("wp add\n")
        self.delay_sim_time(5)
        self.click_location_from_item(mavproxy, items[1])
        mavproxy.send("wp add\n")
        self.delay_sim_time(5)
        self.click_location_from_item(mavproxy, items[2])
        mavproxy.send("wp add\n")
        self.delay_sim_time(5)
        downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_MISSION)
        self.check_mission_waypoint_items_same(items, downloaded_items)
        mavproxy.send("wp split 2\n")
        self.delay_sim_time(5)
        items_with_split_in = [
            items[0],
            items[1],
            self.mav.mav.mission_item_int_encode(
                target_system,
                target_component,
                2, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
                0, # current
                0, # autocontinue
                0, # p1
                0, # p2
                0, # p3
                0, # p4
                int(1.5 * 1e7), # latitude
                int(1.5 * 1e7), # longitude
                33.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_MISSION),
            items[2],
        ]
        downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_MISSION)
        self.check_mission_waypoint_items_same(items_with_split_in,
                                               downloaded_items)

        self.stop_mavproxy(mavproxy)

        # MANUAL> usage: wp <changealt|clear|draw|editor|list|load|loop|move|movemulti|noflyadd|param|remove|save|savecsv|savelocal|set|sethome|show|slope|split|status|undo|update>  # noqa

    def wait_location_sending_target(self, loc, target_system=1, target_component=1, timeout=60, max_delta=2):
        tstart = self.get_sim_time()
        last_sent = 0

        type_mask = (mavutil.mavlink.POSITION_TARGET_TYPEMASK_VX_IGNORE +
                     mavutil.mavlink.POSITION_TARGET_TYPEMASK_VY_IGNORE +
                     mavutil.mavlink.POSITION_TARGET_TYPEMASK_VZ_IGNORE +
                     mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE +
                     mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE +
                     mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE +
                     mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE +
                     mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE)

        self.change_mode('GUIDED')
        tstart = self.get_sim_time()
        while True:
            now = self.get_sim_time_cached()
            if now - tstart > timeout:
                raise AutoTestTimeoutException("Did not get to location")
            if now - last_sent > 10:
                last_sent = now
                self.mav.mav.set_position_target_global_int_send(
                    0,
                    target_system,
                    target_component,
                    mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                    type_mask,
                    int(loc.lat * 1.0e7),
                    int(loc.lng * 1.0e7),
                    0, # alt
                    0, # x-ve
                    0, # y-vel
                    0, # z-vel
                    0, # afx
                    0, # afy
                    0, # afz
                    0, # yaw,
                    0, # yaw-rate
                )
            m = self.mav.recv_match(blocking=True,
                                    timeout=1)
            if m is None:
                continue
            t = m.get_type()
            if t == "POSITION_TARGET_GLOBAL_INT":
                self.progress("Target: (%s)" % str(m), send_statustext=False)
            elif t == "GLOBAL_POSITION_INT":
                self.progress("Position: (%s)" % str(m), send_statustext=False)
                delta = self.get_distance(
                    mavutil.location(m.lat * 1e-7, m.lon * 1e-7, 0, 0),
                    loc)
                self.progress("delta: %s" % str(delta), send_statustext=False)
                if delta < max_delta:
                    self.progress("Reached destination")

    def drive_to_location(self, loc, tolerance=1, timeout=30, target_system=1, target_component=1):
        self.assert_mode('GUIDED')

        type_mask = (mavutil.mavlink.POSITION_TARGET_TYPEMASK_VX_IGNORE +
                     mavutil.mavlink.POSITION_TARGET_TYPEMASK_VY_IGNORE +
                     mavutil.mavlink.POSITION_TARGET_TYPEMASK_VZ_IGNORE +
                     mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE +
                     mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE +
                     mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE +
                     mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE +
                     mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE)

        last_sent = 0
        tstart = self.get_sim_time()
        while True:
            now = self.get_sim_time_cached()
            if now - tstart > timeout:
                raise NotAchievedException("Did not get to location")
            if now - last_sent > 10:
                last_sent = now
                self.mav.mav.set_position_target_global_int_send(
                    0,
                    target_system,
                    target_component,
                    mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                    type_mask,
                    int(loc.lat * 1.0e7),
                    int(loc.lng * 1.0e7),
                    0, # alt
                    0, # x-ve
                    0, # y-vel
                    0, # z-vel
                    0, # afx
                    0, # afy
                    0, # afz
                    0, # yaw,
                    0, # yaw-rate
                )
            if self.get_distance(self.mav.location(), loc) > tolerance:
                continue
            return

    def drive_somewhere_breach_boundary_and_rtl(self, loc, target_system=1, target_component=1, timeout=60):
        tstart = self.get_sim_time()
        last_sent = 0
        seen_fence_breach = False

        type_mask = (mavutil.mavlink.POSITION_TARGET_TYPEMASK_VX_IGNORE +
                     mavutil.mavlink.POSITION_TARGET_TYPEMASK_VY_IGNORE +
                     mavutil.mavlink.POSITION_TARGET_TYPEMASK_VZ_IGNORE +
                     mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE +
                     mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE +
                     mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE +
                     mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE +
                     mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE)

        self.change_mode('GUIDED')
        while True:
            now = self.get_sim_time_cached()
            if now - tstart > timeout:
                raise NotAchievedException("Did not breach boundary + RTL")
            if now - last_sent > 10:
                last_sent = now
                self.mav.mav.set_position_target_global_int_send(
                    0,
                    target_system,
                    target_component,
                    mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                    type_mask,
                    int(loc.lat * 1.0e7),
                    int(loc.lng * 1.0e7),
                    0, # alt
                    0, # x-ve
                    0, # y-vel
                    0, # z-vel
                    0, # afx
                    0, # afy
                    0, # afz
                    0, # yaw,
                    0, # yaw-rate
                )
            m = self.mav.recv_match(blocking=True,
                                    timeout=1)
            if m is None:
                continue
            t = m.get_type()
            if t == "POSITION_TARGET_GLOBAL_INT":
                print("Target: (%s)" % str(m))
            elif t == "GLOBAL_POSITION_INT":
                print("Position: (%s)" % str(m))
            elif t == "FENCE_STATUS":
                print("Fence: %s" % str(m))
                if m.breach_status != 0:
                    seen_fence_breach = True
                    self.progress("Fence breach detected!")
                    if m.breach_type != mavutil.mavlink.FENCE_BREACH_BOUNDARY:
                        raise NotAchievedException("Breach of unexpected type")
            if self.mode_is("RTL", cached=True) and seen_fence_breach:
                break
        self.wait_distance_to_home(3, 7, timeout=30)

    def drive_somewhere_stop_at_boundary(self,
                                         loc,
                                         expected_stopping_point,
                                         expected_distance_epsilon=1.0,
                                         target_system=1,
                                         target_component=1,
                                         timeout=120):
        tstart = self.get_sim_time()
        last_sent = 0

        type_mask = (mavutil.mavlink.POSITION_TARGET_TYPEMASK_VX_IGNORE +
                     mavutil.mavlink.POSITION_TARGET_TYPEMASK_VY_IGNORE +
                     mavutil.mavlink.POSITION_TARGET_TYPEMASK_VZ_IGNORE +
                     mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE +
                     mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE +
                     mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE +
                     mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE +
                     mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE)

        self.change_mode('GUIDED')
        at_stopping_point = False
        while True:
            now = self.get_sim_time_cached()
            if now - tstart > timeout:
                raise NotAchievedException("Did not arrive and stop at boundary")
            if now - last_sent > 10:
                last_sent = now
                self.mav.mav.set_position_target_global_int_send(
                    0,
                    target_system,
                    target_component,
                    mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                    type_mask,
                    int(loc.lat * 1.0e7),
                    int(loc.lng * 1.0e7),
                    0, # alt
                    0, # x-ve
                    0, # y-vel
                    0, # z-vel
                    0, # afx
                    0, # afy
                    0, # afz
                    0, # yaw,
                    0, # yaw-rate
                )
            m = self.mav.recv_match(blocking=True,
                                    timeout=1)
            if m is None:
                continue
            t = m.get_type()
            if t == "POSITION_TARGET_GLOBAL_INT":
                print("Target: (%s)" % str(m))
            elif t == "GLOBAL_POSITION_INT":
                print("Position: (%s)" % str(m))
                delta = self.get_distance(
                    mavutil.location(m.lat * 1e-7, m.lon * 1e-7, 0, 0),
                    mavutil.location(expected_stopping_point.lat,
                                     expected_stopping_point.lng,
                                     0,
                                     0))
                print("delta: %s want_delta<%f" % (str(delta), expected_distance_epsilon))
                at_stopping_point = delta < expected_distance_epsilon
            elif t == "VFR_HUD":
                print("groundspeed: %f" % m.groundspeed)
                if at_stopping_point:
                    if m.groundspeed < 1:
                        self.progress("Seemed to have stopped at stopping point")
                        return

    def assert_fence_breached(self):
        m = self.assert_receive_message('FENCE_STATUS', timeout=10)
        if m.breach_status != 1:
            raise NotAchievedException("Expected to be breached")

    def wait_fence_not_breached(self, timeout=5):
        tstart = self.get_sim_time()
        while True:
            if self.get_sim_time_cached() - tstart > timeout:
                raise AutoTestTimeoutException("Fence remains breached")
            m = self.mav.recv_match(type='FENCE_STATUS',
                                    blocking=True,
                                    timeout=1)
            if m is None:
                self.progress("No FENCE_STATUS received")
                continue
            self.progress("STATUS: %s" % str(m))
            if m.breach_status == 0:
                break

    def test_poly_fence_noarms(self, target_system=1, target_component=1):
        '''various tests to ensure we can't arm when in breach of a polyfence'''
        self.start_subtest("Ensure PolyFence arming checks work")
        self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_FENCE,
                           target_system=target_system,
                           target_component=target_component)
        self.delay_sim_time(5) # let breaches clear
        # FIXME: should we allow this?
        self.progress("Ensure we can arm with no poly in place")
        self.change_mode("GUIDED")
        self.wait_ready_to_arm()
        self.arm_vehicle()
        self.disarm_vehicle()

        self.test_poly_fence_noarms_exclusion_circle(target_system=target_system,
                                                     target_component=target_component)
        self.test_poly_fence_noarms_inclusion_circle(target_system=target_system,
                                                     target_component=target_component)
        self.test_poly_fence_noarms_exclusion_polyfence(target_system=target_system,
                                                        target_component=target_component)
        self.test_poly_fence_noarms_inclusion_polyfence(target_system=target_system,
                                                        target_component=target_component)

    def test_poly_fence_noarms_exclusion_circle(self, target_system=1, target_component=1):
        self.start_subtest("Ensure not armable when within an exclusion circle")

        here = self.mav.location()

        items = [
            self.mav.mav.mission_item_int_encode(
                target_system,
                target_component,
                0, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION,
                0, # current
                0, # autocontinue
                5, # p1 - radius
                0, # p2
                0, # p3
                0, # p4
                int(here.lat * 1e7), # latitude
                int(here.lng * 1e7), # longitude
                33.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_FENCE),
            self.mav.mav.mission_item_int_encode(
                target_system,
                target_component,
                1, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION,
                0, # current
                0, # autocontinue
                5, # p1 - radius
                0, # p2
                0, # p3
                0, # p4
                int(self.offset_location_ne(here, 100, 100).lat * 1e7), # latitude
                int(here.lng * 1e7), # longitude
                33.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_FENCE),
        ]
        self.upload_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE,
                                           items)
        self.delay_sim_time(5) # ArduPilot only checks for breaches @1Hz
        self.drain_mav()
        self.assert_fence_breached()
        try:
            self.arm_motors_with_rc_input()
        except NotAchievedException:
            pass
        if self.armed():
            raise NotAchievedException(
                "Armed when within exclusion zone")

        self.upload_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE,
                                           [])
        self.wait_fence_not_breached()

    def test_poly_fence_noarms_inclusion_circle(self, target_system=1, target_component=1):
        self.start_subtest("Ensure not armable when outside an inclusion circle (but within another")

        here = self.mav.location()

        items = [
            self.mav.mav.mission_item_int_encode(
                target_system,
                target_component,
                0, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION,
                0, # current
                0, # autocontinue
                5, # p1 - radius
                0, # p2
                0, # p3
                0, # p4
                int(here.lat * 1e7), # latitude
                int(here.lng * 1e7), # longitude
                33.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_FENCE),
            self.mav.mav.mission_item_int_encode(
                target_system,
                target_component,
                1, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION,
                0, # current
                0, # autocontinue
                5, # p1 - radius
                0, # p2
                0, # p3
                0, # p4
                int(self.offset_location_ne(here, 100, 100).lat * 1e7), # latitude
                int(here.lng * 1e7), # longitude
                33.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_FENCE),
        ]
        self.upload_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE,
                                           items)
        self.delay_sim_time(5) # ArduPilot only checks for breaches @1Hz
        self.drain_mav()
        self.assert_fence_breached()
        try:
            self.arm_motors_with_rc_input()
        except NotAchievedException:
            pass
        if self.armed():
            raise NotAchievedException(
                "Armed when outside an inclusion zone")

        self.upload_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE,
                                           [])
        self.wait_fence_not_breached()

    def test_poly_fence_noarms_exclusion_polyfence(self, target_system=1, target_component=1):
        self.start_subtest("Ensure not armable when inside an exclusion polyfence (but outside another")

        here = self.mav.location()

        self.upload_fences_from_locations(
            mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION,
            [
                [ # east
                    self.offset_location_ne(here, -50, 20), # bl
                    self.offset_location_ne(here, 50, 20), # br
                    self.offset_location_ne(here, 50, 40), # tr
                    self.offset_location_ne(here, -50, 40), # tl,
                ], [ # over the top of the vehicle
                    self.offset_location_ne(here, -50, -50), # bl
                    self.offset_location_ne(here, -50, 50), # br
                    self.offset_location_ne(here, 50, 50), # tr
                    self.offset_location_ne(here, 50, -50), # tl,
                ]
            ]
        )
        self.delay_sim_time(5) # ArduPilot only checks for breaches @1Hz
        self.drain_mav()
        self.assert_fence_breached()
        try:
            self.arm_motors_with_rc_input()
        except NotAchievedException:
            pass
        if self.armed():
            raise NotAchievedException(
                "Armed when within polygon exclusion zone")

        self.upload_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE,
                                           [])
        self.wait_fence_not_breached()

    def test_poly_fence_noarms_inclusion_polyfence(self, target_system=1, target_component=1):
        self.start_subtest("Ensure not armable when outside an inclusion polyfence (but within another")

        here = self.mav.location()

        self.upload_fences_from_locations(
            mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION,
            [
                [ # east
                    self.offset_location_ne(here, -50, 20), # bl
                    self.offset_location_ne(here, 50, 20), # br
                    self.offset_location_ne(here, 50, 40), # tr
                    self.offset_location_ne(here, -50, 40), # tl,
                ], [ # over the top of the vehicle
                    self.offset_location_ne(here, -50, -50), # bl
                    self.offset_location_ne(here, -50, 50), # br
                    self.offset_location_ne(here, 50, 50), # tr
                    self.offset_location_ne(here, 50, -50), # tl,
                ]
            ]
        )
        self.delay_sim_time(5) # ArduPilot only checks for breaches @1Hz
        self.drain_mav()
        self.assert_fence_breached()
        try:
            self.arm_motors_with_rc_input()
        except NotAchievedException:
            pass
        if self.armed():
            raise NotAchievedException(
                "Armed when outside polygon inclusion zone")

        self.upload_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE,
                                           [])
        self.wait_fence_not_breached()

    def test_fence_upload_timeouts_1(self, target_system=1, target_component=1):
        self.start_subtest("fence_upload timeouts 1")
        self.progress("Telling victim there will be one item coming")
        self.mav.mav.mission_count_send(target_system,
                                        target_component,
                                        1,
                                        mavutil.mavlink.MAV_MISSION_TYPE_FENCE)
        m = self.mav.recv_match(type=['MISSION_REQUEST', 'MISSION_ACK'],
                                blocking=True,
                                timeout=1)
        self.progress("Got (%s)" % str(m))
        if m is None:
            raise NotAchievedException("Did not get ACK or mission request")

        if m.get_type() == "MISSION_ACK":
            raise NotAchievedException("Expected MISSION_REQUEST")

        if m.seq != 0:
            raise NotAchievedException("Expected request for seq=0")

        if m.target_system != self.mav.mav.srcSystem:
            raise NotAchievedException("Incorrect target system in MISSION_REQUEST")
        if m.target_component != self.mav.mav.srcComponent:
            raise NotAchievedException("Incorrect target component in MISSION_REQUEST")
        tstart = self.get_sim_time()
        rerequest_count = 0
        received_text = False
        received_ack = False
        while True:
            if received_ack and received_text:
                break
            if self.get_sim_time_cached() - tstart > 10:
                raise NotAchievedException("Did not get expected ack and statustext")
            m = self.mav.recv_match(type=['MISSION_REQUEST', 'MISSION_ACK', 'STATUSTEXT'],
                                    blocking=True,
                                    timeout=1)
            self.progress("Got (%s)" % str(m))
            if m is None:
                self.progress("Did not receive any messages")
                continue
            if m.get_type() == "MISSION_REQUEST":
                if m.seq != 0:
                    raise NotAchievedException("Received request for invalid seq")
                if m.target_system != self.mav.mav.srcSystem:
                    raise NotAchievedException("Incorrect target system in MISSION_REQUEST")
                if m.target_component != self.mav.mav.srcComponent:
                    raise NotAchievedException("Incorrect target component in MISSION_REQUEST")
                rerequest_count += 1
                self.progress("Valid re-request received.")
                continue
            if m.get_type() == "MISSION_ACK":
                if m.mission_type != mavutil.mavlink.MAV_MISSION_TYPE_FENCE:
                    raise NotAchievedException("Wrong mission type")
                if m.type != mavutil.mavlink.MAV_MISSION_OPERATION_CANCELLED:
                    raise NotAchievedException("Wrong result")
                received_ack = True
                continue
            if m.get_type() == "STATUSTEXT":
                if "upload time" in m.text:
                    received_text = True
                continue
        if rerequest_count < 3:
            raise NotAchievedException("Expected several re-requests of mission item")
        self.end_subtest("fence upload timeouts 1")

    def expect_request_for_item(self, item):
        m = self.mav.recv_match(type=['MISSION_REQUEST', 'MISSION_ACK'],
                                blocking=True,
                                timeout=1)
        self.progress("Got (%s)" % str(m))
        if m is None:
            raise NotAchievedException("Did not get ACK or mission request")

        if m.get_type() == "MISSION_ACK":
            raise NotAchievedException("Expected MISSION_REQUEST")

        if m.seq != item.seq:
            raise NotAchievedException("Expected request for seq=%u" % item.seq)

        if m.target_system != self.mav.mav.srcSystem:
            raise NotAchievedException("Incorrect target system in MISSION_REQUEST")
        if m.target_component != self.mav.mav.srcComponent:
            raise NotAchievedException("Incorrect target component in MISSION_REQUEST")

    def test_fence_upload_timeouts_2(self, target_system=1, target_component=1):
        self.start_subtest("fence upload timeouts 2")
        self.progress("Telling victim there will be two items coming")
        # avoid a timeout race condition where ArduPilot re-requests a
        # fence point before we receive and respond to the first one.
        # Since ArduPilot has a 1s timeout on re-requesting, This only
        # requires a round-trip delay of 1/speedup seconds to trigger
        # - and that has been seen in practise on Travis
        old_speedup = self.get_parameter("SIM_SPEEDUP")
        self.set_parameter("SIM_SPEEDUP", 1)
        self.mav.mav.mission_count_send(target_system,
                                        target_component,
                                        2,
                                        mavutil.mavlink.MAV_MISSION_TYPE_FENCE)
        self.progress("Sending item with seq=0")
        item = self.mav.mav.mission_item_int_encode(
            target_system,
            target_component,
            0, # seq
            mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
            mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION,
            0, # current
            0, # autocontinue
            1, # p1 radius
            0, # p2
            0, # p3
            0, # p4
            int(1.1 * 1e7), # latitude
            int(1.2 * 1e7), # longitude
            33.0000, # altitude
            mavutil.mavlink.MAV_MISSION_TYPE_FENCE)
        self.expect_request_for_item(item)
        item.pack(self.mav.mav)
        self.mav.mav.send(item)

        self.progress("Sending item with seq=1")
        item = self.mav.mav.mission_item_int_encode(
            target_system,
            target_component,
            1, # seq
            mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
            mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION,
            0, # current
            0, # autocontinue
            1, # p1 radius
            0, # p2
            0, # p3
            0, # p4
            int(1.1 * 1e7), # latitude
            int(1.2 * 1e7), # longitude
            33.0000, # altitude
            mavutil.mavlink.MAV_MISSION_TYPE_FENCE)

        self.expect_request_for_item(item)

        self.set_parameter("SIM_SPEEDUP", old_speedup)

        self.progress("Now waiting for a timeout")
        tstart = self.get_sim_time()
        rerequest_count = 0
        received_text = False
        received_ack = False
        while True:
            if received_ack and received_text:
                break
            if self.get_sim_time_cached() - tstart > 10:
                raise NotAchievedException("Did not get expected ack and statustext")
            m = self.mav.recv_match(type=['MISSION_REQUEST', 'MISSION_ACK', 'STATUSTEXT'],
                                    blocking=True,
                                    timeout=0.1)
            self.progress("Got (%s)" % str(m))
            if m is None:
                self.progress("Did not receive any messages")
                continue
            if m.get_type() == "MISSION_REQUEST":
                if m.seq != 1:
                    raise NotAchievedException("Received request for invalid seq")
                if m.target_system != self.mav.mav.srcSystem:
                    raise NotAchievedException("Incorrect target system in MISSION_REQUEST")
                if m.target_component != self.mav.mav.srcComponent:
                    raise NotAchievedException("Incorrect target component in MISSION_REQUEST")
                rerequest_count += 1
                self.progress("Valid re-request received.")
                continue
            if m.get_type() == "MISSION_ACK":
                if m.mission_type != mavutil.mavlink.MAV_MISSION_TYPE_FENCE:
                    raise NotAchievedException("Wrong mission type")
                if m.type != mavutil.mavlink.MAV_MISSION_OPERATION_CANCELLED:
                    raise NotAchievedException("Wrong result")
                received_ack = True
                continue
            if m.get_type() == "STATUSTEXT":
                if "upload time" in m.text:
                    received_text = True
                continue
        if rerequest_count < 3:
            raise NotAchievedException("Expected several re-requests of mission item")
        self.end_subtest("fence upload timeouts 2")

    def test_fence_upload_timeouts(self, target_system=1, target_component=1):
        self.test_fence_upload_timeouts_1(target_system=target_system,
                                          target_component=target_component)
        self.test_fence_upload_timeouts_2(target_system=target_system,
                                          target_component=target_component)

    def test_poly_fence_compatability_ordering(self, target_system=1, target_component=1):
        self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_FENCE,
                           target_system=target_system,
                           target_component=target_component)
        here = self.mav.location()
        self.progress("try uploading return point last")
        self.roundtrip_fence_using_fencepoint_protocol([
            self.offset_location_ne(here, 0, 0), # bl // return point
            self.offset_location_ne(here, -50, 20), # bl
            self.offset_location_ne(here, 50, 20), # br
            self.offset_location_ne(here, 50, 40), # tr
            self.offset_location_ne(here, -50, 40), # tl,
            self.offset_location_ne(here, -50, 20), # closing point
        ], ordering=[1, 2, 3, 4, 5, 0])
        self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_FENCE,
                           target_system=target_system,
                           target_component=target_component)

        self.progress("try uploading return point in middle")
        self.roundtrip_fence_using_fencepoint_protocol([
            self.offset_location_ne(here, 0, 0), # bl // return point
            self.offset_location_ne(here, -50, 20), # bl
            self.offset_location_ne(here, 50, 20), # br
            self.offset_location_ne(here, 50, 40), # tr
            self.offset_location_ne(here, -50, 40), # tl,
            self.offset_location_ne(here, -50, 20), # closing point
        ], ordering=[1, 2, 3, 0, 4, 5])
        self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_FENCE,
                           target_system=target_system,
                           target_component=target_component)

        self.progress("try closing point in middle")
        self.roundtrip_fence_using_fencepoint_protocol([
            self.offset_location_ne(here, 0, 0), # bl // return point
            self.offset_location_ne(here, -50, 20), # bl
            self.offset_location_ne(here, 50, 20), # br
            self.offset_location_ne(here, 50, 40), # tr
            self.offset_location_ne(here, -50, 40), # tl,
            self.offset_location_ne(here, -50, 20), # closing point
        ], ordering=[0, 1, 2, 5, 3, 4])
        self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_FENCE,
                           target_system=target_system,
                           target_component=target_component)

        # this is expected to fail as we don't return the closing
        # point correctly until the first is uploaded
        # self.progress("try closing point first")
        # failed = False
        # try:
        #     self.roundtrip_fence_using_fencepoint_protocol([
        #         self.offset_location_ne(here, 0, 0), # bl // return point
        #         self.offset_location_ne(here, -50, 20), # bl
        #         self.offset_location_ne(here, 50, 20), # br
        #         self.offset_location_ne(here, 50, 40), # tr
        #         self.offset_location_ne(here, -50, 40), # tl,
        #         self.offset_location_ne(here, -50, 20), # closing point
        #     ], ordering=[5, 0, 1, 2, 3, 4])
        # except NotAchievedException as e:
        #     failed = "got=0.000000 want=" in str(e)
        # if not failed:
        #     raise NotAchievedException("Expected failure, did not get it")
        # self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_FENCE,
        #                    target_system=target_system,
        #                    target_component=target_component)

        self.progress("try (almost) reverse order")
        self.roundtrip_fence_using_fencepoint_protocol([
            self.offset_location_ne(here, 0, 0), # bl // return point
            self.offset_location_ne(here, -50, 20), # bl
            self.offset_location_ne(here, 50, 20), # br
            self.offset_location_ne(here, 50, 40), # tr
            self.offset_location_ne(here, -50, 40), # tl,
            self.offset_location_ne(here, -50, 20), # closing point
        ], ordering=[4, 3, 2, 1, 0, 5])
        self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_FENCE,
                           target_system=target_system,
                           target_component=target_component)

    def test_poly_fence_big_then_small(self, target_system=1, target_component=1):
        here = self.mav.location()

        self.roundtrip_fence_using_fencepoint_protocol([
            self.offset_location_ne(here, 0, 0), # bl // return point
            self.offset_location_ne(here, -50, 20), # bl
            self.offset_location_ne(here, 50, 20), # br
            self.offset_location_ne(here, 50, 40), # tr
            self.offset_location_ne(here, -50, 40), # tl,
            self.offset_location_ne(here, -50, 20), # closing point
        ], ordering=[1, 2, 3, 4, 5, 0])
        downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE)
        if len(downloaded_items) != 5:
            # that's one return point and then bl, br, tr, then tl
            raise NotAchievedException("Bad number of downloaded items in original download")

        self.roundtrip_fence_using_fencepoint_protocol([
            self.offset_location_ne(here, 0, 0), # bl // return point
            self.offset_location_ne(here, -50, 20), # bl
            self.offset_location_ne(here, 50, 40), # tr
            self.offset_location_ne(here, -50, 40), # tl,
            self.offset_location_ne(here, -50, 20), # closing point
        ], ordering=[1, 2, 3, 4, 0])

        downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE)
        want_count = 4
        if len(downloaded_items) != want_count:
            # that's one return point and then bl, tr, then tl
            raise NotAchievedException("Bad number of downloaded items in second download got=%u wanted=%u" %
                                       (len(downloaded_items), want_count))
        downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE)
        if len(downloaded_items) != 4:
            # that's one return point and then bl, tr, then tl
            raise NotAchievedException("Bad number of downloaded items in second download (second time) got=%u want=%u" %
                                       (len(downloaded_items), want_count))

    def test_poly_fence_compatability(self, target_system=1, target_component=1):
        self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_FENCE,
                           target_system=target_system,
                           target_component=target_component)

        self.test_poly_fence_compatability_ordering(target_system=target_system, target_component=target_component)

        here = self.mav.location()

        self.progress("Playing with changing point count")
        self.roundtrip_fence_using_fencepoint_protocol(
            [
                self.offset_location_ne(here, 0, 0), # bl // return point
                self.offset_location_ne(here, -50, 20), # bl
                self.offset_location_ne(here, 50, 20), # br
                self.offset_location_ne(here, 50, 40), # tr
                self.offset_location_ne(here, -50, 40), # tl,
                self.offset_location_ne(here, -50, 20), # closing point
            ])
        self.roundtrip_fence_using_fencepoint_protocol(
            [
                self.offset_location_ne(here, 0, 0), # bl // return point
                self.offset_location_ne(here, -50, 20), # bl
                self.offset_location_ne(here, 50, 20), # br
                self.offset_location_ne(here, -50, 40), # tl,
                self.offset_location_ne(here, -50, 20), # closing point
            ])
        self.roundtrip_fence_using_fencepoint_protocol(
            [
                self.offset_location_ne(here, 0, 0), # bl // return point
                self.offset_location_ne(here, -50, 20), # bl
                self.offset_location_ne(here, 50, 20), # br
                self.offset_location_ne(here, 50, 40), # tr
                self.offset_location_ne(here, -50, 40), # tl,
                self.offset_location_ne(here, -50, 20), # closing point
            ])

    def test_poly_fence_reboot_survivability(self):
        here = self.mav.location()

        self.upload_fences_from_locations(
            mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION,
            [
                [ # east
                    self.offset_location_ne(here, -50, 20), # bl
                    self.offset_location_ne(here, 50, 20), # br
                    self.offset_location_ne(here, 50, 40), # tr
                    self.offset_location_ne(here, -50, 40), # tl,
                ], [ # over the top of the vehicle
                    self.offset_location_ne(here, -50, -50), # bl
                    self.offset_location_ne(here, -50, 50), # br
                    self.offset_location_ne(here, 50, 50), # tr
                    self.offset_location_ne(here, 50, -50), # tl,
                ]
            ]
        )
        self.reboot_sitl()
        downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE)
        downloaded_len = len(downloaded_items)
        if downloaded_len != 8:
            raise NotAchievedException("Items did not survive reboot (want=%u got=%u)" %
                                       (8, downloaded_len))

    def PolyFence(self):
        '''test fence-related functions'''
        target_system = 1
        target_component = 1

        self.change_mode("LOITER")
        self.wait_ready_to_arm()
        here = self.mav.location()
        self.progress("here: %f %f" % (here.lat, here.lng))
        self.set_parameters({
            "FENCE_ENABLE": 1,
            "AVOID_ENABLE": 0,
        })

#        self.set_parameter("SIM_SPEEDUP", 1)

        self.test_poly_fence_big_then_small()

        self.test_poly_fence_compatability()

        self.test_fence_upload_timeouts()

        self.test_poly_fence_noarms(target_system=target_system, target_component=target_component)

        self.arm_vehicle()

        self.test_poly_fence_inclusion(here, target_system=target_system, target_component=target_component)
        self.test_poly_fence_exclusion(here, target_system=target_system, target_component=target_component)

        self.disarm_vehicle()

        self.test_poly_fence_reboot_survivability()

    def test_poly_fence_inclusion_overlapping_inclusion_circles(self, here, target_system=1, target_component=1):
        self.start_subtest("Overlapping circular inclusion")
        self.upload_fences_from_locations(
            mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION,
            [
                {
                    "radius": 30,
                    "loc": self.offset_location_ne(here, -20, 0),
                },
                {
                    "radius": 30,
                    "loc": self.offset_location_ne(here, 20, 0),
                },
            ])
        if self.mavproxy is not None:
            # handy for getting pretty pictures
            self.mavproxy.send("fence list\n")

        self.delay_sim_time(5)
        self.progress("Drive outside top circle")
        fence_middle = self.offset_location_ne(here, -150, 0)
        self.drive_somewhere_breach_boundary_and_rtl(
            fence_middle,
            target_system=target_system,
            target_component=target_component)

        self.delay_sim_time(5)
        self.progress("Drive outside bottom circle")
        fence_middle = self.offset_location_ne(here, 150, 0)
        self.drive_somewhere_breach_boundary_and_rtl(
            fence_middle,
            target_system=target_system,
            target_component=target_component)

    def test_poly_fence_inclusion(self, here, target_system=1, target_component=1):
        self.progress("Circle and Polygon inclusion")
        self.test_poly_fence_inclusion_overlapping_inclusion_circles(
            here,
            target_system=target_system,
            target_component=target_component)

        self.upload_fences_from_locations(
            mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION,
            [
                [
                    self.offset_location_ne(here, -40, -20), # tl
                    self.offset_location_ne(here, 50, -20), # tr
                    self.offset_location_ne(here, 50, 20), # br
                    self.offset_location_ne(here, -40, 20), # bl,
                ],
                {
                    "radius": 30,
                    "loc": self.offset_location_ne(here, -20, 0),
                },
            ])

        self.delay_sim_time(5)
        if self.mavproxy is not None:
            self.mavproxy.send("fence list\n")
        self.progress("Drive outside polygon")
        fence_middle = self.offset_location_ne(here, -150, 0)
        self.drive_somewhere_breach_boundary_and_rtl(
            fence_middle,
            target_system=target_system,
            target_component=target_component)

        self.delay_sim_time(5)
        self.progress("Drive outside circle")
        fence_middle = self.offset_location_ne(here, 150, 0)
        self.drive_somewhere_breach_boundary_and_rtl(
            fence_middle,
            target_system=target_system,
            target_component=target_component)

        self.upload_fences_from_locations(
            mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION,
            [
                [
                    self.offset_location_ne(here, -20, -25), # tl
                    self.offset_location_ne(here, 50, -25), # tr
                    self.offset_location_ne(here, 50, 15), # br
                    self.offset_location_ne(here, -20, 15), # bl,
                ],
                [
                    self.offset_location_ne(here, 20, -20), # tl
                    self.offset_location_ne(here, -50, -20), # tr
                    self.offset_location_ne(here, -50, 20), # br
                    self.offset_location_ne(here, 20, 20), # bl,
                ],
            ])

        self.delay_sim_time(5)
        if self.mavproxy is not None:
            self.mavproxy.send("fence list\n")
        self.progress("Drive outside top polygon")
        fence_middle = self.offset_location_ne(here, -150, 0)
        self.drive_somewhere_breach_boundary_and_rtl(
            fence_middle,
            target_system=target_system,
            target_component=target_component)

        self.delay_sim_time(5)
        self.progress("Drive outside bottom polygon")
        fence_middle = self.offset_location_ne(here, 150, 0)
        self.drive_somewhere_breach_boundary_and_rtl(
            fence_middle,
            target_system=target_system,
            target_component=target_component)

    def test_poly_fence_exclusion(self, here, target_system=1, target_component=1):

        self.upload_fences_from_locations(
            mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION,
            [
                [ # east
                    self.offset_location_ne(here, -50, 20), # bl
                    self.offset_location_ne(here, 50, 20), # br
                    self.offset_location_ne(here, 50, 40), # tr
                    self.offset_location_ne(here, -50, 40), # tl,
                ], [ # west
                    self.offset_location_ne(here, -50, -20), # tl
                    self.offset_location_ne(here, 50, -20), # tr
                    self.offset_location_ne(here, 50, -40), # br
                    self.offset_location_ne(here, -50, -40), # bl,
                ], {
                    "radius": 30,
                    "loc": self.offset_location_ne(here, -60, 0),
                },
            ])
        self.delay_sim_time(5)
        if self.mavproxy is not None:
            self.mavproxy.send("fence list\n")

        self.progress("Breach eastern boundary")
        fence_middle = self.offset_location_ne(here, 0, 30)
        self.drive_somewhere_breach_boundary_and_rtl(fence_middle,
                                                     target_system=target_system,
                                                     target_component=target_component)

        self.progress("delaying - hack to work around manual recovery bug")
        self.delay_sim_time(5)

        self.progress("Breach western boundary")
        fence_middle = self.offset_location_ne(here, 0, -30)
        self.drive_somewhere_breach_boundary_and_rtl(fence_middle,
                                                     target_system=target_system,
                                                     target_component=target_component)

        self.progress("delaying - hack to work around manual recovery bug")
        self.delay_sim_time(5)

        self.progress("Breach southern circle")
        fence_middle = self.offset_location_ne(here, -150, 0)
        self.drive_somewhere_breach_boundary_and_rtl(fence_middle,
                                                     target_system=target_system,
                                                     target_component=target_component)

    def SmartRTL(self):
        '''Test SmartRTL'''
        self.change_mode("STEERING")
        self.wait_ready_to_arm()
        self.arm_vehicle()
        # drive two sides of a square, make sure we don't go back through
        # the middle of the square
        self.progress("Driving North")
        self.reach_heading_manual(0)
        self.set_rc(3, 2000)
        self.delay_sim_time(5)
        self.set_rc(3, 1000)
        self.wait_groundspeed(0, 1)
        loc = self.mav.location()
        self.progress("Driving East")
        self.set_rc(3, 2000)
        self.reach_heading_manual(90)
        self.set_rc(3, 2000)
        self.delay_sim_time(5)
        self.set_rc(3, 1000)

        self.progress("Entering smartrtl")
        self.change_mode("SMART_RTL")

        self.progress("Ensure we go via intermediate point")
        self.wait_distance_to_location(loc, 0, 5, timeout=60)

        self.progress("Ensure we get home")
        self.wait_distance_to_home(3, 7, timeout=30)

        self.disarm_vehicle()

    def MotorTest(self):
        '''Motor Test triggered via mavlink'''
        magic_throttle_value = 1812
        self.wait_ready_to_arm()
        self.run_cmd(
            mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST,
            1, # p1 - motor instance
            mavutil.mavlink.MOTOR_TEST_THROTTLE_PWM, # p2 - throttle type
            magic_throttle_value, # p3 - throttle
            5, # p4 - timeout
            1, # p5 - motor count
            0, # p6 - test order (see MOTOR_TEST_ORDER)
            0, # p7
        )
        self.wait_armed()
        self.progress("Waiting for magic throttle value")
        self.wait_servo_channel_value(3, magic_throttle_value)
        self.wait_servo_channel_value(3, self.get_parameter("RC3_TRIM", 5), timeout=10)
        self.wait_disarmed()

    def test_poly_fence_object_avoidance_guided(self, target_system=1, target_component=1):
        if not self.mavproxy_can_do_mision_item_protocols():
            return

        self.test_poly_fence_object_avoidance_guided_pathfinding(
            target_system=target_system,
            target_component=target_component)
        return
        # twosquares is currently disabled because of the requirement to have an inclusion fence (which it doesn't have ATM)
        # self.test_poly_fence_object_avoidance_guided_two_squares(
        #     target_system=target_system,
        #     target_component=target_component)

    def test_poly_fence_object_avoidance_auto(self, target_system=1, target_component=1):
        self.load_fence("rover-path-planning-fence.txt")
        self.load_mission("rover-path-planning-mission.txt")
        self.context_push()
        ex = None
        try:
            self.set_parameters({
                "AVOID_ENABLE": 3,
                "OA_TYPE": 2,
                "FENCE_MARGIN": 0, # FIXME: https://github.com/ArduPilot/ardupilot/issues/11601
            })
            self.reboot_sitl()
            self.change_mode('AUTO')
            self.wait_ready_to_arm()
            self.arm_vehicle()
            self.set_parameter("FENCE_ENABLE", 1)
            if self.mavproxy is not None:
                self.mavproxy.send("fence list\n")
            # target_loc is copied from the mission file
            target_loc = mavutil.location(40.073799, -105.229156)
            self.wait_location(target_loc, timeout=300)
            # mission has RTL as last item
            self.wait_distance_to_home(3, 7, timeout=300)
            self.disarm_vehicle()
        except Exception as e:
            self.print_exception_caught(e)
            ex = e
        self.context_pop()
        self.reboot_sitl()
        if ex is not None:
            raise ex

    def send_guided_mission_item(self, loc, target_system=1, target_component=1):
        self.mav.mav.mission_item_send(
            target_system,
            target_component,
            0,
            mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
            mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
            2, # current
            0, # autocontinue
            0, # param1
            0, # param2
            0, # param3
            0, # param4
            loc.lat, # x
            loc.lng, # y
            0 # z
        )

    def test_poly_fence_object_avoidance_guided_pathfinding(self, target_system=1, target_component=1):
        self.load_fence("rover-path-planning-fence.txt")
        self.context_push()
        ex = None
        try:
            self.set_parameters({
                "AVOID_ENABLE": 3,
                "OA_TYPE": 2,
                "FENCE_MARGIN": 0, # FIXME: https://github.com/ArduPilot/ardupilot/issues/11601
            })
            self.reboot_sitl()
            self.change_mode('GUIDED')
            self.wait_ready_to_arm()
            self.arm_vehicle()
            self.set_parameter("FENCE_ENABLE", 1)
            if self.mavproxy is not None:
                self.mavproxy.send("fence list\n")
            target_loc = mavutil.location(40.073800, -105.229172)
            self.send_guided_mission_item(target_loc,
                                          target_system=target_system,
                                          target_component=target_component)
            self.wait_location(target_loc, timeout=300)
            self.do_RTL(timeout=300)
            self.disarm_vehicle()
        except Exception as e:
            self.print_exception_caught(e)
            ex = e
        self.context_pop()
        self.reboot_sitl()
        if ex is not None:
            raise ex

    def WheelEncoders(self):
        '''make sure wheel encoders are generally working'''
        ex = None
        try:
            self.set_parameters({
                "WENC_TYPE": 10,
                "EK3_ENABLE": 1,
                "AHRS_EKF_TYPE": 3,
            })
            self.reboot_sitl()
            self.change_mode("LOITER")
            self.wait_ready_to_arm()
            self.change_mode("MANUAL")
            self.arm_vehicle()
            self.set_rc(3, 1600)

            m = self.assert_receive_message('WHEEL_DISTANCE', timeout=5)

            tstart = self.get_sim_time()
            while True:
                if self.get_sim_time_cached() - tstart > 10:
                    break
                dist_home = self.distance_to_home(use_cached_home=True)
                m = self.mav.messages.get("WHEEL_DISTANCE")
                delta = abs(m.distance[0] - dist_home)
                self.progress("dist-home=%f wheel-distance=%f delta=%f" %
                              (dist_home, m.distance[0], delta))
                if delta > 5:
                    raise NotAchievedException("wheel distance incorrect")
            self.disarm_vehicle()
        except Exception as e:
            self.print_exception_caught(e)
            self.disarm_vehicle()
            ex = e
        self.reboot_sitl()
        if ex is not None:
            raise ex

    def test_poly_fence_object_avoidance_guided_two_squares(self, target_system=1, target_component=1):
        self.start_subtest("Ensure we can steer around obstacles in guided mode")
        here = self.mav.location()
        self.upload_fences_from_locations(
            mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION,
            [
                [ # east
                    self.offset_location_ne(here, -50, 20), # bl
                    self.offset_location_ne(here, 50, 10), # tl
                    self.offset_location_ne(here, 50, 30), # tr
                    self.offset_location_ne(here, -50, 40), # br,
                ],
                [ # further east (and south
                    self.offset_location_ne(here, -60, 60), # bl
                    self.offset_location_ne(here, 40, 70), # tl
                    self.offset_location_ne(here, 40, 90), # tr
                    self.offset_location_ne(here, -60, 80), # br,
                ],
            ])
        if self.mavproxy is not None:
            self.mavproxy.send("fence list\n")
        self.context_push()
        ex = None
        try:
            self.set_parameters({
                "AVOID_ENABLE": 3,
                "OA_TYPE": 2,
            })
            self.reboot_sitl()
            self.change_mode('GUIDED')
            self.wait_ready_to_arm()
            self.set_parameter("FENCE_ENABLE", 1)
            if self.mavproxy is not None:
                self.mavproxy.send("fence list\n")
            self.arm_vehicle()

            self.change_mode("GUIDED")
            target = mavutil.location(40.071382, -105.228340, 0, 0)
            self.send_guided_mission_item(target,
                                          target_system=target_system,
                                          target_component=target_component)
            self.wait_location(target, timeout=300)
            self.do_RTL()
            self.disarm_vehicle()
        except Exception as e:
            self.print_exception_caught(e)
            ex = e
        self.context_pop()
        self.reboot_sitl()
        if ex is not None:
            raise ex

    def test_poly_fence_avoidance_dont_breach_exclusion(self, target_system=1, target_component=1):
        self.start_subtest("Ensure we stop before breaching an exclusion fence")
        here = self.mav.location()
        self.upload_fences_from_locations(
            mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION,
            [
                [ # east
                    self.offset_location_ne(here, -50, 20), # bl
                    self.offset_location_ne(here, 50, 20), # br
                    self.offset_location_ne(here, 50, 40), # tr
                    self.offset_location_ne(here, -50, 40), # tl,
                ], [ # west
                    self.offset_location_ne(here, -50, -20), # tl
                    self.offset_location_ne(here, 50, -20), # tr
                    self.offset_location_ne(here, 50, -40), # br
                    self.offset_location_ne(here, -50, -40), # bl,
                ], {
                    "radius": 30,
                    "loc": self.offset_location_ne(here, -60, 0),
                },
            ])
        if self.mavproxy is not None:
            self.mavproxy.send("fence list\n")
        self.set_parameters({
            "FENCE_ENABLE": 1,
            "AVOID_ENABLE": 3,
        })
        fence_middle = self.offset_location_ne(here, 0, 30)
        # FIXME: this might be nowhere near "here"!
        expected_stopping_point = mavutil.location(40.0713376, -105.2295738, 0, 0)
        self.drive_somewhere_stop_at_boundary(
            fence_middle,
            expected_stopping_point,
            target_system=target_system,
            target_component=target_component,
            expected_distance_epsilon=3)
        self.set_parameter("AVOID_ENABLE", 0)
        self.do_RTL()

    def do_RTL(self, distance_min=3, distance_max=7, timeout=60):
        self.change_mode("RTL")
        self.wait_distance_to_home(distance_min, distance_max, timeout=timeout)

    def PolyFenceAvoidance(self, target_system=1, target_component=1):
        '''PolyFence avoidance tests'''
        self.change_mode("LOITER")
        self.wait_ready_to_arm()
        self.arm_vehicle()
        self.change_mode("MANUAL")
        self.reach_heading_manual(180, turn_right=False)
        self.change_mode("GUIDED")

        self.test_poly_fence_avoidance_dont_breach_exclusion(target_system=target_system, target_component=target_component)

        self.disarm_vehicle()

    def test_poly_fence_object_avoidance_guided_bendy_ruler(self, target_system=1, target_component=1):
        if not self.mavproxy_can_do_mision_item_protocols():
            return
        self.load_fence("rover-path-bendyruler-fence.txt")
        self.context_push()
        ex = None
        try:
            self.set_parameters({
                "AVOID_ENABLE": 3,
                "OA_TYPE": 1,
                "OA_LOOKAHEAD": 50,
            })
            self.reboot_sitl()
            self.change_mode('GUIDED')
            self.wait_ready_to_arm()
            self.arm_vehicle()
            self.set_parameters({
                "FENCE_ENABLE": 1,
                "WP_RADIUS": 5,
            })
            if self.mavproxy is not None:
                self.mavproxy.send("fence list\n")
            target_loc = mavutil.location(40.071060, -105.227734, 0, 0)
            self.send_guided_mission_item(target_loc,
                                          target_system=target_system,
                                          target_component=target_component)
            # FIXME: we don't get within WP_RADIUS of our target?!
            self.wait_location(target_loc, timeout=300, accuracy=15)
            self.do_RTL(timeout=300)
            self.disarm_vehicle()
        except Exception as e:
            self.print_exception_caught(e)
            ex = e
        self.context_pop()
        self.disarm_vehicle()
        self.reboot_sitl()
        if ex is not None:
            raise ex

    def PolyFenceObjectAvoidanceBendyRulerEasier(self, target_system=1, target_component=1):
        '''PolyFence object avoidance tests - easier bendy ruler test'''
        if not self.mavproxy_can_do_mision_item_protocols():
            return
        self.test_poly_fence_object_avoidance_auto_bendy_ruler_easier(
            target_system=target_system, target_component=target_component)
        self.test_poly_fence_object_avoidance_guided_bendy_ruler_easier(
            target_system=target_system, target_component=target_component)

    def test_poly_fence_object_avoidance_guided_bendy_ruler_easier(self, target_system=1, target_component=1):
        '''finish-line issue means we can't complete the harder one.  This
        test can go away once we've nailed that one.  The only
        difference here is the target point.
        '''
        if not self.mavproxy_can_do_mision_item_protocols():
            return
        self.load_fence("rover-path-bendyruler-fence.txt")
        self.context_push()
        ex = None
        try:
            self.set_parameters({
                "AVOID_ENABLE": 3,
                "OA_TYPE": 1,
                "OA_LOOKAHEAD": 50,
            })
            self.reboot_sitl()
            self.change_mode('GUIDED')
            self.wait_ready_to_arm()
            self.arm_vehicle()
            self.set_parameters({
                "FENCE_ENABLE": 1,
                "WP_RADIUS": 5,
            })
            if self.mavproxy is not None:
                self.mavproxy.send("fence list\n")
            target_loc = mavutil.location(40.071260, -105.227000, 0, 0)
            self.send_guided_mission_item(target_loc,
                                          target_system=target_system,
                                          target_component=target_component)
            # FIXME: we don't get within WP_RADIUS of our target?!
            self.wait_location(target_loc, timeout=300, accuracy=15)
            self.do_RTL(timeout=300)
            self.disarm_vehicle()
        except Exception as e:
            self.print_exception_caught(e)
            ex = e
        self.context_pop()
        self.disarm_vehicle()
        self.reboot_sitl()
        if ex is not None:
            raise ex

    def test_poly_fence_object_avoidance_auto_bendy_ruler_easier(self, target_system=1, target_component=1):
        '''finish-line issue means we can't complete the harder one.  This
        test can go away once we've nailed that one.  The only
        difference here is the target point.
        '''
        if not self.mavproxy_can_do_mision_item_protocols():
            return

        self.load_fence("rover-path-bendyruler-fence.txt")
        self.load_mission("rover-path-bendyruler-mission-easier.txt")
        self.context_push()
        ex = None
        try:
            self.set_parameters({
                "AVOID_ENABLE": 3,
                "OA_TYPE": 1,
                "OA_LOOKAHEAD": 50,
            })
            self.reboot_sitl()
            self.change_mode('AUTO')
            self.wait_ready_to_arm()
            self.arm_vehicle()
            self.set_parameters({
                "FENCE_ENABLE": 1,
                "WP_RADIUS": 5,
            })
            if self.mavproxy is not None:
                self.mavproxy.send("fence list\n")
            target_loc = mavutil.location(40.071260, -105.227000, 0, 0)
            # target_loc is copied from the mission file
            self.wait_location(target_loc, timeout=300)
            # mission has RTL as last item
            self.wait_distance_to_home(3, 7, timeout=300)
            self.disarm_vehicle()
        except Exception as e:
            self.print_exception_caught(e)
            ex = e
        self.context_pop()
        self.disarm_vehicle()
        self.reboot_sitl()
        if ex is not None:
            raise ex

    def PolyFenceObjectAvoidance(self, target_system=1, target_component=1):
        '''PolyFence object avoidance tests'''
        if not self.mavproxy_can_do_mision_item_protocols():
            return

        self.test_poly_fence_object_avoidance_auto(
            target_system=target_system,
            target_component=target_component)
        self.test_poly_fence_object_avoidance_guided(
            target_system=target_system,
            target_component=target_component)

    def PolyFenceObjectAvoidanceBendyRuler(self, target_system=1, target_component=1):
        '''PolyFence object avoidance tests - bendy ruler'''
        if not self.mavproxy_can_do_mision_item_protocols():
            return
        # bendy Ruler isn't as flexible as Dijkstra for planning, so
        # it gets a simpler test:
        self.test_poly_fence_object_avoidance_guided_bendy_ruler(
            target_system=target_system,
            target_component=target_component,
        )

    def test_scripting_simple_loop(self):
        self.start_subtest("Scripting simple loop")

        self.context_push()

        messages = []

        def my_message_hook(mav, message):
            if message.get_type() != 'STATUSTEXT':
                return
            messages.append(message)

        self.install_message_hook_context(my_message_hook)

        self.set_parameter("SCR_ENABLE", 1)
        self.install_example_script_context("simple_loop.lua")
        self.reboot_sitl()
        self.delay_sim_time(10)

        self.context_pop()
        self.reboot_sitl()

        # check all messages to see if we got our message
        count = 0
        for m in messages:
            if "hello, world" in m.text:
                count += 1
        self.progress("Got %u hellos" % count)
        if count < 3:
            raise NotAchievedException("Expected at least three hellos")

    def test_scripting_internal_test(self):
        self.start_subtest("Scripting internal test")

        self.context_push()

        test_scripts = ["scripting_test.lua", "math.lua", "strings.lua", "mavlink_test.lua"]
        success_text = ["Internal tests passed", "Math tests passed", "String tests passed", "Received heartbeat from"]
        named_value_float_types = ["test"]

        messages = []
        named_value_float = []

        def my_message_hook(mav, message):
            if message.get_type() == 'STATUSTEXT':
                messages.append(message)
            # also sniff for named value float messages
            if message.get_type() == 'NAMED_VALUE_FLOAT':
                named_value_float.append(message)

        self.install_message_hook_context(my_message_hook)
        self.set_parameters({
            "SCR_ENABLE": 1,
            "SCR_HEAP_SIZE": 1024000,
            "SCR_VM_I_COUNT": 1000000,
        })
        self.install_test_modules_context()
        self.install_mavlink_module_context()
        for script in test_scripts:
            self.install_test_script_context(script)
        self.reboot_sitl()

        self.delay_sim_time(10)

        self.context_pop()
        self.reboot_sitl()

        # check all messages to see if we got our message
        success = True
        for text in success_text:
            script_success = False
            for m in messages:
                if text in m.text:
                    script_success = True
            success = script_success and success
        if not success:
            raise NotAchievedException("Failed to receive STATUS_TEXT")
        else:
            self.progress("Success STATUS_TEXT")

        for type in named_value_float_types:
            script_success = False
            for m in named_value_float:
                if type == m.name:
                    script_success = True
            success = script_success and success
        if not success:
            raise NotAchievedException("Failed to receive NAMED_VALUE_FLOAT")
        else:
            self.progress("Success NAMED_VALUE_FLOAT")

    def test_scripting_hello_world(self):
        self.start_subtest("Scripting hello world")

        self.context_push()
        self.context_collect("STATUSTEXT")
        self.set_parameter("SCR_ENABLE", 1)
        self.install_example_script_context("hello_world.lua")
        self.reboot_sitl()

        self.wait_statustext('hello, world', check_context=True, timeout=30)

        self.context_pop()
        self.reboot_sitl()

    def ScriptingSteeringAndThrottle(self):
        '''Scripting test - steering and throttle'''
        self.start_subtest("Scripting square")

        self.context_push()
        self.install_example_script_context("rover-set-steering-and-throttle.lua")
        self.set_parameter("SCR_ENABLE", 1)
        self.reboot_sitl()

        self.wait_ready_to_arm()
        self.arm_vehicle()
        self.set_rc(6, 2000)
        tstart = self.get_sim_time()
        while not self.mode_is("HOLD"):
            if self.get_sim_time_cached() - tstart > 30:
                raise NotAchievedException("Did not move to hold")
            m = self.mav.recv_match(type='VFR_HUD', blocking=True, timeout=1)
            if m is not None:
                self.progress("Current speed: %f" % m.groundspeed)
        self.disarm_vehicle()

        self.context_pop()
        self.reboot_sitl()

    def test_scripting_auxfunc(self):
        self.start_subtest("Scripting aufunc triggering")

        self.context_push()
        self.context_collect("STATUSTEXT")
        self.set_parameters({
            "SCR_ENABLE": 1,
            "RELAY_PIN": 1,
        })
        self.install_example_script_context("RCIN_test.lua")
        self.reboot_sitl()

        self.wait_parameter_value("SIM_PIN_MASK", 121)
        self.wait_parameter_value("SIM_PIN_MASK", 123)
        self.wait_parameter_value("SIM_PIN_MASK", 121)

        self.context_pop()
        self.reboot_sitl()

    def test_scripting_print_home_and_origin(self):
        self.start_subtest("Scripting print home and origin")

        self.context_push()

        self.set_parameter("SCR_ENABLE", 1)
        self.install_example_script_context("ahrs-print-home-and-origin.lua")
        self.reboot_sitl()
        self.wait_ready_to_arm()
        self.wait_statustext("Home - ")
        self.wait_statustext("Origin - ")

        self.context_pop()
        self.reboot_sitl()

    def test_scripting_set_home_to_vehicle_location(self):
        self.start_subtest("Scripting set home to vehicle location")

        self.context_push()
        self.set_parameter("SCR_ENABLE", 1)
        self.install_example_script_context("ahrs-set-home-to-vehicle-location.lua")
        self.reboot_sitl()

        self.wait_statustext("Home position reset")

        self.context_pop()
        self.reboot_sitl()

    def Scripting(self):
        '''Scripting test'''
        self.test_scripting_set_home_to_vehicle_location()
        self.test_scripting_print_home_and_origin()
        self.test_scripting_hello_world()
        self.test_scripting_simple_loop()
        self.test_scripting_internal_test()
        self.test_scripting_auxfunc()

    def test_mission_frame(self, frame, target_system=1, target_component=1):
        self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_MISSION,
                           target_system=target_system,
                           target_component=target_component)
        items = [
            # first item is ignored for missions
            self.mav.mav.mission_item_int_encode(
                target_system,
                target_component,
                0, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
                mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
                0, # current
                0, # autocontinue
                3, # p1
                0, # p2
                0, # p3
                0, # p4
                int(1.0000 * 1e7), # latitude
                int(1.0000 * 1e7), # longitude
                31.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_MISSION),
            self.mav.mav.mission_item_int_encode(
                target_system,
                target_component,
                1, # seq
                frame,
                mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
                0, # current
                0, # autocontinue
                3, # p1
                0, # p2
                0, # p3
                0, # p4
                int(1.0000 * 1e7), # latitude
                int(1.0000 * 1e7), # longitude
                31.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_MISSION),
        ]

        self.check_mission_upload_download(items)

    def MissionFrames(self, target_system=1, target_component=1):
        '''Upload/Download of items in different frames'''
        for frame in (mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT_INT,
                      mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT,
                      mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
                      mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
                      mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                      mavutil.mavlink.MAV_FRAME_GLOBAL):
            self.test_mission_frame(frame,
                                    target_system=1,
                                    target_component=1)

    def mavlink_time_boot_ms(self):
        '''returns a time suitable for putting into the time_boot_ms entry in mavlink packets'''
        return int(time.time() * 1000000)

    def mavlink_time_boot_us(self):
        '''returns a time suitable for putting into the time_boot_ms entry in mavlink packets'''
        return int(time.time() * 1000000000)

    def ap_proximity_mav_obstacle_distance_send(self, data):
        increment = data.get("increment", 0)
        increment_f = data.get("increment_f", 0.0)
        max_distance = data["max_distance"]
        invalid_distance = max_distance + 1  # per spec
        distances = data["distances"][:]
        distances.extend([invalid_distance] * (72-len(distances)))
        self.mav.mav.obstacle_distance_send(
            self.mavlink_time_boot_us(),
            mavutil.mavlink.MAV_DISTANCE_SENSOR_LASER,
            distances,
            increment,
            data["min_distance"],
            data["max_distance"],
            increment_f,
            data["angle_offset"],
            mavutil.mavlink.MAV_FRAME_BODY_FRD
        )

    def send_obstacle_distances_expect_distance_sensor_messages(self, obstacle_distances_in, expect_distance_sensor_messages):
        self.delay_sim_time(11)  # allow obstacles to time out
        self.do_timesync_roundtrip()
        expect_distance_sensor_messages_copy = expect_distance_sensor_messages[:]
        last_sent = 0
        while True:
            now = self.get_sim_time_cached()
            if now - last_sent > 1:
                self.progress("Sending")
                self.ap_proximity_mav_obstacle_distance_send(obstacle_distances_in)
                last_sent = now
            m = self.mav.recv_match(type='DISTANCE_SENSOR', blocking=True, timeout=1)
            self.progress("Got (%s)" % str(m))
            if m is None:
                self.delay_sim_time(1)
                continue
            orientation = m.orientation
            found = False
            if m.current_distance == m.max_distance:
                # ignored
                continue
            for expected_distance_sensor_message in expect_distance_sensor_messages_copy:
                if expected_distance_sensor_message["orientation"] != orientation:
                    continue
                found = True
                if not expected_distance_sensor_message.get("__found__", False):
                    self.progress("Marking message as found")
                    expected_distance_sensor_message["__found__"] = True
                if (m.current_distance - expected_distance_sensor_message["distance"] > 1):
                    raise NotAchievedException(
                        "Bad distance for orient=%u want=%u got=%u" %
                        (orientation, expected_distance_sensor_message["distance"], m.current_distance))
                break
            if not found:
                raise NotAchievedException("Got unexpected DISTANCE_SENSOR message")
            all_found = True
            for expected_distance_sensor_message in expect_distance_sensor_messages_copy:
                if not expected_distance_sensor_message.get("__found__", False):
                    self.progress("message still not found (orient=%u" % expected_distance_sensor_message["orientation"])
                    all_found = False
                    break
            if all_found:
                self.progress("Have now seen all expected messages")
                break

    def AP_Proximity_MAV(self):
        '''Test MAV proximity backend'''
        self.context_push()
        ex = None
        try:
            self.set_parameters({
                "PRX1_TYPE": 2,  # AP_Proximity_MAV
                "OA_TYPE": 2,  # dijkstra
                "OA_DB_OUTPUT": 3,  # send all items
            })
            self.reboot_sitl()

            # 1 laser pointing straight forward:
            self.send_obstacle_distances_expect_distance_sensor_messages(
                {
                    "distances": [234],
                    "increment_f": 10,
                    "angle_offset": 0.0,
                    "min_distance": 0,
                    "max_distance": 1000, # cm
                }, [
                    {"orientation": 0, "distance": 234},
                ])

            # 5 lasers at front of vehicle, spread over 40 degrees:
            self.send_obstacle_distances_expect_distance_sensor_messages(
                {
                    "distances": [111, 222, 333, 444, 555],
                    "increment_f": 10,
                    "angle_offset": -20.0,
                    "min_distance": 0,
                    "max_distance": 1000, # cm
                }, [
                    {"orientation": 0, "distance": 111},
                ])

            # lots of dense readings (e.g. vision camera:
            distances = [0] * 72
            for i in range(0, 72):
                distances[i] = 1000 + 10*abs(36-i)

            self.send_obstacle_distances_expect_distance_sensor_messages(
                {
                    "distances": distances,
                    "increment_f": 90/72.0,
                    "angle_offset": -45.0,
                    "min_distance": 0,
                    "max_distance": 2000, # cm
                }, [
                    {"orientation": 0, "distance": 1000},
                    {"orientation": 1, "distance": 1190},
                    {"orientation": 7, "distance": 1190},
                ])

        except Exception as e:
            self.print_exception_caught(e)
            ex = e
        self.context_pop()
        self.reboot_sitl()
        if ex is not None:
            raise ex

    def SendToComponents(self):
        '''Test ArduPilot send_to_components function'''
        self.set_parameter("CAM1_TYPE", 5) # Camera with MAVlink trigger
        self.reboot_sitl() # needed for CAM1_TYPE to take effect
        self.progress("Introducing ourselves to the autopilot as a component")
        old_srcSystem = self.mav.mav.srcSystem
        self.mav.mav.srcSystem = 1
        self.mav.mav.heartbeat_send(
            mavutil.mavlink.MAV_TYPE_ONBOARD_CONTROLLER,
            mavutil.mavlink.MAV_AUTOPILOT_INVALID,
            0,
            0,
            0)
        self.progress("Sending control message")
        self.mav.mav.digicam_control_send(
            1, # target_system
            1, # target_component
            1, # start or keep it up
            1, # zoom_pos
            0, # zoom_step
            0, # focus_lock
            0, # 1 shot or start filming
            17, # command id (de-dupe field)
            0, # extra_param
            0.0, # extra_value
        )
        self.mav.mav.srcSystem = old_srcSystem

        self.progress("Expecting a command long")
        tstart = self.get_sim_time_cached()
        while True:
            now = self.get_sim_time_cached()
            if now - tstart > 2:
                raise NotAchievedException("Did not receive digicam_control message")
            m = self.mav.recv_match(type='COMMAND_LONG', blocking=True, timeout=0.1)
            self.progress("Message: %s" % str(m))
            if m is None:
                continue
            if m.command != mavutil.mavlink.MAV_CMD_DO_DIGICAM_CONTROL:
                raise NotAchievedException("Did not get correct command")
            if m.param6 != 17:
                raise NotAchievedException("Did not get correct command_id")
            break

    def SkidSteer(self):
        '''Check skid-steering'''
        model = "rover-skid"

        self.customise_SITL_commandline([],
                                        model=model,
                                        defaults_filepath=self.model_defaults_filepath(model))

        self.change_mode("MANUAL")
        self.wait_ready_to_arm()
        self.arm_vehicle()
        self.progress("get a known heading to avoid worrying about wrap")
        # this is steering-type-two-paddles
        self.set_rc(1, 1400)
        self.set_rc(3, 1500)
        self.wait_heading(90)
        self.progress("straighten up")
        self.set_rc(1, 1500)
        self.set_rc(3, 1500)
        self.progress("steer one way")
        self.set_rc(1, 1600)
        self.set_rc(3, 1400)
        self.wait_heading(120)
        self.progress("steer the other")
        self.set_rc(1, 1400)
        self.set_rc(3, 1600)
        self.wait_heading(60)
        self.zero_throttle()
        self.disarm_vehicle()

    def SlewRate(self):
        """Test Motor Slew Rate feature."""
        self.context_push()
        self.change_mode("MANUAL")
        self.wait_ready_to_arm()
        self.arm_vehicle()

        self.start_subtest("Test no slew behavior")
        throttle_channel = 3
        throttle_max = 2000
        self.set_parameter("MOT_SLEWRATE", 0)
        self.set_rc(throttle_channel, throttle_max)
        tstart = self.get_sim_time()
        self.wait_servo_channel_value(throttle_channel, throttle_max)
        tstop = self.get_sim_time()
        achieved_time = tstop - tstart
        self.progress("achieved_time: %0.1fs" % achieved_time)
        if achieved_time > 0.5:
            raise NotAchievedException("Output response should be instant, got %f" % achieved_time)
        self.zero_throttle()
        self.wait_groundspeed(0, 0.5)  # why do we not stop?!

        self.start_subtest("Test 100% slew rate")
        self.set_parameter("MOT_SLEWRATE", 100)
        self.set_rc(throttle_channel, throttle_max)
        tstart = self.get_sim_time()
        self.wait_servo_channel_value(throttle_channel, throttle_max)
        tstop = self.get_sim_time()
        achieved_time = tstop - tstart
        self.progress("achieved_time: %0.1fs" % achieved_time)
        if achieved_time < 0.9 or achieved_time > 1.1:
            raise NotAchievedException("Output response should be 1s, got %f" % achieved_time)
        self.zero_throttle()
        self.wait_groundspeed(0, 0.5)  # why do we not stop?!

        self.start_subtest("Test 50% slew rate")
        self.set_parameter("MOT_SLEWRATE", 50)
        self.set_rc(throttle_channel, throttle_max)
        tstart = self.get_sim_time()
        self.wait_servo_channel_value(throttle_channel, throttle_max, timeout=10)
        tstop = self.get_sim_time()
        achieved_time = tstop - tstart
        self.progress("achieved_time: %0.1fs" % achieved_time)
        if achieved_time < 1.8 or achieved_time > 2.2:
            raise NotAchievedException("Output response should be 2s, got %f" % achieved_time)
        self.zero_throttle()
        self.wait_groundspeed(0, 0.5)  # why do we not stop?!

        self.start_subtest("Test 25% slew rate")
        self.set_parameter("MOT_SLEWRATE", 25)
        self.set_rc(throttle_channel, throttle_max)
        tstart = self.get_sim_time()
        self.wait_servo_channel_value(throttle_channel, throttle_max, timeout=10)
        tstop = self.get_sim_time()
        achieved_time = tstop - tstart
        self.progress("achieved_time: %0.1fs" % achieved_time)
        if achieved_time < 3.6 or achieved_time > 4.4:
            raise NotAchievedException("Output response should be 4s, got %f" % achieved_time)
        self.zero_throttle()
        self.wait_groundspeed(0, 0.5)  # why do we not stop?!

        self.start_subtest("Test 10% slew rate")
        self.set_parameter("MOT_SLEWRATE", 10)
        self.set_rc(throttle_channel, throttle_max)
        tstart = self.get_sim_time()
        self.wait_servo_channel_value(throttle_channel, throttle_max, timeout=20)
        tstop = self.get_sim_time()
        achieved_time = tstop - tstart
        self.progress("achieved_time: %0.1fs" % achieved_time)
        if achieved_time < 9 or achieved_time > 11:
            raise NotAchievedException("Output response should be 10s, got %f" % achieved_time)
        self.zero_throttle()
        self.wait_groundspeed(0, 0.5)  # why do we not stop?!
        self.disarm_vehicle()
        self.context_pop()

    def SET_ATTITUDE_TARGET(self, target_sysid=None, target_compid=1):
        '''Test handling of SET_ATTITUDE_TARGET'''
        if target_sysid is None:
            target_sysid = self.sysid_thismav()
        self.change_mode('GUIDED')
        self.wait_ready_to_arm()
        self.arm_vehicle()
        tstart = self.get_sim_time()
        while True:
            now = self.get_sim_time_cached()
            if now - tstart > 10:
                raise AutoTestTimeoutException("Didn't get to speed")
            self.mav.mav.set_attitude_target_send(
                0, # time_boot_ms
                target_sysid,
                target_compid,
                mavutil.mavlink.ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE |
                mavutil.mavlink.ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE |
                mavutil.mavlink.ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE,
                mavextra.euler_to_quat([0,
                                        math.radians(0),
                                        math.radians(0)]), # att
                0, # yaw rate (rad/s)
                0, # pitch rate
                0, # yaw rate
                1) # thrust

            msg = self.mav.recv_match(type='VFR_HUD', blocking=True, timeout=1)
            if msg is None:
                raise NotAchievedException("No VFR_HUD message")
            if msg.groundspeed > 5:
                break
        self.disarm_vehicle()

    def SET_POSITION_TARGET_LOCAL_NED(self, target_sysid=None, target_compid=1):
        '''Test handling of SET_POSITION_TARGET_LOCAL_NED'''
        if target_sysid is None:
            target_sysid = self.sysid_thismav()
        self.change_mode('GUIDED')
        self.wait_ready_to_arm()
        self.arm_vehicle()
        ofs_x = 30.0
        ofs_y = 30.0

        def send_target():
            self.mav.mav.set_position_target_local_ned_send(
                0, # time_boot_ms
                target_sysid,
                target_compid,
                mavutil.mavlink.MAV_FRAME_LOCAL_NED,
                mavutil.mavlink.POSITION_TARGET_TYPEMASK_VX_IGNORE |
                mavutil.mavlink.POSITION_TARGET_TYPEMASK_VY_IGNORE |
                mavutil.mavlink.POSITION_TARGET_TYPEMASK_VZ_IGNORE |
                mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE |
                mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE |
                mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE |
                mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE |
                mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE,
                ofs_x,  # pos-x
                ofs_y,  # pos-y
                0,     # pos-z
                0,     # vel-x
                0,     # vel-y
                0,     # vel-z
                0,     # acc-x
                0,     # acc-y
                0,     # acc-z
                0,     # yaw
                0,     # yaw rate
            )

        self.wait_distance_to_local_position(
            (ofs_x, ofs_y, 0),
            distance_min=0,
            distance_max=3,
            timeout=60,
            called_function=lambda last_value, target : send_target(),
            minimum_duration=5,  # make sure we stop!
        )

        self.do_RTL()
        self.disarm_vehicle()

    def EndMissionBehavior(self, timeout=60):
        '''Test end mission behavior'''
        self.context_push()

        self.load_mission("end-mission.txt")
        self.wait_ready_to_arm()
        self.arm_vehicle()

        self.start_subtest("Test End Mission Behavior HOLD")
        self.context_collect("STATUSTEXT")
        self.change_mode("AUTO")
        self.wait_text("Mission Complete", check_context=True, wallclock_timeout=2)
        # On Hold we should just stop and don't update the navigation target anymore
        tstart = self.get_sim_time()
        while True:
            if self.get_sim_time_cached() - tstart > 15:
                raise AutoTestTimeoutException("Still getting POSITION_TARGET_GLOBAL_INT")
            m = self.mav.recv_match(type="POSITION_TARGET_GLOBAL_INT",
                                    blocking=True,
                                    timeout=10)
            if m is None:
                self.progress("No POSITION_TARGET_GLOBAL_INT received, all good !")
                break
        self.context_clear_collection("STATUSTEXT")
        self.change_mode("GUIDED")
        self.context_collect("STATUSTEXT")

        self.start_subtest("Test End Mission Behavior LOITER")
        self.set_parameter("MIS_DONE_BEHAVE", 1)
        self.change_mode("AUTO")
        self.wait_text("Mission Complete", check_context=True, wallclock_timeout=2)
        # On LOITER we should update the navigation target
        tstart = self.get_sim_time()
        while True:
            if self.get_sim_time_cached() - tstart > 15:
                raise AutoTestTimeoutException("Not getting POSITION_TARGET_GLOBAL_INT")
            m = self.mav.recv_match(type="POSITION_TARGET_GLOBAL_INT",
                                    blocking=True,
                                    timeout=5)
            if m is None:
                self.progress("No POSITION_TARGET_GLOBAL_INT received")
                continue
            else:
                if self.get_sim_time_cached() - tstart > 15:
                    self.progress("Got POSITION_TARGET_GLOBAL_INT, all good !")
                    break

        self.start_subtest("Test End Mission Behavior ACRO")
        self.set_parameter("MIS_DONE_BEHAVE", 2)
        # race conditions here to do with get_sim_time()
        # swallowing heartbeats means we have to be a little
        # circuitous when testing here:
        self.change_mode("GUIDED")
        self.send_cmd_do_set_mode('AUTO')
        self.wait_mode("ACRO")

        self.start_subtest("Test End Mission Behavior MANUAL")
        self.set_parameter("MIS_DONE_BEHAVE", 3)
        # race conditions here to do with get_sim_time()
        # swallowing heartbeats means we have to be a little
        # circuitous when testing here:
        self.change_mode("GUIDED")
        self.send_cmd_do_set_mode("AUTO")
        self.wait_mode("MANUAL")
        self.disarm_vehicle()

        self.context_pop()
        self.reboot_sitl()

    def MAVProxyParam(self):
        '''Test MAVProxy parameter handling'''
        mavproxy = self.start_mavproxy()
        mavproxy.send("param fetch\n")
        mavproxy.expect("Received [0-9]+ parameters")
        self.stop_mavproxy(mavproxy)

    def MAV_CMD_DO_SET_MISSION_CURRENT_mission(self, target_system=1, target_component=1):
        return copy.copy([
            self.mav.mav.mission_item_int_encode(
                target_system,
                target_component,
                0, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
                0, # current
                0, # autocontinue
                3, # p1
                0, # p2
                0, # p3
                0, # p4
                int(1.0000 * 1e7), # latitude
                int(1.0000 * 1e7), # longitude
                31.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_MISSION),
            self.mav.mav.mission_item_int_encode(
                target_system,
                target_component,
                1, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
                0, # current
                0, # autocontinue
                3, # p1
                0, # p2
                0, # p3
                0, # p4
                int(1.0000 * 1e7), # latitude
                int(1.0000 * 1e7), # longitude
                31.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_MISSION),
            self.mav.mav.mission_item_int_encode(
                target_system,
                target_component,
                2, # seq
                mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
                mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
                0, # current
                0, # autocontinue
                3, # p1
                0, # p2
                0, # p3
                0, # p4
                int(1.0000 * 1e7), # latitude
                int(1.0000 * 1e7), # longitude
                31.0000, # altitude
                mavutil.mavlink.MAV_MISSION_TYPE_MISSION),
        ])

    def MAV_CMD_DO_SET_MISSION_CURRENT(self, target_sysid=None, target_compid=1):
        '''Test handling of CMD_DO_SET_MISSION_CURRENT'''
        if target_sysid is None:
            target_sysid = self.sysid_thismav()
        self.check_mission_upload_download(self.MAV_CMD_DO_SET_MISSION_CURRENT_mission())

        self.set_current_waypoint(2)

        self.set_current_waypoint_using_mav_cmd_do_set_mission_current(2)

        self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_MISSION_CURRENT,
                     17,
                     0,
                     0,
                     0,
                     0,
                     0,
                     0,
                     timeout=1,
                     target_sysid=target_sysid,
                     target_compid=target_compid,
                     want_result=mavutil.mavlink.MAV_RESULT_FAILED)

    def FlashStorage(self):
        '''Test flash storage (for parameters etc)'''
        self.set_parameter("LOG_BITMASK", 1)
        self.reboot_sitl()

        self.customise_SITL_commandline([
            "--set-storage-posix-enabled", "0",
            "--set-storage-flash-enabled", "1",
        ])
        if self.get_parameter("LOG_BITMASK") == 1:
            raise NotAchievedException("not using flash storage?")
        self.set_parameter("LOG_BITMASK", 2)
        self.reboot_sitl()
        self.assert_parameter_value("LOG_BITMASK", 2)
        self.set_parameter("LOG_BITMASK", 3)
        self.reboot_sitl()
        self.assert_parameter_value("LOG_BITMASK", 3)

        self.customise_SITL_commandline([])
        # make sure we're back at our original value:
        self.assert_parameter_value("LOG_BITMASK", 1)

    def FRAMStorage(self):
        '''Test FRAM storage (for parameters etc)'''
        self.set_parameter("LOG_BITMASK", 1)
        self.reboot_sitl()

        self.customise_SITL_commandline([
            "--set-storage-posix-enabled", "0",
            "--set-storage-fram-enabled", "1",
        ])
        # TODO: ensure w'ere actually taking stuff from flash storage:
#        if self.get_parameter("LOG_BITMASK") == 1:
#            raise NotAchievedException("not using flash storage?")
        self.set_parameter("LOG_BITMASK", 2)
        self.reboot_sitl()
        self.assert_parameter_value("LOG_BITMASK", 2)
        self.set_parameter("LOG_BITMASK", 3)
        self.reboot_sitl()
        self.assert_parameter_value("LOG_BITMASK", 3)

        self.customise_SITL_commandline([])
        # make sure we're back at our original value:
        self.assert_parameter_value("LOG_BITMASK", 1)

    def RangeFinder(self):
        '''Test RangeFinder'''
        # the following magic numbers correspond to the post locations in SITL
        home_string = "%s,%s,%s,%s" % (51.8752066, 14.6487840, 54.15, 315)

        rangefinder_params = {
            "SIM_SONAR_ROT": 6,
        }
        rangefinder_params.update(self.analog_rangefinder_parameters())

        self.set_parameters(rangefinder_params)
        self.customise_SITL_commandline([
            "--home", home_string,
        ])
        self.wait_ready_to_arm()
        if self.mavproxy is not None:
            self.mavproxy.send('script /tmp/post-locations.scr\n')
        m = self.assert_receive_message('RANGEFINDER', very_verbose=True)
        if m.voltage == 0:
            raise NotAchievedException("Did not get non-zero voltage")
        want_range = 10
        if abs(m.distance - want_range) > 0.1:
            raise NotAchievedException("Expected %fm got %fm" % (want_range, m.distance))

    def DepthFinder(self):
        '''Test mulitple depthfinders for boats'''
        # Setup rangefinders
        self.customise_SITL_commandline([
            "--uartH=sim:nmea", # NMEA Rangefinder
        ])

        # RANGEFINDER_INSTANCES = [0, 2, 5]
        self.set_parameters({
            "RNGFND1_TYPE" : 17,     # NMEA must attach uart to SITL
            "RNGFND1_ORIENT" : 25,   # Set to downward facing
            "SERIAL7_PROTOCOL" : 9,  # Rangefinder on uartH
            "SERIAL7_BAUD" : 9600,   # Rangefinder specific baudrate

            "RNGFND3_TYPE" : 2,      # MaxbotixI2C
            "RNGFND3_ADDR" : 112,    # 0x70 address from SIM_I2C.cpp
            "RNGFND3_ORIENT" : 0,    # Set to forward facing, thus we should not receive DPTH messages from this one

            "RNGFND6_ADDR" : 113,    # 0x71 address from SIM_I2C.cpp
            "RNGFND6_ORIENT" : 25,   # Set to downward facing
            "RNGFND6_TYPE" : 2,      # MaxbotixI2C
        })

        self.reboot_sitl()
        self.wait_ready_to_arm()

        # should not get WATER_DEPTH messages or DPTH logs when the FRAME_CLASS is not a boat
        m = self.mav.recv_match(type="WATER_DEPTH", blocking=True, timeout=2)
        if m is not None:
            raise NotAchievedException("WATER_DEPTH: received message when FRAME_CLASS not a Boat")

        # Set FRAME_CLASS to start receiving WATER_DEPTH messages & logging DPTH
        self.set_parameters({
            "FRAME_CLASS": 2,       # Boat
        })

        # Check each rangefinder instance is in collection
        rangefinder = [None, None, None, None, None, None] # Be lazy FIXME only need [3]

        def check_rangefinder(mav, m):
            if m.get_type() != 'WATER_DEPTH':
                return

            id = m.id

            # Should not find instance 3 as it is forward facing
            if id == 2:
                raise NotAchievedException("Depthfinder Instance %i with non-downward orientation found" % (id))

            rangefinder[id] = True

            if id == 0:
                if float(m.temperature) == 0.0:
                    raise NotAchievedException("Depthfinder Instance %i NMEA with temperature not found" % (id))
            elif id == 5:
                if float(m.temperature) != 0.0:
                    raise NotAchievedException("Depthfinder Instance %i should not have temperature" % (id))

        self.wait_ready_to_arm()
        self.arm_vehicle()

        self.install_message_hook_context(check_rangefinder)
        self.drive_mission("rover1.txt", strict=False)

        if rangefinder[0] is None:
            raise NotAchievedException("Never saw Depthfinder 1")
        if rangefinder[2] is not None:
            raise NotAchievedException("Should not have found a Depthfinder 3")
        if rangefinder[5] is None:
            raise NotAchievedException("Never saw Depthfinder 6")
        if not self.current_onboard_log_contains_message("DPTH"):
            raise NotAchievedException("Expected DPTH log message")

        # self.context_pop()

    def EStopAtBoot(self):
        '''Ensure EStop prevents arming when asserted at boot time'''
        self.context_push()
        self.set_parameters({
            "RC9_OPTION": 31,
        })
        self.set_rc(9, 2000)
        self.reboot_sitl()
        self.assert_prearm_failure(
            "Motors Emergency Stopped",
            other_prearm_failures_fatal=False)
        self.context_pop()
        self.reboot_sitl()

    def assert_mode(self, mode):
        if not self.mode_is(mode):
            raise NotAchievedException("Mode is not %s" % str(mode))

    def ChangeModeByNumber(self):
        '''ensure we can set a mode by number, handy when we don't have a
        pymavlink number for it yet'''
        for (x, want) in (0, 'MANUAL'), (1, 'ACRO'), (3, 3):
            self.change_mode(x)
            self.assert_mode(want)

    def StickMixingAuto(self):
        '''Ensure Stick Mixing works in auto'''
        items = []
        self.set_parameter('STICK_MIXING', 1)
        # home
        items.append((mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0),)
        # 1 waypoint a long way away
        items.append((mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 2000, 0, 0),)
        self.upload_simple_relhome_mission(items)
        if self.mavproxy is not None:
            # handy for getting pretty pictures
            self.mavproxy.send("wp list\n")
        self.change_mode('AUTO')
        self.wait_ready_to_arm()
        self.arm_vehicle()
        self.set_rc(1, 1150)
        self.wait_heading(45)
        self.wait_heading(90)
        self.disarm_vehicle()

    def AutoDock(self):
        '''Test automatic docking of rover for multiple FOVs of simulated beacon'''
        self.context_push()

        self.set_parameters({
            "PLND_ENABLED": 1,
            "PLND_TYPE": 4,
            "PLND_ORIENT": 0,
        })

        start = self.mav.location()
        target = self.offset_location_ne(start, 50, 0)
        self.progress("Setting target to %f %f" % (start.lat, start.lng))
        stopping_dist = 0.5

        self.set_parameters({
            "SIM_PLD_ENABLE": 1,
            "SIM_PLD_LAT": target.lat,
            "SIM_PLD_LON": target.lng,
            "SIM_PLD_HEIGHT": 0,
            "SIM_PLD_ALT_LMT": 30,
            "SIM_PLD_DIST_LMT": 30,
            "SIM_PLD_ORIENT": 4,    # emit beams towards south, vehicle's heading must be north to see it
            "SIM_PLD_OPTIONS": 1,
            "DOCK_SPEED": 2,
            "DOCK_STOP_DIST": stopping_dist,
        })

        for type in range(0, 3):  # CYLINDRICAL FOV, CONICAL FOV, SPHERICAL FOV
            ex = None
            try:
                self.set_parameter("SIM_PLD_TYPE", type)
                self.reboot_sitl()
                self.change_mode('GUIDED')
                self.wait_ready_to_arm()
                self.arm_vehicle()
                initial_position = self.offset_location_ne(target, -20, -2)
                self.drive_to_location(initial_position)
                self.change_mode(8) # DOCK mode
                max_delta = 1
                self.wait_distance_to_location(target, 0, max_delta, timeout=180)
                self.disarm_vehicle()
                self.assert_receive_message('GLOBAL_POSITION_INT')
                new_pos = self.mav.location()
                delta = abs(self.get_distance(target, new_pos) - stopping_dist)
                self.progress("Docked %f metres from stopping point" % delta)
                if delta > max_delta:
                    raise NotAchievedException("Did not dock close enough to stopping point (%fm > %fm" % (delta, max_delta))

                if not self.current_onboard_log_contains_message("PL"):
                    raise NotAchievedException("Did not see expected PL message")

            except Exception as e:
                self.print_exception_caught(e)
                ex = e
                break

        self.context_pop()

        if ex is not None:
            raise ex

        self.reboot_sitl()
        self.progress("All done")

    def PrivateChannel(self):
        '''test the serial option bit specifying a mavlink channel as private'''
        global mav2
        port = self.adjust_ardupilot_port(5763)
        mav2 = mavutil.mavlink_connection("tcp:localhost:%u" % port,
                                          robust_parsing=True,
                                          source_system=7,
                                          source_component=7)
        # send a heartbeat or two to make sure ArduPilot's aware:

        def heartbeat_on_mav2(mav, m):
            '''send a heartbeat on mav2 whenever we get one on mav'''
            global mav2
            if mav == mav2:
                return
            if m.get_type() == 'HEARTBEAT':
                mav2.mav.heartbeat_send(
                    mavutil.mavlink.MAV_TYPE_ONBOARD_CONTROLLER,
                    mavutil.mavlink.MAV_AUTOPILOT_INVALID,
                    0,
                    0,
                    0)
                return

        self.assert_receive_message("HEARTBEAT", mav=mav2)

        # ensure a targetted message is received:
        self.install_message_hook_context(heartbeat_on_mav2)

        self.progress("Ensuring we can get a message normally")
        self.poll_message("AUTOPILOT_VERSION", mav=mav2)

        self.progress("Polling AUTOPILOT_VERSION from random sysid")
        self.send_poll_message("AUTOPILOT_VERSION", mav=mav2, target_sysid=134)
        self.assert_not_receive_message("AUTOPILOT_VERSION", mav=mav2, timeout=10)

        # make sure we get heartbeats on the main channel from the non-private mav2:
        tstart = self.get_sim_time()
        while True:
            if self.get_sim_time_cached() - tstart > 5:
                raise NotAchievedException("Did not get expected heartbeat from %u" % 7)
            m = self.assert_receive_message("HEARTBEAT")
            if m.get_srcSystem() == 7:
                self.progress("Got heartbeat from (%u) on non-private channel" % 7)
                break

        # make sure we receive heartbeats from the autotest suite into
        # the component:
        tstart = self.get_sim_time()
        while True:
            if self.get_sim_time_cached() - tstart > 5:
                raise NotAchievedException("Did not get expected heartbeat from %u" % self.mav.source_system)
            m = self.assert_receive_message("HEARTBEAT", mav=mav2)
            if m.get_srcSystem() == self.mav.source_system:
                self.progress("Got heartbeat from (%u) on non-private channel" % self.mav.source_system)
                break

        def printmessage(mav, m):
            global mav2
            if mav == mav2:
                return

            print("Got (%u/%u) (%s) " % (m.get_srcSystem(), m.get_srcComponent(), str(m)))

#        self.install_message_hook_context(printmessage)

        # ensure setting the private channel mask doesn't cause us to
        # execute these commands:
        self.set_parameter("SERIAL2_OPTIONS", 1024)
        self.reboot_sitl()  # mavlink-private is reboot-required
        mav2 = mavutil.mavlink_connection("tcp:localhost:5763",
                                          robust_parsing=True,
                                          source_system=7,
                                          source_component=7)
#        self.send_debug_trap()
        self.send_poll_message("AUTOPILOT_VERSION", mav=mav2, target_sysid=134)
        self.assert_not_receive_message("AUTOPILOT_VERSION", mav=mav2, timeout=10)

        # make sure messages from a private channel don't make it to
        # the main channel:
        self.drain_mav(self.mav)
        self.drain_mav(mav2)

        # make sure we do NOT get heartbeats on the main channel from
        # the private mav2:
        tstart = self.get_sim_time()
        while True:
            if self.get_sim_time_cached() - tstart > 5:
                break
            m = self.assert_receive_message("HEARTBEAT")
            if m.get_srcSystem() == 7:
                raise NotAchievedException("Got heartbeat from private channel")

        self.progress("ensure no outside heartbeats reach private channels")
        tstart = self.get_sim_time()
        while True:
            if self.get_sim_time_cached() - tstart > 5:
                break
            m = self.assert_receive_message("HEARTBEAT")
            if m.get_srcSystem() == 1 and m.get_srcComponent() == 1:
                continue
            # note the above test which shows we get heartbeats from
            # both the vehicle and this tests's special heartbeat
            raise NotAchievedException("Got heartbeat on private channel from non-vehicle")

    def tests(self):
        '''return list of all tests'''
        ret = super(AutoTestRover, self).tests()

        ret.extend([
            self.MAVProxy_SetModeUsingSwitch,
            self.HIGH_LATENCY2,
            self.MAVProxy_SetModeUsingMode,
            self.ModeSwitch,
            self.AuxModeSwitch,
            self.DriveRTL,
            self.SmartRTL,
            self.DriveSquare,
            self.DriveMaxRCIN,
            self.DriveMission,
            # self.DriveBrake,  # disabled due to frequent failures
            self.GetBanner,
            self.DO_SET_MODE,
            self.MAVProxy_DO_SET_MODE,
            self.ServoRelayEvents,
            self.RCOverrides,
            self.RCOverridesCancel,
            self.MANUAL_CONTROL,
            self.Sprayer,
            self.AC_Avoidance,
            self.CameraMission,
            self.Gripper,
            self.GripperMission,
            self.SET_MESSAGE_INTERVAL,
            self.REQUEST_MESSAGE,
            self.SYSID_ENFORCE,
            self.SET_ATTITUDE_TARGET,
            self.SET_POSITION_TARGET_LOCAL_NED,
            self.MAV_CMD_DO_SET_MISSION_CURRENT,
            self.Button,
            self.Rally,
            self.Offboard,
            self.MAVProxyParam,
            self.GCSFence,
            self.GCSMission,
            self.GCSRally,
            self.MotorTest,
            self.WheelEncoders,
            self.DataFlashOverMAVLink,
            self.DataFlash,
            self.SkidSteer,
            self.PolyFence,
            self.PolyFenceAvoidance,
            self.PolyFenceObjectAvoidance,
            self.PolyFenceObjectAvoidanceBendyRuler,
            self.SendToComponents,
            self.PolyFenceObjectAvoidanceBendyRulerEasier,
            self.SlewRate,
            self.Scripting,
            self.ScriptingSteeringAndThrottle,
            self.MissionFrames,
            self.SetpointGlobalPos,
            self.SetpointGlobalVel,
            self.AccelCal,
            self.RangeFinder,
            self.AP_Proximity_MAV,
            self.EndMissionBehavior,
            self.FlashStorage,
            self.FRAMStorage,
            self.DepthFinder,
            self.ChangeModeByNumber,
            self.EStopAtBoot,
            self.StickMixingAuto,
            self.AutoDock,
            self.PrivateChannel,
            self.GCSFailsafe,
            self.InitialMode,
            self.DriveMaxRCIN,
            self.NoArmWithoutMissionItems,
            self.CompassPrearms,
        ])
        return ret

    def disabled_tests(self):
        return {
            "SlewRate": "got timing report failure on CI",
        }

    def rc_defaults(self):
        ret = super(AutoTestRover, self).rc_defaults()
        ret[3] = 1500
        ret[8] = 1800
        return ret

    def initial_mode_switch_mode(self):
        return "MANUAL"

    def default_mode(self):
        return 'MANUAL'