ardupilot/Tools/autotest/ardusub.py

#!/usr/bin/env python

'''
Dive ArduSub in SITL

AP_FLAKE8_CLEAN
'''

from __future__ import print_function
import os
import sys
import time

from pymavlink import mavutil
from pymavlink import mavextra
import math

from common import AutoTest
from common import NotAchievedException
from common import AutoTestTimeoutException

if sys.version_info[0] < 3:
    ConnectionResetError = AutoTestTimeoutException

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

SITL_START_LOCATION = mavutil.location(33.810313, -118.393867, 0, 185)


class Joystick():
    Pitch = 1
    Roll = 2
    Throttle = 3
    Yaw = 4
    Forward = 5
    Lateral = 6


class AutoTestSub(AutoTest):
    @staticmethod
    def get_not_armable_mode_list():
        return []

    @staticmethod
    def get_not_disarmed_settable_modes_list():
        return []

    @staticmethod
    def get_no_position_not_settable_modes_list():
        return ["AUTO", "GUIDED", "CIRCLE", "POSHOLD"]

    @staticmethod
    def get_position_armable_modes_list():
        return []

    @staticmethod
    def get_normal_armable_modes_list():
        return ["ACRO", "ALT_HOLD", "MANUAL", "STABILIZE", "SURFACE"]

    def log_name(self):
        return "ArduSub"

    def default_speedup(self):
        '''Sub seems to be race-free'''
        return 100

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

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

    def default_mode(self):
        return 'MANUAL'

    def sitl_start_location(self):
        return SITL_START_LOCATION

    def default_frame(self):
        return 'vectored_6dof'

    def is_sub(self):
        return True

    def watch_altitude_maintained(self, delta=0.3, timeout=5.0):
        """Watch and wait for the actual altitude to be maintained

        Keyword Arguments:
            delta {float} -- Maximum altitude range to be allowed from actual point (default: {0.5})
            timeout {float} -- Timeout time in simulation seconds (default: {5.0})

        Raises:
            NotAchievedException: Exception when altitude fails to hold inside the time and
                altitude range
        """
        tstart = self.get_sim_time_cached()
        previous_altitude = self.mav.recv_match(type='VFR_HUD', blocking=True).alt
        self.progress('Altitude to be watched: %f' % (previous_altitude))
        while True:
            m = self.mav.recv_match(type='VFR_HUD', blocking=True)
            if self.get_sim_time_cached() - tstart > timeout:
                self.progress('Altitude hold done: %f' % (previous_altitude))
                return
            if abs(m.alt - previous_altitude) > delta:
                raise NotAchievedException(
                    "Altitude not maintained: want %.2f (+/- %.2f) got=%.2f" %
                    (previous_altitude, delta, m.alt))

    def set_attitude(self, roll, pitch, yaw):
        """
        angles in eulers
        """
        self.mav.mav.set_attitude_target_send(
            0,
            0, 0,
            1 << 6,
            mavextra.euler_to_quat([math.radians(roll), math.radians(pitch), math.radians(yaw)]),
            0, # roll rate
            0, # pitch rate
            0, 0 # yaw rate, thrust
        )

    def test_alt_hold(self):
        """Test ALT_HOLD mode
        """
        self.wait_ready_to_arm()
        self.arm_vehicle()
        self.change_mode('ALT_HOLD')

        msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=5)
        if msg is None:
            raise NotAchievedException("Did not get GLOBAL_POSITION_INT")
        pwm = 1000
        if msg.relative_alt/1000.0 < -5.5:
            # need to g`o up, not down!
            pwm = 2000
        self.set_rc(Joystick.Throttle, pwm)
        self.wait_altitude(altitude_min=-6, altitude_max=-5)
        self.set_rc(Joystick.Throttle, 1500)

        # let the vehicle settle (momentum / stopping point shenanigans....)
        self.delay_sim_time(1)

        self.watch_altitude_maintained()

        self.set_rc(Joystick.Throttle, 1000)
        self.wait_altitude(altitude_min=-20, altitude_max=-19)
        self.set_rc(Joystick.Throttle, 1500)

        # let the vehicle settle (momentum / stopping point shenanigans....)
        self.delay_sim_time(1)

        self.watch_altitude_maintained()

        self.set_rc(Joystick.Throttle, 1900)
        self.wait_altitude(altitude_min=-14, altitude_max=-13)
        self.set_rc(Joystick.Throttle, 1500)

        # let the vehicle settle (momentum / stopping point shenanigans....)
        self.delay_sim_time(1)

        self.watch_altitude_maintained()

        self.set_rc(Joystick.Throttle, 1900)
        self.wait_altitude(altitude_min=-5, altitude_max=-4)
        self.set_rc(Joystick.Throttle, 1500)

        # let the vehicle settle (momentum / stopping point shenanigans....)
        self.delay_sim_time(1)
        self.watch_altitude_maintained()

        self.progress("Testing if depth is controlled with large buoyancies")
        # Make sure the code can handle buoyancy changes
        self.set_parameter("SIM_BUOYANCY", 10)
        self.watch_altitude_maintained()
        self.set_parameter("SIM_BUOYANCY", -10)
        self.watch_altitude_maintained()

        # Make sure that the ROV will dive with a small input down even if there is a 10N buoyancy force upwards
        self.set_parameter("SIM_BUOYANCY", 10)
        self.set_rc(Joystick.Throttle, 1350)
        self.wait_altitude(altitude_min=-6, altitude_max=-5.5)

        self.set_rc(Joystick.Throttle, 1500)
        self.watch_altitude_maintained()

        self.delay_sim_time(1)

        self.progress("Testing depth hold can dive by pointing down and moving forwards")
        # point down, drive forwards
        self.set_attitude(0, -90, 0)
        self.delay_sim_time(2)
        self.set_attitude(0, -90, 0)
        self.set_rc(Joystick.Forward, 1900)
        self.wait_altitude(altitude_min=-10, altitude_max=-9)
        self.set_rc(Joystick.Forward, 1500)

        self.progress("Testing depth hold can dive by rolling clockwise and moving right")
        # point down, drive forwards
        self.set_attitude(90, 0, 0)
        self.delay_sim_time(2)
        self.set_attitude(90, 0, 0)
        self.set_rc(Joystick.Lateral, 1900)
        self.wait_altitude(altitude_min=-15, altitude_max=-14, timeout=60)
        self.set_rc(Joystick.Lateral, 1500)

        # got to depth, can we keep it?
        self.set_rc(Joystick.Forward, 1500)
        self.delay_sim_time(2)
        self.watch_altitude_maintained()

        self.progress("Testing depth hold WHILE changing attitude")
        # but can we keep it while changing attitude?
        self.set_attitude(0, 0, 0)
        self.delay_sim_time(2)
        self.set_attitude(180, 0, 0)
        self.watch_altitude_maintained(delta=1.0)
        self.delay_sim_time(5)
        self.set_attitude(0, 90, 0)
        self.watch_altitude_maintained(delta=1.0)
        self.delay_sim_time(5)
        self.disarm_vehicle()

    def test_mode_changes(self, delta=0.2):
        """Check if alternating between ALTHOLD, STABILIZE and POSHOLD affects altitude"""
        self.wait_ready_to_arm()
        self.arm_vehicle()
        self.set_parameter("SIM_BUOYANCY", 0)
        self.set_parameter("SIM_BARO_RND", 0)
        self.change_mode('STABILIZE')
        self.set_rc(Joystick.Throttle, 1350)
        self.delay_sim_time(10)
        self.set_rc(Joystick.Throttle, 1500)
        self.delay_sim_time(3)
        previous_altitude = self.mav.recv_match(type='VFR_HUD', blocking=True).alt
        self.change_mode('ALT_HOLD')
        self.delay_sim_time(2)
        self.change_mode('POSHOLD')
        self.delay_sim_time(2)
        self.change_mode('STABILIZE')
        self.delay_sim_time(2)
        self.change_mode('ALT_HOLD')
        self.delay_sim_time(2)
        self.change_mode('STABILIZE')
        self.delay_sim_time(2)
        self.change_mode('ALT_HOLD')
        self.delay_sim_time(2)
        self.change_mode('MANUAL')
        self.disarm_vehicle()
        final_altitude = self.mav.recv_match(type='VFR_HUD', blocking=True).alt
        if abs(previous_altitude - final_altitude) > delta:
            raise NotAchievedException(
                "Changing modes affected depth with no throttle input!, started at {}, ended at {}"
                .format(previous_altitude, final_altitude)
            )

    def test_pos_hold(self):
        """Test POSHOLD mode"""
        self.wait_ready_to_arm()
        self.arm_vehicle()
        # point North
        self.reach_heading_manual(0)
        self.change_mode('POSHOLD')

        # dive a little
        self.set_rc(Joystick.Throttle, 1300)
        self.delay_sim_time(3)
        self.set_rc(Joystick.Throttle, 1500)
        self.delay_sim_time(2)

        # Save starting point
        msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=5)
        if msg is None:
            raise NotAchievedException("Did not get GLOBAL_POSITION_INT")
        start_pos = self.mav.location()
        # Hold in perfect conditions
        self.progress("Testing position hold in perfect conditions")
        self.delay_sim_time(10)
        distance_m = self.get_distance(start_pos, self.mav.location())
        if distance_m > 1:
            raise NotAchievedException("Position Hold was unable to keep position in calm waters within 1 meter after 10 seconds, drifted {} meters".format(distance_m))  # noqa

        # Hold in 1 m/s current
        self.progress("Testing position hold in current")
        self.set_parameter("SIM_WIND_SPD", 1)
        self.set_parameter("SIM_WIND_T", 1)
        self.delay_sim_time(10)
        distance_m = self.get_distance(start_pos, self.mav.location())
        if distance_m > 1:
            raise NotAchievedException("Position Hold was unable to keep position in 1m/s current within 1 meter after 10 seconds, drifted {} meters".format(distance_m))  # noqa

        # Move forward slowly in 1 m/s current
        start_pos = self.mav.location()
        self.progress("Testing moving forward in position hold in 1m/s current")
        self.set_rc(Joystick.Forward, 1600)
        self.delay_sim_time(10)
        distance_m = self.get_distance(start_pos, self.mav.location())
        bearing = self.get_bearing(start_pos, self.mav.location())
        if distance_m < 2 or (bearing > 30 and bearing < 330):
            raise NotAchievedException("Position Hold was unable to move north 2 meters, moved {} at {} degrees instead".format(distance_m, bearing))  # noqa
        self.disarm_vehicle()

    def test_mot_thst_hover_ignore(self):
        """Test if we are ignoring MOT_THST_HOVER parameter
        """

        # Test default parameter value
        mot_thst_hover_value = self.get_parameter("MOT_THST_HOVER")
        if mot_thst_hover_value != 0.5:
            raise NotAchievedException("Unexpected default MOT_THST_HOVER parameter value {}".format(mot_thst_hover_value))

        # Test if parameter is being ignored
        for value in [0.25, 0.75]:
            self.set_parameter("MOT_THST_HOVER", value)
            self.test_alt_hold()
            self.test_mode_changes()

    def dive_manual(self):
        self.wait_ready_to_arm()
        self.arm_vehicle()

        self.set_rc(Joystick.Throttle, 1600)
        self.set_rc(Joystick.Forward, 1600)
        self.set_rc(Joystick.Lateral, 1550)

        self.wait_distance(50, accuracy=7, timeout=200)
        self.set_rc(Joystick.Yaw, 1550)

        self.wait_heading(0)
        self.set_rc(Joystick.Yaw, 1500)

        self.wait_distance(50, accuracy=7, timeout=100)
        self.set_rc(Joystick.Yaw, 1550)

        self.wait_heading(0)
        self.set_rc(Joystick.Yaw, 1500)
        self.set_rc(Joystick.Forward, 1500)
        self.set_rc(Joystick.Lateral, 1100)

        self.wait_distance(75, accuracy=7, timeout=100)
        self.set_rc_default()

        self.disarm_vehicle()
        self.progress("Manual dive OK")

        m = self.mav.recv_match(type='SCALED_PRESSURE3', blocking=True)
        if m is None:
            raise NotAchievedException("Did not get SCALED_PRESSURE3")
        if m.temperature != 2650:
            raise NotAchievedException("Did not get correct TSYS01 temperature")

    def dive_mission(self, filename):
        self.progress("Executing mission %s" % filename)
        self.load_mission(filename)
        self.set_rc_default()

        self.arm_vehicle()

        self.change_mode('AUTO')

        self.wait_waypoint(1, 5, max_dist=5)

        self.disarm_vehicle()

        self.progress("Mission OK")

    def test_gripper_mission(self):
        try:
            self.get_parameter("GRIP_ENABLE", timeout=5)
        except NotAchievedException:
            self.progress("Skipping; Gripper not enabled in config?")
            return

        self.load_mission("sub-gripper-mission.txt")
        self.change_mode('LOITER')
        self.wait_ready_to_arm()
        self.arm_vehicle()
        self.change_mode('AUTO')
        self.wait_statustext("Gripper Grabbed", timeout=60)
        self.wait_statustext("Gripper Released", timeout=60)

    def dive_set_position_target(self):
        self.change_mode('GUIDED')
        self.wait_ready_to_arm()
        self.arm_vehicle()

        startpos = self.mav.recv_match(type='GLOBAL_POSITION_INT',
                                       blocking=True)

        lat = 5
        lon = 5
        alt = -10

        # send a position-control command
        self.mav.mav.set_position_target_global_int_send(
            0, # timestamp
            1, # target system_id
            1, # target component id
            mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
            0b1111111111111000, # mask specifying use-only-lat-lon-alt
            lat, # lat
            lon, # lon
            alt, # alt
            0, # vx
            0, # vy
            0, # vz
            0, # afx
            0, # afy
            0, # afz
            0, # yaw
            0, # yawrate
        )

        tstart = self.get_sim_time()
        while True:
            if self.get_sim_time_cached() - tstart > 200:
                raise NotAchievedException("Did not move far enough")
            pos = self.mav.recv_match(type='GLOBAL_POSITION_INT',
                                      blocking=True)
            delta = self.get_distance_int(startpos, pos)
            self.progress("delta=%f (want >10)" % delta)
            if delta > 10:
                break
        self.change_mode('MANUAL')
        self.disarm_vehicle()

    def reboot_sitl(self):
        """Reboot SITL instance and wait it to reconnect."""
        # out battery is reset to full on reboot.  So reduce it to 10%
        # and wait for it to go above 50.
        self.run_cmd(mavutil.mavlink.MAV_CMD_BATTERY_RESET,
                     255,  # battery mask
                     10,  # percentage
                     0,
                     0,
                     0,
                     0,
                     0,
                     0)
        self.run_cmd_reboot()
        tstart = time.time()
        while True:
            if time.time() - tstart > 30:
                raise NotAchievedException("Did not detect reboot")
            # ask for the message:
            batt = None
            try:
                self.send_cmd(mavutil.mavlink.MAV_CMD_REQUEST_MESSAGE,
                              mavutil.mavlink.MAVLINK_MSG_ID_BATTERY_STATUS,
                              0,
                              0,
                              0,
                              0,
                              0,
                              0)
                batt = self.mav.recv_match(type='BATTERY_STATUS',
                                           blocking=True,
                                           timeout=1)
            except ConnectionResetError:
                pass
            self.progress("Battery: %s" % str(batt))
            if batt is None:
                continue
            if batt.battery_remaining > 50:
                break
        self.initialise_after_reboot_sitl()

    def default_parameter_list(self):
        ret = super(AutoTestSub, self).default_parameter_list()
        ret["FS_GCS_ENABLE"] = 0  # FIXME
        return ret

    def disabled_tests(self):
        ret = super(AutoTestSub, self).disabled_tests()
        ret.update({
            "ConfigErrorLoop": "Sub does not instantiate AP_Stats.  Also see https://github.com/ArduPilot/ardupilot/issues/10247",  # noqa
        })
        return ret

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

        ret.extend([
            ("DiveManual", "Dive manual", self.dive_manual),

            ("AltitudeHold", "Test altitude holde mode", self.test_alt_hold),
            ("PositionHold", "Test position hold mode", self.test_pos_hold),
            ("ModeChanges", "Test if mode changes affect altitude", self.test_mode_changes),

            ("DiveMission",
             "Dive mission",
             lambda: self.dive_mission("sub_mission.txt")),

            ("GripperMission",
             "Test gripper mission items",
             self.test_gripper_mission),

            ("MotorThrustHoverParameterIgnore", "Test if we are ignoring MOT_THST_HOVER", self.test_mot_thst_hover_ignore),

            ("SET_POSITION_TARGET_GLOBAL_INT",
             "Move vehicle using SET_POSITION_TARGET_GLOBAL_INT",
             self.dive_set_position_target),

            ("TestLogDownloadMAVProxy",
             "Test Onboard Log Download using MAVProxy",
             self.test_log_download_mavproxy),

            ("LogUpload",
             "Upload logs",
             self.log_upload),
        ])

        return ret