#!/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) 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() self.delay_sim_time(5) self.set_attitude(0, 90, 0) self.watch_altitude_maintained() self.delay_sim_time(5) self.disarm_vehicle() 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() 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), ("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