2017-02-21 13:32:26 -04:00
#!/usr/bin/env python
2021-02-12 06:14:08 -04:00
'''
Dive ArduSub in SITL
AP_FLAKE8_CLEAN
'''
2017-02-24 19:22:38 -04:00
from __future__ import print_function
import os
2021-02-12 00:13:47 -04:00
import sys
2021-02-02 20:42:50 -04:00
import time
2017-02-24 19:22:38 -04:00
from pymavlink import mavutil
2018-03-14 08:08:53 -03:00
from common import AutoTest
2018-10-23 01:56:36 -03:00
from common import NotAchievedException
2021-02-12 00:13:47 -04:00
from common import AutoTestTimeoutException
if sys . version_info [ 0 ] < 3 :
ConnectionResetError = AutoTestTimeoutException
2018-03-14 08:08:53 -03:00
2018-10-10 10:06:01 -03:00
# get location of scripts
testdir = os . path . dirname ( os . path . realpath ( __file__ ) )
2019-02-15 20:46:33 -04:00
SITL_START_LOCATION = mavutil . location ( 33.810313 , - 118.393867 , 0 , 185 )
2017-02-24 19:22:38 -04:00
2018-10-10 10:06:01 -03:00
2019-03-26 13:28:49 -03:00
class Joystick ( ) :
Pitch = 1
Roll = 2
Throttle = 3
Yaw = 4
Forward = 5
Lateral = 6
2018-10-10 10:06:01 -03:00
2018-03-14 08:08:53 -03:00
class AutoTestSub ( AutoTest ) :
2018-10-10 10:06:01 -03:00
@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 " ]
2019-03-09 00:20:36 -04:00
def log_name ( self ) :
return " ArduSub "
2021-03-16 21:17:02 -03:00
def default_speedup ( self ) :
''' Sub seems to be race-free '''
return 100
2019-03-09 00:20:36 -04:00
def test_filepath ( self ) :
2021-02-12 06:14:08 -04:00
return os . path . realpath ( __file__ )
2018-03-15 08:54:34 -03:00
2020-03-10 09:20:31 -03:00
def set_current_test_name ( self , name ) :
self . current_test_name_directory = " ArduSub_Tests/ " + name + " / "
2018-12-15 06:37:17 -04:00
def default_mode ( self ) :
return ' MANUAL '
2019-02-15 20:46:33 -04:00
def sitl_start_location ( self ) :
return SITL_START_LOCATION
2019-03-09 00:20:36 -04:00
def default_frame ( self ) :
return ' vectored '
2018-03-05 11:14:34 -04:00
2018-10-10 10:07:21 -03:00
def is_sub ( self ) :
return True
2020-08-27 09:24:27 -03:00
def watch_altitude_maintained ( self , delta = 1 , timeout = 5.0 ) :
2019-11-01 18:20:39 -03:00
""" 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 :
2021-02-12 06:14:08 -04:00
raise NotAchievedException (
" Altitude not maintained: want %.2f (+/- %.2f ) got= %.2f " %
( previous_altitude , delta , m . alt ) )
2019-11-01 18:20:39 -03:00
def test_alt_hold ( self ) :
""" Test ALT_HOLD mode
"""
self . wait_ready_to_arm ( )
self . arm_vehicle ( )
2021-02-18 00:12:25 -04:00
self . change_mode ( ' ALT_HOLD ' )
2019-11-01 18:20:39 -03:00
2020-04-05 06:19:43 -03:00
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 )
2020-02-20 11:46:22 -04:00
self . wait_altitude ( altitude_min = - 6 , altitude_max = - 5 )
2019-11-01 18:20:39 -03:00
self . set_rc ( Joystick . Throttle , 1500 )
2020-01-24 21:03:02 -04:00
# let the vehicle settle (momentum / stopping point shenanigans....)
self . delay_sim_time ( 1 )
2019-11-01 18:20:39 -03:00
self . watch_altitude_maintained ( )
self . set_rc ( Joystick . Throttle , 1000 )
2020-02-20 11:46:22 -04:00
self . wait_altitude ( altitude_min = - 20 , altitude_max = - 19 )
2019-11-01 18:20:39 -03:00
self . set_rc ( Joystick . Throttle , 1500 )
2020-01-24 21:03:02 -04:00
# let the vehicle settle (momentum / stopping point shenanigans....)
self . delay_sim_time ( 1 )
2019-11-01 18:20:39 -03:00
self . watch_altitude_maintained ( )
self . set_rc ( Joystick . Throttle , 1900 )
2020-02-20 11:46:22 -04:00
self . wait_altitude ( altitude_min = - 14 , altitude_max = - 13 )
2019-11-01 18:20:39 -03:00
self . set_rc ( Joystick . Throttle , 1500 )
2020-01-24 21:03:02 -04:00
# let the vehicle settle (momentum / stopping point shenanigans....)
self . delay_sim_time ( 1 )
2019-11-01 18:20:39 -03:00
self . watch_altitude_maintained ( )
self . set_rc ( Joystick . Throttle , 1900 )
2020-02-20 11:46:22 -04:00
self . wait_altitude ( altitude_min = - 5 , altitude_max = - 4 )
2019-11-01 18:20:39 -03:00
self . set_rc ( Joystick . Throttle , 1500 )
2020-01-15 20:53:47 -04:00
# let the vehicle settle (momentum / stopping point shenanigans....)
2020-01-24 21:03:02 -04:00
self . delay_sim_time ( 1 )
2020-01-15 20:53:47 -04:00
2019-11-01 18:20:39 -03:00
self . watch_altitude_maintained ( )
self . disarm_vehicle ( )
2020-08-03 03:57:38 -03:00
def test_pos_hold ( self ) :
""" Test POSHOLD mode """
self . wait_ready_to_arm ( )
self . arm_vehicle ( )
# point North
self . reach_heading_manual ( 0 )
2021-02-18 00:12:25 -04:00
self . change_mode ( ' POSHOLD ' )
2020-08-03 03:57:38 -03:00
2021-02-12 06:14:08 -04:00
# dive a little
2020-08-03 03:57:38 -03:00
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 :
2021-02-12 06:14:08 -04:00
raise NotAchievedException ( " Position Hold was unable to keep position in calm waters within 1 meter after 10 seconds, drifted {} meters " . format ( distance_m ) ) # noqa
2020-08-03 03:57:38 -03:00
# 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 :
2021-02-12 06:14:08 -04:00
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
2020-08-03 03:57:38 -03:00
# 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 ) :
2021-02-12 06:14:08 -04:00
raise NotAchievedException ( " Position Hold was unable to move north 2 meters, moved {} at {} degrees instead " . format ( distance_m , bearing ) ) # noqa
2020-08-03 03:57:38 -03:00
self . disarm_vehicle ( )
2020-01-27 17:03:23 -04:00
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 ( )
2018-03-05 11:14:34 -04:00
def dive_manual ( self ) :
2018-12-15 06:37:17 -04:00
self . wait_ready_to_arm ( )
self . arm_vehicle ( )
2019-03-26 13:28:49 -03:00
self . set_rc ( Joystick . Throttle , 1600 )
self . set_rc ( Joystick . Forward , 1600 )
self . set_rc ( Joystick . Lateral , 1550 )
2018-03-05 11:14:34 -04:00
2018-04-27 15:21:53 -03:00
self . wait_distance ( 50 , accuracy = 7 , timeout = 200 )
2019-03-26 13:28:49 -03:00
self . set_rc ( Joystick . Yaw , 1550 )
2018-03-05 11:14:34 -04:00
2018-04-27 15:21:53 -03:00
self . wait_heading ( 0 )
2019-03-26 13:28:49 -03:00
self . set_rc ( Joystick . Yaw , 1500 )
2018-03-05 11:14:34 -04:00
2018-04-27 15:21:53 -03:00
self . wait_distance ( 50 , accuracy = 7 , timeout = 100 )
2019-03-26 13:28:49 -03:00
self . set_rc ( Joystick . Yaw , 1550 )
2018-03-05 11:14:34 -04:00
2018-04-27 15:21:53 -03:00
self . wait_heading ( 0 )
2019-03-26 13:28:49 -03:00
self . set_rc ( Joystick . Yaw , 1500 )
self . set_rc ( Joystick . Forward , 1500 )
self . set_rc ( Joystick . Lateral , 1100 )
2018-03-05 11:14:34 -04:00
2018-04-27 15:21:53 -03:00
self . wait_distance ( 75 , accuracy = 7 , timeout = 100 )
2018-03-05 11:14:34 -04:00
self . set_rc_default ( )
self . disarm_vehicle ( )
2018-03-14 08:08:53 -03:00
self . progress ( " Manual dive OK " )
2018-03-05 11:14:34 -04:00
2021-01-03 20:49:58 -04:00
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 " )
2018-03-05 11:14:34 -04:00
def dive_mission ( self , filename ) :
2018-03-14 08:08:53 -03:00
self . progress ( " Executing mission %s " % filename )
2018-11-09 08:32:02 -04:00
self . load_mission ( filename )
2018-03-05 11:14:34 -04:00
self . set_rc_default ( )
2018-04-27 15:21:53 -03:00
self . arm_vehicle ( )
2018-03-05 11:14:34 -04:00
2020-04-05 06:19:43 -03:00
self . change_mode ( ' AUTO ' )
2018-03-05 11:14:34 -04:00
2018-04-27 15:21:53 -03:00
self . wait_waypoint ( 1 , 5 , max_dist = 5 )
2018-03-05 11:14:34 -04:00
self . disarm_vehicle ( )
2018-03-14 08:08:53 -03:00
self . progress ( " Mission OK " )
2018-03-05 11:14:34 -04:00
2018-10-23 01:56:36 -03:00
def test_gripper_mission ( self ) :
try :
2021-03-26 23:22:16 -03:00
self . get_parameter ( " GRIP_ENABLE " , timeout = 5 )
except NotAchievedException :
self . progress ( " Skipping; Gripper not enabled in config? " )
return
2018-10-23 01:56:36 -03:00
2021-03-26 23:22:16 -03:00
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 )
2018-10-23 01:56:36 -03:00
2019-02-11 23:41:11 -04:00
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
2020-11-24 21:15:04 -04:00
alt = - 10
2019-02-11 23:41:11 -04:00
2020-11-24 21:28:36 -04:00
# 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
)
2019-02-11 23:41:11 -04:00
tstart = self . get_sim_time ( )
while True :
2019-03-07 18:34:09 -04:00
if self . get_sim_time_cached ( ) - tstart > 200 :
2019-02-11 23:41:11 -04:00
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 ( )
2018-12-13 00:45:43 -04:00
def reboot_sitl ( self ) :
""" Reboot SITL instance and wait it to reconnect. """
2021-02-02 20:42:50 -04:00
# 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
2019-03-13 02:53:56 -03:00
try :
2021-02-02 20:42:50 -04:00
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 )
2021-02-12 06:14:08 -04:00
except ConnectionResetError :
2019-03-13 02:53:56 -03:00
pass
2021-02-02 20:42:50 -04:00
self . progress ( " Battery: %s " % str ( batt ) )
if batt is None :
continue
if batt . battery_remaining > 50 :
break
2018-12-13 00:45:43 -04:00
self . initialise_after_reboot_sitl ( )
2021-06-23 02:58:52 -03:00
def DoubleCircle ( self ) :
self . change_mode ( ' CIRCLE ' )
self . wait_ready_to_arm ( )
self . arm_vehicle ( )
self . change_mode ( ' STABILIZE ' )
self . change_mode ( ' CIRCLE ' )
self . disarm_vehicle ( )
2021-03-16 06:13:26 -03:00
def default_parameter_list ( self ) :
ret = super ( AutoTestSub , self ) . default_parameter_list ( )
ret [ " FS_GCS_ENABLE " ] = 0 # FIXME
return ret
2020-12-20 07:29:46 -04:00
2019-01-05 07:46:45 -04:00
def disabled_tests ( self ) :
ret = super ( AutoTestSub , self ) . disabled_tests ( )
ret . update ( {
2021-02-12 06:14:08 -04:00
" ConfigErrorLoop " : " Sub does not instantiate AP_Stats. Also see https://github.com/ArduPilot/ardupilot/issues/10247 " , # noqa
2019-01-05 07:46:45 -04:00
} )
return ret
2018-12-15 06:37:17 -04:00
def tests ( self ) :
''' return list of all tests '''
ret = super ( AutoTestSub , self ) . tests ( )
2018-04-27 15:21:53 -03:00
2018-12-15 06:37:17 -04:00
ret . extend ( [
( " DiveManual " , " Dive manual " , self . dive_manual ) ,
2018-10-23 01:56:36 -03:00
2019-11-01 18:20:39 -03:00
( " AltitudeHold " , " Test altitude holde mode " , self . test_alt_hold ) ,
2020-08-03 03:57:38 -03:00
( " PositionHold " , " Test position hold mode " , self . test_pos_hold ) ,
2019-11-01 18:20:39 -03:00
2018-12-15 06:37:17 -04:00
( " DiveMission " ,
" Dive mission " ,
lambda : self . dive_mission ( " sub_mission.txt " ) ) ,
2018-04-27 15:21:53 -03:00
2018-12-15 06:37:17 -04:00
( " GripperMission " ,
" Test gripper mission items " ,
self . test_gripper_mission ) ,
2018-03-05 11:14:34 -04:00
2021-06-23 02:58:52 -03:00
( " DoubleCircle " ,
" Test entering circle twice " ,
self . DoubleCircle ) ,
2020-01-27 17:03:23 -04:00
( " MotorThrustHoverParameterIgnore " , " Test if we are ignoring MOT_THST_HOVER " , self . test_mot_thst_hover_ignore ) ,
2019-02-11 23:41:11 -04:00
( " SET_POSITION_TARGET_GLOBAL_INT " ,
" Move vehicle using SET_POSITION_TARGET_GLOBAL_INT " ,
self . dive_set_position_target ) ,
2020-05-11 20:29:39 -03:00
( " TestLogDownloadMAVProxy " ,
" Test Onboard Log Download using MAVProxy " ,
self . test_log_download_mavproxy ) ,
( " LogUpload " ,
" Upload logs " ,
self . log_upload ) ,
2018-12-15 06:37:17 -04:00
] )
2018-03-05 11:14:34 -04:00
2018-12-15 06:37:17 -04:00
return ret