2017-02-21 13:32:26 -04:00
#!/usr/bin/env python
# Fly ArduCopter in SITL
2016-11-08 07:06:05 -04:00
from __future__ import print_function
2019-09-15 20:57:27 -03:00
import copy
2016-07-31 07:22:06 -03:00
import math
import os
import shutil
2018-06-29 07:38:10 -03:00
import time
2019-10-03 13:12:34 -03:00
import numpy
2016-07-31 07:22:06 -03:00
2018-03-14 08:08:53 -03:00
from pymavlink import mavutil
2018-10-12 18:20:13 -03:00
from pymavlink import mavextra
2016-07-31 07:22:06 -03:00
2018-07-15 20:54:00 -03:00
from pysim import util , rotmat
2018-07-31 06:50:02 -03:00
from common import AutoTest
from common import NotAchievedException , AutoTestTimeoutException , PreconditionFailedException
2018-03-14 08:08:53 -03:00
2011-10-31 04:29:52 -03:00
# get location of scripts
2016-07-31 07:22:06 -03:00
testdir = os . path . dirname ( os . path . realpath ( __file__ ) )
2019-02-15 20:46:33 -04:00
SITL_START_LOCATION = mavutil . location ( - 35.362938 , 149.165085 , 584 , 270 )
SITL_START_LOCATION_AVC = mavutil . location ( 40.072842 , - 105.230575 , 1586 , 0 )
2011-10-30 23:50:34 -03:00
2018-07-31 06:50:02 -03:00
2017-08-16 10:55:21 -03:00
# Flight mode switch positions are set-up in arducopter.param to be
# switch 1 = Circle
# switch 2 = Land
# switch 3 = RTL
# switch 4 = Auto
# switch 5 = Loiter
# switch 6 = Stabilize
2011-11-09 01:43:18 -04:00
2011-10-30 23:50:34 -03:00
2018-03-14 08:08:53 -03:00
class AutoTestCopter ( AutoTest ) :
2018-10-10 10:06:01 -03:00
@staticmethod
def get_not_armable_mode_list ( ) :
return [ " AUTO " , " AUTOTUNE " , " BRAKE " , " CIRCLE " , " FLIP " , " LAND " , " RTL " , " SMART_RTL " , " AVOID_ADSB " , " FOLLOW " ]
@staticmethod
def get_not_disarmed_settable_modes_list ( ) :
return [ " FLIP " , " AUTOTUNE " ]
@staticmethod
def get_no_position_not_settable_modes_list ( ) :
return [ ]
@staticmethod
def get_position_armable_modes_list ( ) :
return [ " DRIFT " , " GUIDED " , " LOITER " , " POSHOLD " , " THROW " ]
@staticmethod
def get_normal_armable_modes_list ( ) :
return [ " ACRO " , " ALT_HOLD " , " SPORT " , " STABILIZE " , " GUIDED_NOGPS " ]
2019-03-09 00:20:36 -04:00
def log_name ( self ) :
return " ArduCopter "
def test_filepath ( self ) :
return os . path . realpath ( __file__ )
2018-03-05 11:14:34 -04:00
2020-03-10 09:18:29 -03:00
def set_current_test_name ( self , name ) :
self . current_test_name_directory = " ArduCopter_Tests/ " + name + " / "
2019-02-15 20:46:33 -04:00
def sitl_start_location ( self ) :
return SITL_START_LOCATION
2018-03-15 08:31:19 -03:00
def mavproxy_options ( self ) :
ret = super ( AutoTestCopter , self ) . mavproxy_options ( )
if self . frame != ' heli ' :
ret . append ( ' --quadcopter ' )
return ret
def sitl_streamrate ( self ) :
return 5
2018-03-15 19:56:37 -03:00
def vehicleinfo_key ( self ) :
return ' ArduCopter '
2019-03-09 00:20:36 -04:00
def default_frame ( self ) :
return " + "
2018-12-13 00:45:43 -04:00
2019-03-09 00:20:36 -04:00
def uses_vicon ( self ) :
return True
2018-03-05 11:14:34 -04:00
2020-03-12 20:51:17 -03:00
def wait_disarmed_default_wait_time ( self ) :
return 120
2018-03-05 11:14:34 -04:00
def close ( self ) :
2018-03-15 08:54:34 -03:00
super ( AutoTestCopter , self ) . close ( )
2018-03-05 11:14:34 -04:00
2018-03-14 08:08:53 -03:00
# [2014/05/07] FC Because I'm doing a cross machine build
# (source is on host, build is on guest VM) I cannot hard link
2018-03-05 11:14:34 -04:00
# This flag tells me that I need to copy the data out
if self . copy_tlog :
shutil . copy ( self . logfile , self . buildlog )
2018-10-10 10:07:21 -03:00
def is_copter ( self ) :
return True
2019-03-26 09:17:11 -03:00
def get_stick_arming_channel ( self ) :
2018-10-10 10:07:21 -03:00
return int ( self . get_parameter ( " RCMAP_YAW " ) )
def get_disarm_delay ( self ) :
return int ( self . get_parameter ( " DISARM_DELAY " ) )
def set_autodisarm_delay ( self , delay ) :
self . set_parameter ( " DISARM_DELAY " , delay )
2018-07-30 08:19:14 -03:00
def user_takeoff ( self , alt_min = 30 ) :
''' takeoff using mavlink takeoff command '''
self . run_cmd ( mavutil . mavlink . MAV_CMD_NAV_TAKEOFF ,
0 , # param1
0 , # param2
0 , # param3
0 , # param4
0 , # param5
0 , # param6
alt_min # param7
2018-11-22 23:51:58 -04:00
)
2018-07-30 08:19:14 -03:00
self . progress ( " Ran command " )
self . wait_for_alt ( alt_min )
2018-10-02 19:28:21 -03:00
def takeoff ( self ,
alt_min = 30 ,
takeoff_throttle = 1700 ,
2018-12-17 02:42:21 -04:00
require_absolute = True ,
2019-02-02 00:19:18 -04:00
mode = " STABILIZE " ,
timeout = 30 ) :
2018-03-05 11:14:34 -04:00
""" Takeoff get to 30m altitude. """
2018-07-30 08:19:14 -03:00
self . progress ( " TAKEOFF " )
2018-12-17 02:42:21 -04:00
self . change_mode ( mode )
2018-08-23 07:24:34 -03:00
if not self . armed ( ) :
2018-10-02 19:28:21 -03:00
self . wait_ready_to_arm ( require_absolute = require_absolute )
2018-10-10 10:07:21 -03:00
self . zero_throttle ( )
2018-05-10 04:15:38 -03:00
self . arm_vehicle ( )
2018-03-05 11:14:34 -04:00
self . set_rc ( 3 , takeoff_throttle )
2019-02-02 00:19:18 -04:00
self . wait_for_alt ( alt_min = alt_min , timeout = timeout )
2018-07-30 08:19:14 -03:00
self . hover ( )
self . progress ( " TAKEOFF COMPLETE " )
2019-02-02 00:19:18 -04:00
def wait_for_alt ( self , alt_min = 30 , timeout = 30 ) :
2018-07-31 06:50:02 -03:00
""" Wait for altitude to be reached. """
2018-05-29 05:05:56 -03:00
m = self . mav . recv_match ( type = ' GLOBAL_POSITION_INT ' , blocking = True )
alt = m . relative_alt / 1000.0 # mm -> m
if alt < alt_min :
2018-07-31 06:53:36 -03:00
self . wait_altitude ( alt_min - 1 ,
2018-05-29 05:05:56 -03:00
( alt_min + 5 ) ,
2019-02-02 00:19:18 -04:00
relative = True ,
timeout = timeout )
2018-03-05 11:14:34 -04:00
2019-04-08 23:26:18 -03:00
def land_and_disarm ( self , timeout = 60 ) :
2018-03-05 11:14:34 -04:00
""" Land the quad. """
2018-03-14 08:08:53 -03:00
self . progress ( " STARTING LANDING " )
2018-12-17 02:42:21 -04:00
self . change_mode ( " LAND " )
2018-07-31 06:47:49 -03:00
self . wait_altitude ( - 5 , 1 , relative = True , timeout = timeout )
2018-04-27 15:21:53 -03:00
self . progress ( " LANDING: ok! " )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2018-03-05 11:14:34 -04:00
def hover ( self , hover_throttle = 1500 ) :
self . set_rc ( 3 , hover_throttle )
2019-09-28 02:41:56 -03:00
#Climb/descend to a given altitude
def setAlt ( self , desiredAlt = 50 ) :
pos = self . mav . location ( relative_alt = True )
if pos . alt > desiredAlt :
self . set_rc ( 3 , 1300 )
self . wait_altitude ( ( desiredAlt - 5 ) , desiredAlt , relative = True )
if pos . alt < ( desiredAlt - 5 ) :
self . set_rc ( 3 , 1800 )
self . wait_altitude ( ( desiredAlt - 5 ) , desiredAlt , relative = True )
self . hover ( )
# Takeoff, climb to given altitude, and fly east for 10 seconds
def takeoffAndMoveAway ( self , dAlt = 50 , dDist = 50 ) :
self . progress ( " Centering sticks " )
self . set_rc ( 1 , 1500 )
self . set_rc ( 2 , 1500 )
self . set_rc ( 3 , 1000 )
self . set_rc ( 4 , 1500 )
self . takeoff ( alt_min = dAlt )
self . change_mode ( " ALT_HOLD " )
self . progress ( " Yaw to east " )
self . set_rc ( 4 , 1580 )
self . wait_heading ( 90 )
self . set_rc ( 4 , 1500 )
self . progress ( " Fly eastbound away from home " )
self . set_rc ( 2 , 1800 )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 10 )
2019-09-28 02:41:56 -03:00
self . set_rc ( 2 , 1500 )
self . hover ( )
self . progress ( " Cotper staging 50 meters east of home at 50 meters altitude In mode Alt Hold " )
# Start and stop the GCS heartbeat for GCS failsafe testing
2020-06-03 04:00:38 -03:00
def set_heartbeat_rate ( self , rate ) :
self . mavproxy . send ( ' set heartbeat %u \n ' % rate )
2018-03-05 11:14:34 -04:00
2018-03-14 08:08:53 -03:00
# loiter - fly south west, then loiter within 5m position and altitude
2018-03-05 11:14:34 -04:00
def loiter ( self , holdtime = 10 , maxaltchange = 5 , maxdistchange = 5 ) :
""" Hold loiter position. """
2018-12-17 02:42:21 -04:00
self . takeoff ( 10 , mode = " LOITER " )
2018-03-05 11:14:34 -04:00
# first aim south east
2018-03-14 08:08:53 -03:00
self . progress ( " turn south east " )
2018-03-05 11:14:34 -04:00
self . set_rc ( 4 , 1580 )
2018-04-27 15:21:53 -03:00
self . wait_heading ( 170 )
2018-03-05 11:14:34 -04:00
self . set_rc ( 4 , 1500 )
# fly south east 50m
self . set_rc ( 2 , 1100 )
2018-04-27 15:21:53 -03:00
self . wait_distance ( 50 )
2018-03-05 11:14:34 -04:00
self . set_rc ( 2 , 1500 )
# wait for copter to slow moving
2018-04-27 15:21:53 -03:00
self . wait_groundspeed ( 0 , 2 )
2013-05-02 23:22:15 -03:00
2018-03-05 11:14:34 -04:00
m = self . mav . recv_match ( type = ' VFR_HUD ' , blocking = True )
start_altitude = m . alt
start = self . mav . location ( )
tstart = self . get_sim_time ( )
2018-03-14 08:08:53 -03:00
self . progress ( " Holding loiter at %u meters for %u seconds " %
( start_altitude , holdtime ) )
2019-03-07 18:34:09 -04:00
while self . get_sim_time_cached ( ) < tstart + holdtime :
2018-03-05 11:14:34 -04:00
m = self . mav . recv_match ( type = ' VFR_HUD ' , blocking = True )
pos = self . mav . location ( )
delta = self . get_distance ( start , pos )
alt_delta = math . fabs ( m . alt - start_altitude )
2018-03-14 08:08:53 -03:00
self . progress ( " Loiter Dist: %.2f m, alt: %u " % ( delta , m . alt ) )
2018-03-05 11:14:34 -04:00
if alt_delta > maxaltchange :
2018-10-17 23:55:16 -03:00
raise NotAchievedException (
" Loiter alt shifted %u meters (> limit %u ) " %
( alt_delta , maxaltchange ) )
2018-03-05 11:14:34 -04:00
if delta > maxdistchange :
2018-10-17 23:55:16 -03:00
raise NotAchievedException (
" Loiter shifted %u meters (> limit of %u ) " %
2018-11-22 23:51:58 -04:00
( delta , maxdistchange ) )
2018-04-27 15:21:53 -03:00
self . progress ( " Loiter OK for %u seconds " % holdtime )
2018-03-05 11:14:34 -04:00
2018-12-17 02:42:21 -04:00
self . progress ( " Climb to 30m " )
self . change_alt ( 30 )
self . progress ( " Descend to 20m " )
self . change_alt ( 20 )
self . do_RTL ( )
2019-03-05 21:29:32 -04:00
def watch_altitude_maintained ( self , min_alt , max_alt , timeout = 10 ) :
''' watch alt, relative alt must remain between min_alt and max_alt '''
tstart = self . get_sim_time_cached ( )
while True :
if self . get_sim_time_cached ( ) - tstart > timeout :
return
m = self . mav . recv_match ( type = ' VFR_HUD ' , blocking = True )
if m . alt < = min_alt :
raise NotAchievedException ( " Altitude not maintained: want > %f got= %f " % ( min_alt , m . alt ) )
def test_mode_ALT_HOLD ( self ) :
self . takeoff ( 10 , mode = " ALT_HOLD " )
self . watch_altitude_maintained ( 9 , 11 , timeout = 5 )
# feed in full elevator and aileron input and make sure we
# retain altitude:
self . set_rc ( 1 , 1000 )
self . set_rc ( 2 , 1000 )
self . watch_altitude_maintained ( 9 , 11 , timeout = 5 )
self . set_rc ( 1 , 1500 )
self . set_rc ( 2 , 1500 )
self . do_RTL ( )
2018-03-05 11:14:34 -04:00
def change_alt ( self , alt_min , climb_throttle = 1920 , descend_throttle = 1080 ) :
""" Change altitude. """
2019-07-08 13:15:02 -03:00
def adjust_altitude ( current_alt , target_alt , accuracy ) :
if math . fabs ( current_alt - target_alt ) < = accuracy :
self . hover ( )
elif current_alt < target_alt :
self . set_rc ( 3 , climb_throttle )
else :
self . set_rc ( 3 , descend_throttle )
self . wait_altitude ( ( alt_min - 5 ) , alt_min , relative = True , called_function = lambda current_alt , target_alt : adjust_altitude ( current_alt , target_alt , 1 ) )
2018-03-05 11:14:34 -04:00
self . hover ( )
2019-09-28 02:41:56 -03:00
def setGCSfailsafe ( self , paramValue = 0 ) :
# Slow down the sim rate if GCS Failsafe is in use
if paramValue == 0 :
self . set_parameter ( " FS_GCS_ENABLE " , paramValue )
self . set_parameter ( " SIM_SPEEDUP " , 10 )
else :
self . set_parameter ( " SIM_SPEEDUP " , 4 )
self . set_parameter ( " FS_GCS_ENABLE " , paramValue )
2018-03-05 11:14:34 -04:00
#################################################
# TESTS FLY
#################################################
2018-03-15 08:31:19 -03:00
# fly a square in alt_hold mode
2018-03-05 11:14:34 -04:00
def fly_square ( self , side = 50 , timeout = 300 ) :
2018-12-17 02:42:21 -04:00
2019-08-06 00:27:58 -03:00
self . clear_mission_using_mavproxy ( )
2018-12-17 02:42:21 -04:00
self . takeoff ( 10 )
2018-03-05 11:14:34 -04:00
""" Fly a square, flying N then E . """
tstart = self . get_sim_time ( )
2013-05-02 23:22:15 -03:00
2018-03-05 11:14:34 -04:00
# ensure all sticks in the middle
self . set_rc ( 1 , 1500 )
self . set_rc ( 2 , 1500 )
self . set_rc ( 3 , 1500 )
self . set_rc ( 4 , 1500 )
# switch to loiter mode temporarily to stop us from rising
2018-12-17 02:42:21 -04:00
self . change_mode ( ' LOITER ' )
2018-03-05 11:14:34 -04:00
# first aim north
2018-03-14 08:08:53 -03:00
self . progress ( " turn right towards north " )
2018-03-05 11:14:34 -04:00
self . set_rc ( 4 , 1580 )
2018-04-27 15:21:53 -03:00
self . wait_heading ( 10 )
2018-03-05 11:14:34 -04:00
self . set_rc ( 4 , 1500 )
# save bottom left corner of box as waypoint
2018-03-14 08:08:53 -03:00
self . progress ( " Save WP 1 & 2 " )
2018-03-05 11:14:34 -04:00
self . save_wp ( )
# switch back to stabilize mode
2018-12-17 02:42:21 -04:00
self . change_mode ( ' STABILIZE ' )
2018-03-05 11:14:34 -04:00
2018-07-17 00:36:46 -03:00
# increase throttle a bit because we're about to pitch:
self . set_rc ( 3 , 1525 )
2018-03-05 11:14:34 -04:00
# pitch forward to fly north
2018-03-14 08:08:53 -03:00
self . progress ( " Going north %u meters " % side )
2018-03-05 11:14:34 -04:00
self . set_rc ( 2 , 1300 )
2018-04-27 15:21:53 -03:00
self . wait_distance ( side )
2018-03-05 11:14:34 -04:00
self . set_rc ( 2 , 1500 )
2013-10-02 05:45:10 -03:00
2018-03-05 11:14:34 -04:00
# save top left corner of square as waypoint
2018-03-14 08:08:53 -03:00
self . progress ( " Save WP 3 " )
2018-03-05 11:14:34 -04:00
self . save_wp ( )
2016-07-31 07:22:06 -03:00
2018-03-05 11:14:34 -04:00
# roll right to fly east
2018-03-14 08:08:53 -03:00
self . progress ( " Going east %u meters " % side )
2018-03-05 11:14:34 -04:00
self . set_rc ( 1 , 1700 )
2018-04-27 15:21:53 -03:00
self . wait_distance ( side )
2018-03-05 11:14:34 -04:00
self . set_rc ( 1 , 1500 )
2013-05-19 03:32:54 -03:00
2018-03-05 11:14:34 -04:00
# save top right corner of square as waypoint
2018-03-14 08:08:53 -03:00
self . progress ( " Save WP 4 " )
2018-03-05 11:14:34 -04:00
self . save_wp ( )
2013-05-19 03:32:54 -03:00
2018-03-05 11:14:34 -04:00
# pitch back to fly south
2018-03-14 08:08:53 -03:00
self . progress ( " Going south %u meters " % side )
2018-03-05 11:14:34 -04:00
self . set_rc ( 2 , 1700 )
2018-04-27 15:21:53 -03:00
self . wait_distance ( side )
2018-03-05 11:14:34 -04:00
self . set_rc ( 2 , 1500 )
2013-05-19 03:32:54 -03:00
2018-03-05 11:14:34 -04:00
# save bottom right corner of square as waypoint
2018-03-14 08:08:53 -03:00
self . progress ( " Save WP 5 " )
2018-03-05 11:14:34 -04:00
self . save_wp ( )
2013-05-19 03:32:54 -03:00
2018-03-05 11:14:34 -04:00
# roll left to fly west
2018-03-14 08:08:53 -03:00
self . progress ( " Going west %u meters " % side )
2018-03-05 11:14:34 -04:00
self . set_rc ( 1 , 1300 )
2018-04-27 15:21:53 -03:00
self . wait_distance ( side )
2018-03-05 11:14:34 -04:00
self . set_rc ( 1 , 1500 )
# save bottom left corner of square (should be near home) as waypoint
2018-03-14 08:08:53 -03:00
self . progress ( " Save WP 6 " )
2018-03-05 11:14:34 -04:00
self . save_wp ( )
2018-07-17 00:36:46 -03:00
# reduce throttle again
self . set_rc ( 3 , 1500 )
2018-03-05 11:14:34 -04:00
# descend to 10m
2018-03-14 08:08:53 -03:00
self . progress ( " Descend to 10m in Loiter " )
2018-03-05 11:14:34 -04:00
self . mavproxy . send ( ' switch 5 \n ' ) # loiter mode
self . wait_mode ( ' LOITER ' )
self . set_rc ( 3 , 1300 )
time_left = timeout - ( self . get_sim_time ( ) - tstart )
2018-03-14 08:08:53 -03:00
self . progress ( " timeleft = %u " % time_left )
2018-03-05 11:14:34 -04:00
if time_left < 20 :
time_left = 20
2019-07-08 13:15:02 -03:00
self . wait_altitude ( - 10 , 10 , timeout = time_left , relative = True )
2018-08-20 12:47:58 -03:00
self . set_rc ( 3 , 1500 )
2018-03-05 11:14:34 -04:00
self . save_wp ( )
2018-12-17 02:42:21 -04:00
# save the stored mission to file
num_wp = self . save_mission_to_file ( os . path . join ( testdir ,
" ch7_mission.txt " ) )
if not num_wp :
self . fail_list . append ( " save_mission_to_file " )
self . progress ( " save_mission_to_file failed " )
self . progress ( " test: Fly a mission from 1 to %u " % num_wp )
self . mavproxy . send ( ' wp set 1 \n ' )
self . change_mode ( ' AUTO ' )
self . wait_waypoint ( 0 , num_wp - 1 , timeout = 500 )
self . progress ( " test: MISSION COMPLETE: passed! " )
self . change_mode ( ' LAND ' )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2018-12-17 02:42:21 -04:00
2018-05-10 04:15:38 -03:00
# enter RTL mode and wait for the vehicle to disarm
2018-12-17 02:42:21 -04:00
def do_RTL ( self , timeout = 250 ) :
2018-03-05 11:14:34 -04:00
""" Return, land. """
2018-12-17 02:42:21 -04:00
self . change_mode ( " RTL " )
2018-05-15 03:41:15 -03:00
self . set_rc ( 3 , 1500 )
2018-03-05 11:14:34 -04:00
tstart = self . get_sim_time ( )
2019-03-07 18:34:09 -04:00
while self . get_sim_time_cached ( ) < tstart + timeout :
2018-05-29 05:05:56 -03:00
m = self . mav . recv_match ( type = ' GLOBAL_POSITION_INT ' , blocking = True )
alt = m . relative_alt / 1000.0 # mm -> m
2019-03-07 18:28:03 -04:00
home_distance = self . distance_to_home ( use_cached_home = True )
2018-05-10 04:15:38 -03:00
home = " "
2018-05-29 05:05:56 -03:00
if alt < = 1 and home_distance < 10 :
2018-05-10 04:15:38 -03:00
home = " HOME "
2018-07-02 00:22:54 -03:00
self . progress ( " Alt: %.02f HomeDist: %.02f %s " %
2018-05-29 05:05:56 -03:00
( alt , home_distance , home ) )
2018-05-10 04:15:38 -03:00
# our post-condition is that we are disarmed:
if not self . armed ( ) :
2018-10-17 23:55:16 -03:00
if home == " " :
raise NotAchievedException ( " Did not get home " )
# success!
2018-04-27 15:21:53 -03:00
return
2018-10-17 23:55:16 -03:00
raise AutoTestTimeoutException ( " Did not get home and disarm " )
2017-12-08 01:41:17 -04:00
2018-07-24 23:57:08 -03:00
def fly_loiter_to_alt ( self ) :
""" loiter to alt """
self . context_push ( )
ex = None
try :
self . set_parameter ( " PLND_ENABLED " , 1 )
self . fetch_parameters ( )
self . set_parameter ( " PLND_TYPE " , 4 )
2019-07-10 10:57:46 -03:00
self . set_analog_rangefinder_parameters ( )
2018-07-24 23:57:08 -03:00
self . reboot_sitl ( )
2020-03-12 20:51:17 -03:00
num_wp = self . load_mission ( " copter_loiter_to_alt.txt " )
2018-07-24 23:57:08 -03:00
self . mavproxy . send ( ' switch 5 \n ' )
self . wait_mode ( ' LOITER ' )
self . wait_ready_to_arm ( )
self . arm_vehicle ( )
self . mavproxy . send ( ' mode auto \n ' )
self . wait_mode ( ' AUTO ' )
self . set_rc ( 3 , 1550 )
self . wait_current_waypoint ( 2 )
self . set_rc ( 3 , 1500 )
2020-03-12 20:51:17 -03:00
self . wait_waypoint ( 0 , num_wp - 1 , timeout = 500 )
self . wait_disarmed ( )
2018-07-24 23:57:08 -03:00
except Exception as e :
ex = e
self . context_pop ( )
self . reboot_sitl ( )
if ex is not None :
raise ex
2019-09-28 02:41:56 -03:00
# Tests all actions and logic behind the radio failsafe
def fly_throttle_failsafe ( self , side = 60 , timeout = 360 ) :
2020-02-23 18:47:16 -04:00
self . start_subtest ( " If you haven ' t taken off yet RC failure should be instant disarm " )
self . change_mode ( " STABILIZE " )
self . set_parameter ( " DISARM_DELAY " , 0 )
self . arm_vehicle ( )
self . set_parameter ( " SIM_RC_FAIL " , 1 )
self . disarm_wait ( timeout = 1 )
self . set_parameter ( " SIM_RC_FAIL " , 0 )
self . set_parameter ( " DISARM_DELAY " , 10 )
# Trigger an RC failure with the failsafe disabled. Verify no action taken.
2019-09-28 02:41:56 -03:00
self . start_subtest ( " Radio failsafe disabled test: FS_THR_ENABLE=0 should take no failsafe action " )
self . set_parameter ( ' FS_THR_ENABLE ' , 0 )
self . set_parameter ( ' FS_OPTIONS ' , 0 )
self . takeoffAndMoveAway ( )
self . set_parameter ( " SIM_RC_FAIL " , 1 )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 5 )
2019-09-28 02:41:56 -03:00
self . wait_mode ( " ALT_HOLD " )
self . set_parameter ( " SIM_RC_FAIL " , 0 )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 5 )
2019-09-28 02:41:56 -03:00
self . wait_mode ( " ALT_HOLD " )
self . end_subtest ( " Completed Radio failsafe disabled test " )
2018-03-05 11:14:34 -04:00
2019-09-28 02:41:56 -03:00
# Trigger an RC failure, verify radio failsafe triggers, restore radio, verify RC function by changing modes to cicle and stabilize.
self . start_subtest ( " Radio failsafe recovery test " )
self . set_parameter ( ' FS_THR_ENABLE ' , 1 )
self . set_parameter ( " SIM_RC_FAIL " , 1 )
self . wait_mode ( " RTL " )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 5 )
2019-09-28 02:41:56 -03:00
self . set_parameter ( " SIM_RC_FAIL " , 0 )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 5 )
2019-09-28 02:41:56 -03:00
self . set_rc ( 5 , 1050 )
self . wait_mode ( " CIRCLE " )
self . set_rc ( 5 , 1950 )
self . wait_mode ( " STABILIZE " )
self . end_subtest ( " Completed Radio failsafe recovery test " )
# Trigger and RC failure, verify failsafe triggers and RTL completes
self . start_subtest ( " Radio failsafe RTL with no options test: FS_THR_ENABLE=1 & FS_OPTIONS=0 " )
self . set_parameter ( " SIM_RC_FAIL " , 1 )
self . wait_mode ( " RTL " )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2019-09-28 02:41:56 -03:00
self . set_parameter ( " SIM_RC_FAIL " , 0 )
self . end_subtest ( " Completed Radio failsafe RTL with no options test " )
2018-03-05 11:14:34 -04:00
2019-09-28 02:41:56 -03:00
# Trigger and RC failure, verify failsafe triggers and land completes
self . start_subtest ( " Radio failsafe LAND with no options test: FS_THR_ENABLE=3 & FS_OPTIONS=0 " )
self . set_parameter ( ' FS_THR_ENABLE ' , 3 )
self . takeoffAndMoveAway ( )
self . set_parameter ( " SIM_RC_FAIL " , 1 )
self . wait_mode ( " LAND " )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2019-09-28 02:41:56 -03:00
self . set_parameter ( " SIM_RC_FAIL " , 0 )
self . end_subtest ( " Completed Radio failsafe LAND with no options test " )
2018-03-05 11:14:34 -04:00
2019-09-28 02:41:56 -03:00
# Trigger and RC failure, verify failsafe triggers and SmartRTL completes
self . start_subtest ( " Radio failsafe SmartRTL->RTL with no options test: FS_THR_ENABLE=4 & FS_OPTIONS=0 " )
self . set_parameter ( ' FS_THR_ENABLE ' , 4 )
self . takeoffAndMoveAway ( )
self . set_parameter ( " SIM_RC_FAIL " , 1 )
self . wait_mode ( " SMART_RTL " )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2019-09-28 02:41:56 -03:00
self . set_parameter ( " SIM_RC_FAIL " , 0 )
self . end_subtest ( " Completed Radio failsafe SmartRTL->RTL with no options test " )
2018-12-17 02:42:21 -04:00
2019-09-28 02:41:56 -03:00
# Trigger and RC failure, verify failsafe triggers and SmartRTL completes
self . start_subtest ( " Radio failsafe SmartRTL->Land with no options test: FS_THR_ENABLE=5 & FS_OPTIONS=0 " )
self . set_parameter ( ' FS_THR_ENABLE ' , 5 )
self . takeoffAndMoveAway ( )
self . set_parameter ( " SIM_RC_FAIL " , 1 )
self . wait_mode ( " SMART_RTL " )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2019-09-28 02:41:56 -03:00
self . set_parameter ( " SIM_RC_FAIL " , 0 )
self . end_subtest ( " Completed Radio failsafe SmartRTL_Land with no options test " )
# Trigger a GPS failure and RC failure, verify RTL fails into land mode and completes
self . start_subtest ( " Radio failsafe RTL fails into land mode due to bad position. " )
self . set_parameter ( ' FS_THR_ENABLE ' , 1 )
self . takeoffAndMoveAway ( )
self . set_parameter ( ' SIM_GPS_DISABLE ' , 1 )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 5 )
2019-09-28 02:41:56 -03:00
self . set_parameter ( " SIM_RC_FAIL " , 1 )
self . wait_mode ( " LAND " )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2019-09-28 02:41:56 -03:00
self . set_parameter ( " SIM_RC_FAIL " , 0 )
self . set_parameter ( ' SIM_GPS_DISABLE ' , 0 )
self . wait_ekf_happy ( )
self . end_subtest ( " Completed Radio failsafe RTL fails into land mode due to bad position. " )
# Trigger a GPS failure and RC failure, verify SmartRTL fails into land mode and completes
self . start_subtest ( " Radio failsafe SmartRTL->RTL fails into land mode due to bad position. " )
self . set_parameter ( ' FS_THR_ENABLE ' , 4 )
self . takeoffAndMoveAway ( )
self . set_parameter ( ' SIM_GPS_DISABLE ' , 1 )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 5 )
2019-09-28 02:41:56 -03:00
self . set_parameter ( " SIM_RC_FAIL " , 1 )
self . wait_mode ( " LAND " )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2019-09-28 02:41:56 -03:00
self . set_parameter ( " SIM_RC_FAIL " , 0 )
self . set_parameter ( ' SIM_GPS_DISABLE ' , 0 )
self . wait_ekf_happy ( )
self . end_subtest ( " Completed Radio failsafe SmartRTL->RTL fails into land mode due to bad position. " )
# Trigger a GPS failure and RC failure, verify SmartRTL fails into land mode and completes
self . start_subtest ( " Radio failsafe SmartRTL->LAND fails into land mode due to bad position. " )
self . set_parameter ( ' FS_THR_ENABLE ' , 5 )
self . takeoffAndMoveAway ( )
self . set_parameter ( ' SIM_GPS_DISABLE ' , 1 )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 5 )
2019-09-28 02:41:56 -03:00
self . set_parameter ( " SIM_RC_FAIL " , 1 )
self . wait_mode ( " LAND " )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2019-09-28 02:41:56 -03:00
self . set_parameter ( " SIM_RC_FAIL " , 0 )
self . set_parameter ( ' SIM_GPS_DISABLE ' , 0 )
self . wait_ekf_happy ( )
self . end_subtest ( " Completed Radio failsafe SmartRTL->LAND fails into land mode due to bad position. " )
# Trigger a GPS failure, then restore the GPS. Trigger an RC failure, verify SmartRTL fails into RTL and completes
self . start_subtest ( " Radio failsafe SmartRTL->RTL fails into RTL mode due to no path. " )
self . set_parameter ( ' FS_THR_ENABLE ' , 4 )
self . takeoffAndMoveAway ( )
self . set_parameter ( ' SIM_GPS_DISABLE ' , 1 )
self . mavproxy . expect ( " SmartRTL deactivated: bad position " )
self . set_parameter ( ' SIM_GPS_DISABLE ' , 0 )
self . wait_ekf_happy ( )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 5 )
2019-09-28 02:41:56 -03:00
self . set_parameter ( " SIM_RC_FAIL " , 1 )
self . wait_mode ( " RTL " )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2019-09-28 02:41:56 -03:00
self . set_parameter ( " SIM_RC_FAIL " , 0 )
self . end_subtest ( " Completed Radio failsafe SmartRTL->RTL fails into RTL mode due to no path. " )
# Trigger a GPS failure, then restore the GPS. Trigger an RC failure, verify SmartRTL fails into Land and completes
self . start_subtest ( " Radio failsafe SmartRTL->LAND fails into land mode due to no path. " )
self . set_parameter ( ' FS_THR_ENABLE ' , 5 )
self . takeoffAndMoveAway ( )
self . set_parameter ( ' SIM_GPS_DISABLE ' , 1 )
self . mavproxy . expect ( " SmartRTL deactivated: bad position " )
self . set_parameter ( ' SIM_GPS_DISABLE ' , 0 )
self . wait_ekf_happy ( )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 5 )
2019-09-28 02:41:56 -03:00
self . set_parameter ( " SIM_RC_FAIL " , 1 )
self . wait_mode ( " LAND " )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2019-09-28 02:41:56 -03:00
self . set_parameter ( " SIM_RC_FAIL " , 0 )
self . end_subtest ( " Completed Radio failsafe SmartRTL->LAND fails into land mode due to no path. " )
# Trigger an RC failure in guided mode with the option enabled to continue in guided. Verify no failsafe action takes place
self . start_subtest ( " Radio failsafe with option to continue in guided mode: FS_THR_ENABLE=1 & FS_OPTIONS=4 " )
self . setGCSfailsafe ( 1 )
self . set_parameter ( ' FS_THR_ENABLE ' , 1 )
self . set_parameter ( ' FS_OPTIONS ' , 4 )
self . takeoffAndMoveAway ( )
self . change_mode ( " GUIDED " )
self . set_parameter ( " SIM_RC_FAIL " , 1 )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 5 )
2019-09-28 02:41:56 -03:00
self . wait_mode ( " GUIDED " )
self . set_parameter ( " SIM_RC_FAIL " , 0 )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 5 )
2019-09-28 02:41:56 -03:00
self . change_mode ( " ALT_HOLD " )
self . setGCSfailsafe ( 0 )
# self.change_mode("RTL")
2020-03-12 20:51:17 -03:00
# self.wait_disarmed()
2019-09-28 02:41:56 -03:00
self . end_subtest ( " Completed Radio failsafe with option to continue in guided mode " )
# Trigger an RC failure in AUTO mode with the option enabled to continue the mission. Verify no failsafe action takes place
self . start_subtest ( " Radio failsafe RTL with option to continue mission: FS_THR_ENABLE=1 & FS_OPTIONS=1 " )
self . set_parameter ( ' FS_OPTIONS ' , 1 )
self . progress ( " # Load copter_mission " )
num_wp = self . load_mission ( " copter_mission.txt " )
if not num_wp :
raise NotAchievedException ( " load copter_mission failed " )
self . takeoffAndMoveAway ( )
self . change_mode ( " AUTO " )
self . set_parameter ( " SIM_RC_FAIL " , 1 )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 5 )
2019-09-28 02:41:56 -03:00
self . wait_mode ( " AUTO " )
self . set_parameter ( " SIM_RC_FAIL " , 0 )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 5 )
2019-09-28 02:41:56 -03:00
self . wait_mode ( " AUTO " )
# self.change_mode("RTL")
2020-03-12 20:51:17 -03:00
# self.wait_disarmed()
2019-09-28 02:41:56 -03:00
self . end_subtest ( " Completed Radio failsafe RTL with option to continue mission " )
# Trigger an RC failure in AUTO mode without the option enabled to continue. Verify failsafe triggers and RTL completes
self . start_subtest ( " Radio failsafe RTL in mission without option to continue should RTL: FS_THR_ENABLE=1 & FS_OPTIONS=0 " )
self . set_parameter ( ' FS_OPTIONS ' , 0 )
self . set_parameter ( " SIM_RC_FAIL " , 1 )
self . wait_mode ( " RTL " )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2019-09-28 02:41:56 -03:00
self . clear_mission_using_mavproxy ( )
self . set_parameter ( " SIM_RC_FAIL " , 0 )
self . end_subtest ( " Completed Radio failsafe RTL in mission without option to continue " )
2018-03-05 11:14:34 -04:00
2019-09-28 02:41:56 -03:00
self . progress ( " All radio failsafe tests complete " )
self . set_parameter ( ' FS_THR_ENABLE ' , 0 )
self . reboot_sitl ( )
# Tests all actions and logic behind the GCS failsafe
def fly_gcs_failsafe ( self , side = 60 , timeout = 360 ) :
# Trigger telemety 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 )
self . takeoffAndMoveAway ( )
2020-06-03 04:00:38 -03:00
self . set_heartbeat_rate ( 0 )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 5 )
2019-09-28 02:41:56 -03:00
self . wait_mode ( " ALT_HOLD " )
2020-06-03 04:00:38 -03:00
self . set_heartbeat_rate ( self . speedup )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 5 )
2019-09-28 02:41:56 -03:00
self . wait_mode ( " ALT_HOLD " )
self . end_subtest ( " Completed GCS failsafe disabled test " )
# Trigger telemetry loss with failsafe enabled. Verify failsafe tirggers to RTL. Restory telemety, verify failsafe clears, and change modes.
self . start_subtest ( " GCS failsafe recovery test " )
self . setGCSfailsafe ( 1 )
2020-06-03 04:00:38 -03:00
self . set_heartbeat_rate ( 0 )
2019-09-28 02:41:56 -03:00
self . wait_mode ( " RTL " )
2020-06-03 04:00:38 -03:00
self . set_heartbeat_rate ( self . speedup )
2019-09-28 02:41:56 -03:00
self . mavproxy . expect ( " GCS Failsafe Cleared " )
self . change_mode ( " LOITER " )
self . end_subtest ( " Completed GCS failsafe recovery test " )
2018-03-05 11:14:34 -04:00
2019-09-28 02:41:56 -03:00
# Trigger telemetry loss with failsafe enabled. Verify failsafe tirggers and RTL completes
self . start_subtest ( " GCS failsafe RTL with no options test: FS_GCS_ENABLE=1 & FS_OPTIONS=0 " )
2020-06-03 04:00:38 -03:00
self . set_heartbeat_rate ( 0 )
2019-09-28 02:41:56 -03:00
self . wait_mode ( " RTL " )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2020-06-03 04:00:38 -03:00
self . set_heartbeat_rate ( self . speedup )
2019-09-28 02:41:56 -03:00
self . mavproxy . expect ( " GCS Failsafe Cleared " )
self . end_subtest ( " Completed GCS failsafe RTL with no options test " )
# Trigger telemetry loss with failsafe enabled. Verify failsafe tirggers and land completes
self . start_subtest ( " GCS failsafe LAND with no options test: FS_GCS_ENABLE=5 & FS_OPTIONS=0 " )
self . setGCSfailsafe ( 5 )
self . takeoffAndMoveAway ( )
2020-06-03 04:00:38 -03:00
self . set_heartbeat_rate ( 0 )
2019-09-28 02:41:56 -03:00
self . wait_mode ( " LAND " )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2020-06-03 04:00:38 -03:00
self . set_heartbeat_rate ( self . speedup )
2019-09-28 02:41:56 -03:00
self . mavproxy . expect ( " GCS Failsafe Cleared " )
self . end_subtest ( " Completed GCS failsafe land with no options test " )
# Trigger telemetry loss with failsafe enabled. Verify failsafe tirggers and SmartRTL completes
self . start_subtest ( " GCS failsafe SmartRTL->RTL with no options test: FS_GCS_ENABLE=3 & FS_OPTIONS=0 " )
self . setGCSfailsafe ( 3 )
self . takeoffAndMoveAway ( )
2020-06-03 04:00:38 -03:00
self . set_heartbeat_rate ( 0 )
2019-09-28 02:41:56 -03:00
self . wait_mode ( " SMART_RTL " )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2020-06-03 04:00:38 -03:00
self . set_heartbeat_rate ( self . speedup )
2019-09-28 02:41:56 -03:00
self . mavproxy . expect ( " GCS Failsafe Cleared " )
self . end_subtest ( " Completed GCS failsafe SmartRTL->RTL with no options test " )
# Trigger telemetry loss with failsafe enabled. Verify failsafe tirggers and SmartRTL completes
self . start_subtest ( " GCS failsafe SmartRTL->Land with no options test: FS_GCS_ENABLE=4 & FS_OPTIONS=0 " )
self . setGCSfailsafe ( 4 )
self . takeoffAndMoveAway ( )
2020-06-03 04:00:38 -03:00
self . set_heartbeat_rate ( 0 )
2019-09-28 02:41:56 -03:00
self . wait_mode ( " SMART_RTL " )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2020-06-03 04:00:38 -03:00
self . set_heartbeat_rate ( self . speedup )
2019-09-28 02:41:56 -03:00
self . mavproxy . expect ( " GCS Failsafe Cleared " )
self . end_subtest ( " Completed GCS failsafe SmartRTL->Land with no options test " )
# Trigger telemetry loss with an invalid failsafe value. Verify failsafe tirggers and RTL completes
self . start_subtest ( " GCS failsafe invalid value with no options test: FS_GCS_ENABLE=99 & FS_OPTIONS=0 " )
self . setGCSfailsafe ( 99 )
self . takeoffAndMoveAway ( )
2020-06-03 04:00:38 -03:00
self . set_heartbeat_rate ( 0 )
2019-09-28 02:41:56 -03:00
self . wait_mode ( " RTL " )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2020-06-03 04:00:38 -03:00
self . set_heartbeat_rate ( self . speedup )
2019-09-28 02:41:56 -03:00
self . mavproxy . expect ( " GCS Failsafe Cleared " )
self . end_subtest ( " Completed GCS failsafe invalid value with no options test " )
2018-03-05 11:14:34 -04:00
2019-09-28 02:41:56 -03:00
# Trigger telemetry loss with failsafe enabled to test FS_OPTIONS settings
self . start_subtest ( " GCS failsafe with option bit tests: FS_GCS_ENABLE=1 & FS_OPTIONS=64/2/16 " )
num_wp = self . load_mission ( " copter_mission.txt " )
if not num_wp :
raise NotAchievedException ( " load copter_mission failed " )
self . setGCSfailsafe ( 1 )
self . set_parameter ( ' FS_OPTIONS ' , 16 )
self . takeoffAndMoveAway ( )
self . progress ( " Testing continue in pilot controlled modes " )
2020-06-03 04:00:38 -03:00
self . set_heartbeat_rate ( 0 )
2019-09-28 02:41:56 -03:00
self . mavproxy . expect ( " GCS Failsafe - Continuing Pilot Control " )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 5 )
2019-09-28 02:41:56 -03:00
self . wait_mode ( " ALT_HOLD " )
2020-06-03 04:00:38 -03:00
self . set_heartbeat_rate ( self . speedup )
2019-09-28 02:41:56 -03:00
self . mavproxy . expect ( " GCS Failsafe Cleared " )
self . progress ( " Testing continue in auto mission " )
self . set_parameter ( ' FS_OPTIONS ' , 2 )
self . change_mode ( " AUTO " )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 5 )
2020-06-03 04:00:38 -03:00
self . set_heartbeat_rate ( 0 )
2019-09-28 02:41:56 -03:00
self . mavproxy . expect ( " GCS Failsafe - Continuing Auto Mode " )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 5 )
2019-09-28 02:41:56 -03:00
self . wait_mode ( " AUTO " )
2020-06-03 04:00:38 -03:00
self . set_heartbeat_rate ( self . speedup )
2019-09-28 02:41:56 -03:00
self . mavproxy . expect ( " GCS Failsafe Cleared " )
self . progress ( " Testing continue landing in land mode " )
self . set_parameter ( ' FS_OPTIONS ' , 8 )
self . change_mode ( " LAND " )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 5 )
2020-06-03 04:00:38 -03:00
self . set_heartbeat_rate ( 0 )
2019-09-28 02:41:56 -03:00
self . mavproxy . expect ( " GCS Failsafe - Continuing Landing " )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 5 )
2019-09-28 02:41:56 -03:00
self . wait_mode ( " LAND " )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2020-06-03 04:00:38 -03:00
self . set_heartbeat_rate ( self . speedup )
2019-09-28 02:41:56 -03:00
self . mavproxy . expect ( " GCS Failsafe Cleared " )
self . end_subtest ( " Completed GCS failsafe with option bits " )
2018-12-17 02:42:21 -04:00
2019-09-28 02:41:56 -03:00
self . setGCSfailsafe ( 0 )
self . set_parameter ( ' FS_OPTIONS ' , 0 )
self . progress ( " All GCS failsafe tests complete " )
2018-12-17 02:42:21 -04:00
self . reboot_sitl ( )
2019-09-28 02:41:56 -03:00
# Tests all actions and logic behind the battery failsafe
2018-07-31 06:47:49 -03:00
def fly_battery_failsafe ( self , timeout = 300 ) :
2019-09-28 02:41:56 -03:00
self . progress ( " Configure battery failsafe parameters " )
self . set_parameter ( ' SIM_SPEEDUP ' , 4 )
self . set_parameter ( ' BATT_LOW_VOLT ' , 11.5 )
self . set_parameter ( ' BATT_CRT_VOLT ' , 10.1 )
self . set_parameter ( ' BATT_FS_LOW_ACT ' , 0 )
self . set_parameter ( ' BATT_FS_CRT_ACT ' , 0 )
self . set_parameter ( ' FS_OPTIONS ' , 0 )
self . set_parameter ( ' SIM_BATT_VOLTAGE ' , 12.5 )
# Trigger low battery condition with failsafe disabled. Verify no action taken.
self . start_subtest ( " Batt failsafe disabled test " )
self . takeoffAndMoveAway ( )
self . set_parameter ( ' SIM_BATT_VOLTAGE ' , 11.4 )
self . mavproxy . expect ( " Battery 1 is low " )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 5 )
2019-09-28 02:41:56 -03:00
self . wait_mode ( " ALT_HOLD " )
self . set_parameter ( ' SIM_BATT_VOLTAGE ' , 10.0 )
self . mavproxy . expect ( " Battery 1 is critical " )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 5 )
2019-09-28 02:41:56 -03:00
self . wait_mode ( " ALT_HOLD " )
self . change_mode ( " RTL " )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2019-09-28 02:41:56 -03:00
self . set_parameter ( ' SIM_BATT_VOLTAGE ' , 12.5 )
self . mavproxy . send ( ' reboot \n ' )
self . end_subtest ( " Completed Batt failsafe disabled test " )
# TWO STAGE BATTERY FAILSAFE: Trigger low battery condition, then critical battery condition. Verify RTL and Land actions complete.
self . start_subtest ( " Two stage battery failsafe test with RTL and Land " )
self . takeoffAndMoveAway ( )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 3 )
2019-09-28 02:41:56 -03:00
self . set_parameter ( ' BATT_FS_LOW_ACT ' , 2 )
self . set_parameter ( ' BATT_FS_CRT_ACT ' , 1 )
self . set_parameter ( ' SIM_BATT_VOLTAGE ' , 11.4 )
self . mavproxy . expect ( " Battery 1 is low " )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 5 )
2019-09-28 02:41:56 -03:00
self . wait_mode ( " RTL " )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 10 )
2019-09-28 02:41:56 -03:00
self . set_parameter ( ' SIM_BATT_VOLTAGE ' , 10.0 )
self . mavproxy . expect ( " Battery 1 is critical " )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 5 )
2019-09-28 02:41:56 -03:00
self . wait_mode ( " LAND " )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2019-09-28 02:41:56 -03:00
self . set_parameter ( ' SIM_BATT_VOLTAGE ' , 12.5 )
self . mavproxy . send ( ' reboot \n ' )
self . end_subtest ( " Completed two stage battery failsafe test with RTL and Land " )
# TWO STAGE BATTERY FAILSAFE: Trigger low battery condition, then critical battery condition. Verify both SmartRTL actions complete
self . start_subtest ( " Two stage battery failsafe test with SmartRTL " )
self . takeoffAndMoveAway ( )
self . set_parameter ( ' BATT_FS_LOW_ACT ' , 3 )
self . set_parameter ( ' BATT_FS_CRT_ACT ' , 4 )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 10 )
2019-09-28 02:41:56 -03:00
self . set_parameter ( ' SIM_BATT_VOLTAGE ' , 11.4 )
self . mavproxy . expect ( " Battery 1 is low " )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 5 )
2019-09-28 02:41:56 -03:00
self . wait_mode ( " SMART_RTL " )
self . change_mode ( " LOITER " )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 10 )
2019-09-28 02:41:56 -03:00
self . set_parameter ( ' SIM_BATT_VOLTAGE ' , 10.0 )
self . mavproxy . expect ( " Battery 1 is critical " )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 5 )
2019-09-28 02:41:56 -03:00
self . wait_mode ( " SMART_RTL " )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2019-09-28 02:41:56 -03:00
self . set_parameter ( ' SIM_BATT_VOLTAGE ' , 12.5 )
self . mavproxy . send ( ' reboot \n ' )
self . end_subtest ( " Completed two stage battery failsafe test with SmartRTL " )
# Trigger low battery condition in land mode with FS_OPTIONS set to allow land mode to continue. Verify landing completes uninterupted.
self . start_subtest ( " Battery failsafe with FS_OPTIONS set to continue landing " )
self . takeoffAndMoveAway ( )
self . set_parameter ( ' FS_OPTIONS ' , 8 )
self . change_mode ( " LAND " )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 5 )
2019-09-28 02:41:56 -03:00
self . set_parameter ( ' SIM_BATT_VOLTAGE ' , 11.4 )
self . mavproxy . expect ( " Battery 1 is low " )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 5 )
2019-09-28 02:41:56 -03:00
self . wait_mode ( " LAND " )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2019-09-28 02:41:56 -03:00
self . set_parameter ( ' SIM_BATT_VOLTAGE ' , 12.5 )
self . mavproxy . send ( ' reboot \n ' )
self . end_subtest ( " Completed battery failsafe with FS_OPTIONS set to continue landing " )
# Trigger a critical battery condition, which triggers a land mode failsafe. Trigger an RC failure. Verify the RC failsafe is prevented from stopping the low battery landing.
self . start_subtest ( " Battery failsafe critical landing " )
self . takeoffAndMoveAway ( 100 , 50 )
self . set_parameter ( ' FS_OPTIONS ' , 0 )
2018-03-20 01:22:06 -03:00
self . set_parameter ( ' BATT_FS_LOW_ACT ' , 1 )
2019-09-28 02:41:56 -03:00
self . set_parameter ( ' BATT_FS_CRT_ACT ' , 1 )
self . set_parameter ( ' FS_THR_ENABLE ' , 1 )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 5 )
2019-09-28 02:41:56 -03:00
self . set_parameter ( ' SIM_BATT_VOLTAGE ' , 10.0 )
self . mavproxy . expect ( " Battery 1 is critical " )
self . wait_mode ( " LAND " )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 10 )
2019-09-28 02:41:56 -03:00
self . set_parameter ( " SIM_RC_FAIL " , 1 )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 10 )
2019-09-28 02:41:56 -03:00
self . wait_mode ( " LAND " )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2019-09-28 02:41:56 -03:00
self . set_parameter ( ' SIM_BATT_VOLTAGE ' , 12.5 )
self . set_parameter ( " SIM_RC_FAIL " , 0 )
self . mavproxy . send ( ' reboot \n ' )
self . end_subtest ( " Completed battery failsafe critical landing " )
# Trigger low battery condition with failsafe set to terminate. Copter will disarm and crash.
self . start_subtest ( " Battery failsafe terminate " )
self . takeoffAndMoveAway ( )
self . set_parameter ( ' BATT_FS_LOW_ACT ' , 5 )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 10 )
2019-09-28 02:41:56 -03:00
self . set_parameter ( ' SIM_BATT_VOLTAGE ' , 11.4 )
self . mavproxy . expect ( " Battery 1 is low " )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2019-09-28 02:41:56 -03:00
self . end_subtest ( " Completed terminate failsafe test " )
self . progress ( " All Battery failsafe tests complete " )
self . set_parameter ( ' BATT_LOW_VOLT ' , 0 )
self . set_parameter ( ' BATT_CRT_VOLT ' , 0 )
self . set_parameter ( ' BATT_FS_LOW_ACT ' , 0 )
self . set_parameter ( ' BATT_FS_CRT_ACT ' , 0 )
self . set_parameter ( ' FS_OPTIONS ' , 0 )
2018-08-20 12:47:58 -03:00
self . reboot_sitl ( )
2016-07-31 07:22:06 -03:00
2018-03-14 08:08:53 -03:00
# fly_stability_patch - fly south, then hold loiter within 5m
# position and altitude and reduce 1 motor to 60% efficiency
def fly_stability_patch ( self ,
holdtime = 30 ,
maxaltchange = 5 ,
maxdistchange = 10 ) :
2018-12-17 02:42:21 -04:00
self . takeoff ( 10 , mode = " LOITER " )
2014-04-07 10:25:38 -03:00
2018-03-05 11:14:34 -04:00
# first south
2018-03-14 08:08:53 -03:00
self . progress ( " turn south " )
2018-03-05 11:14:34 -04:00
self . set_rc ( 4 , 1580 )
2018-04-27 15:21:53 -03:00
self . wait_heading ( 180 )
2018-03-05 11:14:34 -04:00
self . set_rc ( 4 , 1500 )
2014-04-07 10:25:38 -03:00
2018-03-05 11:14:34 -04:00
# fly west 80m
self . set_rc ( 2 , 1100 )
2018-04-27 15:21:53 -03:00
self . wait_distance ( 80 )
2018-03-05 11:14:34 -04:00
self . set_rc ( 2 , 1500 )
2014-04-07 10:25:38 -03:00
2018-03-05 11:14:34 -04:00
# wait for copter to slow moving
2018-04-27 15:21:53 -03:00
self . wait_groundspeed ( 0 , 2 )
2014-04-07 10:25:38 -03:00
2018-03-05 11:14:34 -04:00
m = self . mav . recv_match ( type = ' VFR_HUD ' , blocking = True )
start_altitude = m . alt
start = self . mav . location ( )
tstart = self . get_sim_time ( )
2018-03-14 08:08:53 -03:00
self . progress ( " Holding loiter at %u meters for %u seconds " %
( start_altitude , holdtime ) )
2018-03-05 11:14:34 -04:00
# cut motor 1 to 55% efficiency
2018-03-14 08:08:53 -03:00
self . progress ( " Cutting motor 1 to 60 % e fficiency " )
2018-08-06 09:30:42 -03:00
self . set_parameter ( " SIM_ENGINE_MUL " , 0.60 )
2018-03-05 11:14:34 -04:00
2019-03-07 18:34:09 -04:00
while self . get_sim_time_cached ( ) < tstart + holdtime :
2018-03-05 11:14:34 -04:00
m = self . mav . recv_match ( type = ' VFR_HUD ' , blocking = True )
pos = self . mav . location ( )
delta = self . get_distance ( start , pos )
alt_delta = math . fabs ( m . alt - start_altitude )
2018-03-14 08:08:53 -03:00
self . progress ( " Loiter Dist: %.2f m, alt: %u " % ( delta , m . alt ) )
2018-03-05 11:14:34 -04:00
if alt_delta > maxaltchange :
2018-10-17 23:55:16 -03:00
raise NotAchievedException (
" Loiter alt shifted %u meters (> limit %u ) " %
( alt_delta , maxaltchange ) )
2018-03-05 11:14:34 -04:00
if delta > maxdistchange :
2018-10-17 23:55:16 -03:00
raise NotAchievedException (
( " Loiter shifted %u meters (> limit of %u ) " %
( delta , maxdistchange ) ) )
2016-07-31 07:22:06 -03:00
2018-03-05 11:14:34 -04:00
# restore motor 1 to 100% efficiency
2018-08-06 09:30:42 -03:00
self . set_parameter ( " SIM_ENGINE_MUL " , 1.0 )
2016-07-31 07:22:06 -03:00
2018-05-09 00:32:23 -03:00
self . progress ( " Stability patch and Loiter OK for %u s " % holdtime )
2018-03-05 11:14:34 -04:00
2018-12-17 02:42:21 -04:00
self . progress ( " RTL after stab patch " )
self . do_RTL ( )
2019-03-05 23:04:46 -04:00
def debug_arming_issue ( self ) :
while True :
self . send_mavlink_arm_command ( )
m = self . mav . recv_match ( blocking = True , timeout = 1 )
if m is None :
continue
if m . get_type ( ) in [ " STATUSTEXT " , " COMMAND_ACK " ] :
print ( " Got: %s " % str ( m ) )
if self . mav . motors_armed ( ) :
self . progress ( " Armed " )
return
2019-06-06 21:57:13 -03:00
# fly_fence_test - fly east until you hit the horizontal circular fence
avoid_behave_slide = 0
def fly_fence_avoid_test_radius_check ( self , timeout = 180 , avoid_behave = avoid_behave_slide ) :
using_mode = " LOITER " # must be something which adjusts velocity!
self . change_mode ( using_mode )
self . set_parameter ( " FENCE_ENABLE " , 1 ) # fence
self . set_parameter ( " FENCE_TYPE " , 2 ) # circle
fence_radius = 15
self . set_parameter ( " FENCE_RADIUS " , fence_radius )
fence_margin = 3
self . set_parameter ( " FENCE_MARGIN " , fence_margin )
self . set_parameter ( " AVOID_ENABLE " , 1 )
self . set_parameter ( " AVOID_BEHAVE " , avoid_behave )
self . set_parameter ( " RC10_OPTION " , 40 ) # avoid-enable
self . wait_ready_to_arm ( )
self . set_rc ( 10 , 2000 )
home_distance = self . distance_to_home ( use_cached_home = True )
if home_distance > 5 :
raise PreconditionFailedException ( " Expected to be within 5m of home " )
self . zero_throttle ( )
self . arm_vehicle ( )
self . set_rc ( 3 , 1700 )
self . wait_altitude ( 10 , 100 , relative = True )
self . set_rc ( 3 , 1500 )
self . set_rc ( 2 , 1400 )
2020-02-12 18:47:09 -04:00
self . wait_distance_to_home ( 12 , 20 )
2019-06-06 21:57:13 -03:00
tstart = self . get_sim_time ( )
push_time = 70 # push against barrier for 60 seconds
failed_max = False
failed_min = False
while True :
if self . get_sim_time ( ) - tstart > push_time :
self . progress ( " Push time up " )
break
# make sure we don't RTL:
if not self . mode_is ( using_mode ) :
raise NotAchievedException ( " Changed mode away from %s " % using_mode )
distance = self . distance_to_home ( use_cached_home = True )
inner_radius = fence_radius - fence_margin
want_min = inner_radius - 1 # allow 1m either way
want_max = inner_radius + 1 # allow 1m either way
self . progress ( " Push: distance= %f %f <want< %f " %
( distance , want_min , want_max ) )
if distance < want_min :
if failed_min is False :
self . progress ( " Failed min " )
failed_min = True
if distance > want_max :
if failed_max is False :
self . progress ( " Failed max " )
failed_max = True
if failed_min and failed_max :
raise NotAchievedException ( " Failed both min and max checks. Clever " )
if failed_min :
raise NotAchievedException ( " Failed min " )
if failed_max :
raise NotAchievedException ( " Failed max " )
self . set_rc ( 2 , 1500 )
self . do_RTL ( )
def fly_fence_avoid_test ( self , timeout = 180 ) :
self . fly_fence_avoid_test_radius_check ( avoid_behave = 1 , timeout = timeout )
self . fly_fence_avoid_test_radius_check ( avoid_behave = 0 , timeout = timeout )
2019-08-05 02:53:40 -03:00
def assert_prearm_failure ( self , expected_statustext , timeout = 5 , ignore_prearm_failures = [ ] ) :
2019-03-05 23:04:46 -04:00
seen_statustext = False
seen_command_ack = False
2019-08-05 02:53:40 -03:00
self . drain_mav ( )
2019-03-05 23:04:46 -04:00
tstart = self . get_sim_time_cached ( )
2019-08-05 02:53:40 -03:00
arm_last_send = 0
2019-03-05 23:04:46 -04:00
while True :
if seen_command_ack and seen_statustext :
break
2019-08-05 02:53:40 -03:00
now = self . get_sim_time_cached ( )
if now - tstart > timeout :
2019-03-05 23:04:46 -04:00
raise NotAchievedException ( " Did not see failure-to-arm messages (statustext= %s command_ack= %s " % ( seen_statustext , seen_command_ack ) )
2019-08-05 02:53:40 -03:00
if now - arm_last_send > 1 :
arm_last_send = now
self . send_mavlink_arm_command ( )
2019-03-05 23:04:46 -04:00
m = self . mav . recv_match ( blocking = True , timeout = 1 )
if m is None :
continue
if m . get_type ( ) == " STATUSTEXT " :
2019-08-05 02:53:40 -03:00
if expected_statustext in m . text :
2019-03-05 23:04:46 -04:00
self . progress ( " Got: %s " % str ( m ) )
seen_statustext = True
2019-08-05 02:53:40 -03:00
elif " PreArm " in m . text and m . text [ 8 : ] not in ignore_prearm_failures :
2019-03-05 23:04:46 -04:00
self . progress ( " Got: %s " % str ( m ) )
raise NotAchievedException ( " Unexpected prearm failure ( %s ) " % m . text )
if m . get_type ( ) == " COMMAND_ACK " :
print ( " Got: %s " % str ( m ) )
if m . command == mavutil . mavlink . MAV_CMD_COMPONENT_ARM_DISARM :
if m . result != 4 :
raise NotAchievedException ( " command-ack says we didn ' t fail to arm " )
self . progress ( " Got: %s " % str ( m ) )
seen_command_ack = True
if self . mav . motors_armed ( ) :
raise NotAchievedException ( " Armed when we shouldn ' t have " )
2019-08-05 02:53:40 -03:00
# fly_fence_test - fly east until you hit the horizontal circular fence
def fly_fence_test ( self , timeout = 180 ) :
# enable fence, disable avoidance
self . set_parameter ( " FENCE_ENABLE " , 1 )
self . set_parameter ( " AVOID_ENABLE " , 0 )
self . change_mode ( " LOITER " )
self . wait_ready_to_arm ( )
# fence requires home to be set:
m = self . poll_home_position ( )
if m is None :
raise NotAchievedException ( " Did not receive HOME_POSITION " )
self . progress ( " home: %s " % str ( m ) )
self . start_subtest ( " ensure we can ' t arm if ouside fence " )
self . load_fence_using_mavproxy ( " fence-in-middle-of-nowhere.txt " )
self . delay_sim_time ( 5 ) # let fence check run so it loads-from-eeprom
self . assert_prearm_failure ( " vehicle outside fence " )
2019-03-05 23:04:46 -04:00
self . progress ( " Failed to arm outside fence (good!) " )
2019-08-05 02:53:40 -03:00
self . clear_fence ( )
self . delay_sim_time ( 5 ) # let fence breach clear
2019-03-05 23:04:46 -04:00
self . drain_mav ( )
2019-08-05 02:53:40 -03:00
self . end_subtest ( " ensure we can ' t arm if ouside fence " )
self . start_subtest ( " ensure we can ' t arm with bad radius " )
self . context_push ( )
self . set_parameter ( " FENCE_RADIUS " , - 1 )
self . assert_prearm_failure ( " Invalid FENCE_RADIUS value " )
self . context_pop ( )
self . progress ( " Failed to arm with bad radius " )
self . drain_mav ( )
self . end_subtest ( " ensure we can ' t arm with bad radius " )
self . start_subtest ( " ensure we can ' t arm with bad alt " )
self . context_push ( )
self . set_parameter ( " FENCE_ALT_MAX " , - 1 )
self . assert_prearm_failure ( " Invalid FENCE_ALT_MAX value " )
self . context_pop ( )
self . progress ( " Failed to arm with bad altitude " )
self . end_subtest ( " ensure we can ' t arm with bad radius " )
2019-03-05 23:04:46 -04:00
self . start_subtest ( " Check breach-fence behaviour " )
self . set_parameter ( " FENCE_TYPE " , 2 )
self . takeoff ( 10 , mode = " LOITER " )
2018-03-05 11:14:34 -04:00
# first east
2018-03-14 08:08:53 -03:00
self . progress ( " turn east " )
2018-03-05 11:14:34 -04:00
self . set_rc ( 4 , 1580 )
2018-04-27 15:21:53 -03:00
self . wait_heading ( 160 )
2018-03-05 11:14:34 -04:00
self . set_rc ( 4 , 1500 )
2019-03-07 18:28:46 -04:00
fence_radius = self . get_parameter ( " FENCE_RADIUS " )
self . progress ( " flying forward (east) until we hit fence " )
2018-03-05 11:14:34 -04:00
pitching_forward = True
self . set_rc ( 2 , 1100 )
2019-03-07 18:28:46 -04:00
self . progress ( " Waiting for fence breach " )
tstart = self . get_sim_time ( )
while not self . mode_is ( " RTL " ) :
if self . get_sim_time_cached ( ) - tstart > 30 :
self . NotAchievedException ( " Did not breach fence " )
m = self . mav . recv_match ( type = ' GLOBAL_POSITION_INT ' , blocking = True )
alt = m . relative_alt / 1000.0 # mm -> m
home_distance = self . distance_to_home ( use_cached_home = True )
self . progress ( " Alt: %.02f HomeDistance: %.02f (fence radius= %f ) " %
( alt , home_distance , fence_radius ) )
self . progress ( " Waiting until we get home and disarm " )
2018-03-05 11:14:34 -04:00
tstart = self . get_sim_time ( )
2019-03-07 18:34:09 -04:00
while self . get_sim_time_cached ( ) < tstart + timeout :
2018-05-29 05:05:56 -03:00
m = self . mav . recv_match ( type = ' GLOBAL_POSITION_INT ' , blocking = True )
alt = m . relative_alt / 1000.0 # mm -> m
2019-03-07 18:28:03 -04:00
home_distance = self . distance_to_home ( use_cached_home = True )
2018-07-02 00:22:54 -03:00
self . progress ( " Alt: %.02f HomeDistance: %.02f " %
2018-05-29 05:05:56 -03:00
( alt , home_distance ) )
2018-03-14 08:08:53 -03:00
# recenter pitch sticks once we're home so we don't fly off again
2019-03-07 18:28:46 -04:00
if pitching_forward and home_distance < 50 :
2018-03-05 11:14:34 -04:00
pitching_forward = False
2018-09-03 23:53:53 -03:00
self . set_rc ( 2 , 1475 )
2018-03-05 11:14:34 -04:00
# disable fence
2018-08-06 09:30:42 -03:00
self . set_parameter ( " FENCE_ENABLE " , 0 )
2018-08-29 09:53:55 -03:00
if ( alt < = 1 and home_distance < 10 ) or ( not self . armed ( ) and home_distance < 10 ) :
2018-03-05 11:14:34 -04:00
# reduce throttle
2018-10-10 10:07:21 -03:00
self . zero_throttle ( )
2018-12-17 02:42:21 -04:00
self . change_mode ( " LAND " )
2018-03-14 08:08:53 -03:00
self . progress ( " Waiting for disarm " )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2018-03-14 08:08:53 -03:00
self . progress ( " Reached home OK " )
2018-10-10 10:07:21 -03:00
self . zero_throttle ( )
2018-04-27 15:21:53 -03:00
return
2018-03-05 11:14:34 -04:00
2018-12-17 02:42:21 -04:00
# give we're testing RTL, doing one here probably doesn't make sense
2019-03-07 18:28:03 -04:00
home_distance = self . distance_to_home ( use_cached_home = True )
2018-10-17 23:55:16 -03:00
raise AutoTestTimeoutException (
2019-03-07 18:28:46 -04:00
" Fence test failed to reach home ( %f m distance) - "
" timed out after %u seconds " % ( home_distance , timeout , ) )
2017-08-16 10:55:21 -03:00
2018-03-05 11:14:34 -04:00
# fly_alt_fence_test - fly up until you hit the fence
2018-07-31 06:47:49 -03:00
def fly_alt_max_fence_test ( self ) :
2018-12-17 02:42:21 -04:00
self . takeoff ( 10 , mode = " LOITER " )
2018-03-05 11:14:34 -04:00
""" Hold loiter position. """
self . mavproxy . send ( ' switch 5 \n ' ) # loiter mode
self . wait_mode ( ' LOITER ' )
2011-11-13 04:24:25 -04:00
2018-03-05 11:14:34 -04:00
# enable fence, disable avoidance
2018-08-06 09:30:42 -03:00
self . set_parameter ( " FENCE_ENABLE " , 1 )
self . set_parameter ( " AVOID_ENABLE " , 0 )
self . set_parameter ( " FENCE_TYPE " , 1 )
2013-09-19 03:51:18 -03:00
2018-04-27 15:21:53 -03:00
self . change_alt ( 10 )
2018-03-05 11:14:34 -04:00
# first east
2018-03-14 08:08:53 -03:00
self . progress ( " turn east " )
2018-03-05 11:14:34 -04:00
self . set_rc ( 4 , 1580 )
2018-04-27 15:21:53 -03:00
self . wait_heading ( 160 )
2018-03-05 11:14:34 -04:00
self . set_rc ( 4 , 1500 )
# fly forward (east) at least 20m
self . set_rc ( 2 , 1100 )
2018-04-27 15:21:53 -03:00
self . wait_distance ( 20 )
2018-03-05 11:14:34 -04:00
# stop flying forward and start flying up:
self . set_rc ( 2 , 1500 )
self . set_rc ( 3 , 1800 )
# wait for fence to trigger
2018-04-14 08:31:22 -03:00
self . wait_mode ( ' RTL ' , timeout = 120 )
2018-03-05 11:14:34 -04:00
2018-03-14 08:08:53 -03:00
self . progress ( " Waiting for disarm " )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2018-03-05 11:14:34 -04:00
2018-10-10 10:07:21 -03:00
self . zero_throttle ( )
2018-03-05 11:14:34 -04:00
def fly_gps_glitch_loiter_test ( self , timeout = 30 , max_distance = 20 ) :
2018-05-09 00:32:23 -03:00
""" fly_gps_glitch_loiter_test. Fly south east in loiter and test
reaction to gps glitch . """
2018-12-17 02:42:21 -04:00
self . takeoff ( 10 , mode = " LOITER " )
2018-03-05 11:14:34 -04:00
# turn on simulator display of gps and actual position
if self . use_map :
self . show_gps_and_sim_positions ( True )
# set-up gps glitch array
2018-03-14 08:08:53 -03:00
glitch_lat = [ 0.0002996 ,
0.0006958 ,
0.0009431 ,
0.0009991 ,
0.0009444 ,
0.0007716 ,
0.0006221 ]
glitch_lon = [ 0.0000717 ,
0.0000912 ,
0.0002761 ,
0.0002626 ,
0.0002807 ,
0.0002049 ,
0.0001304 ]
2018-03-05 11:14:34 -04:00
glitch_num = len ( glitch_lat )
2018-03-14 08:08:53 -03:00
self . progress ( " GPS Glitches: " )
2018-03-05 11:14:34 -04:00
for i in range ( 1 , glitch_num ) :
2018-03-14 08:08:53 -03:00
self . progress ( " glitch %d %.7f %.7f " %
( i , glitch_lat [ i ] , glitch_lon [ i ] ) )
2018-03-05 11:14:34 -04:00
# turn south east
2018-03-14 08:08:53 -03:00
self . progress ( " turn south east " )
2018-03-05 11:14:34 -04:00
self . set_rc ( 4 , 1580 )
2018-04-27 15:21:53 -03:00
try :
self . wait_heading ( 150 )
self . set_rc ( 4 , 1500 )
# fly forward (south east) at least 60m
self . set_rc ( 2 , 1100 )
self . wait_distance ( 60 )
self . set_rc ( 2 , 1500 )
# wait for copter to slow down
except Exception as e :
2018-03-05 11:14:34 -04:00
if self . use_map :
self . show_gps_and_sim_positions ( False )
2018-04-27 15:21:53 -03:00
raise e
2018-03-05 11:14:34 -04:00
# record time and position
tstart = self . get_sim_time ( )
tnow = tstart
start_pos = self . sim_location ( )
2011-11-13 04:24:25 -04:00
2018-03-05 11:14:34 -04:00
# initialise current glitch
glitch_current = 0
2018-03-14 08:08:53 -03:00
self . progress ( " Apply first glitch " )
2019-06-20 22:05:54 -03:00
self . set_parameter ( " SIM_GPS_GLITCH_X " , glitch_lat [ glitch_current ] )
self . set_parameter ( " SIM_GPS_GLITCH_Y " , glitch_lon [ glitch_current ] )
2018-03-05 11:14:34 -04:00
# record position for 30 seconds
while tnow < tstart + timeout :
2019-03-07 18:34:09 -04:00
tnow = self . get_sim_time_cached ( )
2018-03-05 11:14:34 -04:00
desired_glitch_num = int ( ( tnow - tstart ) * 2.2 )
if desired_glitch_num > glitch_current and glitch_current != - 1 :
glitch_current = desired_glitch_num
2018-03-14 08:08:53 -03:00
# turn off glitching if we've reached the end of glitch list
2018-03-05 11:14:34 -04:00
if glitch_current > = glitch_num :
glitch_current = - 1
2018-03-14 08:08:53 -03:00
self . progress ( " Completed Glitches " )
2018-08-06 09:30:42 -03:00
self . set_parameter ( " SIM_GPS_GLITCH_X " , 0 )
self . set_parameter ( " SIM_GPS_GLITCH_Y " , 0 )
2018-03-05 11:14:34 -04:00
else :
2018-03-14 08:08:53 -03:00
self . progress ( " Applying glitch %u " % glitch_current )
2018-03-05 11:14:34 -04:00
# move onto the next glitch
2019-06-20 22:05:54 -03:00
self . set_parameter ( " SIM_GPS_GLITCH_X " , glitch_lat [ glitch_current ] )
self . set_parameter ( " SIM_GPS_GLITCH_Y " , glitch_lon [ glitch_current ] )
2018-03-05 11:14:34 -04:00
# start displaying distance moved after all glitches applied
if glitch_current == - 1 :
2018-05-29 05:05:56 -03:00
m = self . mav . recv_match ( type = ' GLOBAL_POSITION_INT ' ,
blocking = True )
alt = m . alt / 1000.0 # mm -> m
2018-03-05 11:14:34 -04:00
curr_pos = self . sim_location ( )
moved_distance = self . get_distance ( curr_pos , start_pos )
2018-07-02 00:22:54 -03:00
self . progress ( " Alt: %.02f Moved: %.0f " %
( alt , moved_distance ) )
2018-03-05 11:14:34 -04:00
if moved_distance > max_distance :
2018-10-17 23:55:16 -03:00
raise NotAchievedException (
" Moved over %u meters, Failed! " % max_distance )
2019-03-07 18:34:09 -04:00
else :
self . drain_mav ( )
2018-03-05 11:14:34 -04:00
# disable gps glitch
if glitch_current != - 1 :
2018-08-06 09:30:42 -03:00
self . set_parameter ( " SIM_GPS_GLITCH_X " , 0 )
self . set_parameter ( " SIM_GPS_GLITCH_Y " , 0 )
2018-03-05 11:14:34 -04:00
if self . use_map :
self . show_gps_and_sim_positions ( False )
2018-04-27 15:21:53 -03:00
self . progress ( " GPS glitch test passed! "
" stayed within %u meters for %u seconds " %
( max_distance , timeout ) )
2019-02-26 00:29:12 -04:00
self . do_RTL ( )
2019-03-10 21:59:12 -03:00
# re-arming is problematic because the GPS is glitching!
self . reboot_sitl ( )
2018-03-05 11:14:34 -04:00
# fly_gps_glitch_auto_test - fly mission and test reaction to gps glitch
2019-03-13 00:09:59 -03:00
def fly_gps_glitch_auto_test ( self , timeout = 180 ) :
2018-03-05 11:14:34 -04:00
# set-up gps glitch array
2018-03-14 08:08:53 -03:00
glitch_lat = [ 0.0002996 ,
0.0006958 ,
0.0009431 ,
0.0009991 ,
0.0009444 ,
0.0007716 ,
0.0006221 ]
glitch_lon = [ 0.0000717 ,
0.0000912 ,
0.0002761 ,
0.0002626 ,
0.0002807 ,
0.0002049 ,
0.0001304 ]
2018-03-05 11:14:34 -04:00
glitch_num = len ( glitch_lat )
2018-03-14 08:08:53 -03:00
self . progress ( " GPS Glitches: " )
2018-03-05 11:14:34 -04:00
for i in range ( 1 , glitch_num ) :
2018-03-14 08:08:53 -03:00
self . progress ( " glitch %d %.7f %.7f " %
( i , glitch_lat [ i ] , glitch_lon [ i ] ) )
2018-03-05 11:14:34 -04:00
# Fly mission #1
2018-03-14 08:08:53 -03:00
self . progress ( " # Load copter_glitch_mission " )
2018-03-05 11:14:34 -04:00
# load the waypoint count
2018-03-14 08:08:53 -03:00
num_wp = self . load_mission ( " copter_glitch_mission.txt " )
2017-08-16 10:55:21 -03:00
if not num_wp :
2018-10-17 23:55:16 -03:00
raise NotAchievedException ( " load copter_glitch_mission failed " )
2018-03-05 11:14:34 -04:00
# turn on simulator display of gps and actual position
if self . use_map :
self . show_gps_and_sim_positions ( True )
2018-03-14 08:08:53 -03:00
self . progress ( " test: Fly a mission from 1 to %u " % num_wp )
2018-03-05 11:14:34 -04:00
self . mavproxy . send ( ' wp set 1 \n ' )
2018-12-17 02:42:21 -04:00
self . change_mode ( " STABILIZE " )
self . wait_ready_to_arm ( )
2018-10-10 10:07:21 -03:00
self . zero_throttle ( )
2018-12-17 02:42:21 -04:00
self . arm_vehicle ( )
2018-03-05 11:14:34 -04:00
# switch into AUTO mode and raise throttle
self . mavproxy . send ( ' switch 4 \n ' ) # auto mode
self . wait_mode ( ' AUTO ' )
self . set_rc ( 3 , 1500 )
# wait until 100m from home
2018-04-27 15:21:53 -03:00
try :
2018-10-16 23:56:06 -03:00
self . wait_distance ( 100 , 5 , 90 )
2018-04-27 15:21:53 -03:00
except Exception as e :
2018-03-05 11:14:34 -04:00
if self . use_map :
self . show_gps_and_sim_positions ( False )
2018-04-27 15:21:53 -03:00
raise e
2018-03-05 11:14:34 -04:00
# record time and position
tstart = self . get_sim_time ( )
# initialise current glitch
glitch_current = 0
2018-03-14 08:08:53 -03:00
self . progress ( " Apply first glitch " )
2019-06-20 22:05:54 -03:00
self . set_parameter ( " SIM_GPS_GLITCH_X " , glitch_lat [ glitch_current ] )
self . set_parameter ( " SIM_GPS_GLITCH_Y " , glitch_lon [ glitch_current ] )
2018-03-05 11:14:34 -04:00
# record position for 30 seconds
while glitch_current < glitch_num :
tnow = self . get_sim_time ( )
desired_glitch_num = int ( ( tnow - tstart ) * 2.2 )
if desired_glitch_num > glitch_current and glitch_current != - 1 :
glitch_current = desired_glitch_num
# apply next glitch
if glitch_current < glitch_num :
2018-03-14 08:08:53 -03:00
self . progress ( " Applying glitch %u " % glitch_current )
2019-06-20 22:05:54 -03:00
self . set_parameter ( " SIM_GPS_GLITCH_X " ,
2018-03-14 08:08:53 -03:00
glitch_lat [ glitch_current ] )
2019-06-20 22:05:54 -03:00
self . set_parameter ( " SIM_GPS_GLITCH_Y " ,
2018-03-14 08:08:53 -03:00
glitch_lon [ glitch_current ] )
2018-03-05 11:14:34 -04:00
# turn off glitching
2018-03-14 08:08:53 -03:00
self . progress ( " Completed Glitches " )
2018-08-06 09:30:42 -03:00
self . set_parameter ( " SIM_GPS_GLITCH_X " , 0 )
self . set_parameter ( " SIM_GPS_GLITCH_Y " , 0 )
2018-03-05 11:14:34 -04:00
# continue with the mission
2018-04-27 15:21:53 -03:00
self . wait_waypoint ( 0 , num_wp - 1 , timeout = 500 )
2018-03-05 11:14:34 -04:00
# wait for arrival back home
2020-02-12 18:47:09 -04:00
self . wait_distance_to_home ( 0 , 10 , timeout = timeout )
2018-03-05 11:14:34 -04:00
# turn off simulator display of gps and actual position
if self . use_map :
self . show_gps_and_sim_positions ( False )
2018-04-27 15:21:53 -03:00
self . progress ( " GPS Glitch test Auto completed: passed! " )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2019-03-10 21:59:12 -03:00
# re-arming is problematic because the GPS is glitching!
self . reboot_sitl ( )
2018-03-05 11:14:34 -04:00
2018-03-14 08:08:53 -03:00
# fly_simple - assumes the simple bearing is initialised to be
# directly north flies a box with 100m west, 15 seconds north,
# 50 seconds east, 15 seconds south
2018-07-31 06:47:49 -03:00
def fly_simple ( self , side = 50 ) :
2018-12-17 02:42:21 -04:00
self . takeoff ( 10 , mode = " LOITER " )
2018-03-05 11:14:34 -04:00
# hold position in loiter
self . mavproxy . send ( ' switch 5 \n ' ) # loiter mode
self . wait_mode ( ' LOITER ' )
# set SIMPLE mode for all flight modes
2018-08-06 09:30:42 -03:00
self . set_parameter ( " SIMPLE " , 63 )
2018-03-05 11:14:34 -04:00
# switch to stabilize mode
self . mavproxy . send ( ' switch 6 \n ' )
self . wait_mode ( ' STABILIZE ' )
2020-06-03 23:17:17 -03:00
self . set_rc ( 3 , 1545 )
2018-03-05 11:14:34 -04:00
# fly south 50m
2018-03-14 08:08:53 -03:00
self . progress ( " # Flying south %u meters " % side )
2018-03-05 11:14:34 -04:00
self . set_rc ( 1 , 1300 )
2018-04-27 15:21:53 -03:00
self . wait_distance ( side , 5 , 60 )
2018-03-05 11:14:34 -04:00
self . set_rc ( 1 , 1500 )
# fly west 8 seconds
2018-03-14 08:08:53 -03:00
self . progress ( " # Flying west for 8 seconds " )
2018-03-05 11:14:34 -04:00
self . set_rc ( 2 , 1300 )
tstart = self . get_sim_time ( )
2019-03-07 18:34:09 -04:00
while self . get_sim_time_cached ( ) < ( tstart + 8 ) :
2018-03-14 08:08:53 -03:00
self . mav . recv_match ( type = ' VFR_HUD ' , blocking = True )
2018-03-05 11:14:34 -04:00
self . set_rc ( 2 , 1500 )
# fly north 25 meters
2018-03-14 08:08:53 -03:00
self . progress ( " # Flying north %u meters " % ( side / 2.0 ) )
2018-03-05 11:14:34 -04:00
self . set_rc ( 1 , 1700 )
2018-04-27 15:21:53 -03:00
self . wait_distance ( side / 2 , 5 , 60 )
2018-03-05 11:14:34 -04:00
self . set_rc ( 1 , 1500 )
# fly east 8 seconds
2018-03-14 08:08:53 -03:00
self . progress ( " # Flying east for 8 seconds " )
2018-03-05 11:14:34 -04:00
self . set_rc ( 2 , 1700 )
tstart = self . get_sim_time ( )
2019-03-07 18:34:09 -04:00
while self . get_sim_time_cached ( ) < ( tstart + 8 ) :
2018-03-14 08:08:53 -03:00
self . mav . recv_match ( type = ' VFR_HUD ' , blocking = True )
2018-03-05 11:14:34 -04:00
self . set_rc ( 2 , 1500 )
# hover in place
self . hover ( )
2018-12-17 02:42:21 -04:00
self . do_RTL ( )
2018-03-05 11:14:34 -04:00
# fly_super_simple - flies a circle around home for 45 seconds
def fly_super_simple ( self , timeout = 45 ) :
2018-12-17 02:42:21 -04:00
self . takeoff ( 10 , mode = " LOITER " )
2018-03-05 11:14:34 -04:00
# fly forward 20m
2018-03-14 08:08:53 -03:00
self . progress ( " # Flying forward 20 meters " )
2018-03-05 11:14:34 -04:00
self . set_rc ( 2 , 1300 )
2018-04-27 15:21:53 -03:00
self . wait_distance ( 20 , 5 , 60 )
2018-03-05 11:14:34 -04:00
self . set_rc ( 2 , 1500 )
# set SUPER SIMPLE mode for all flight modes
2018-08-06 09:30:42 -03:00
self . set_parameter ( " SUPER_SIMPLE " , 63 )
2018-03-05 11:14:34 -04:00
# switch to stabilize mode
2018-12-17 02:42:21 -04:00
self . change_mode ( " STABILIZE " )
2020-07-04 03:13:23 -03:00
self . set_rc ( 3 , 1700 )
2018-03-05 11:14:34 -04:00
# start copter yawing slowly
self . set_rc ( 4 , 1550 )
# roll left for timeout seconds
2018-03-14 08:08:53 -03:00
self . progress ( " # rolling left from pilot ' s POV for %u seconds "
% timeout )
2018-03-05 11:14:34 -04:00
self . set_rc ( 1 , 1300 )
tstart = self . get_sim_time ( )
2019-03-07 18:34:09 -04:00
while self . get_sim_time_cached ( ) < ( tstart + timeout ) :
2018-03-14 08:08:53 -03:00
self . mav . recv_match ( type = ' VFR_HUD ' , blocking = True )
2018-03-05 11:14:34 -04:00
# stop rolling and yawing
self . set_rc ( 1 , 1500 )
self . set_rc ( 4 , 1500 )
# restore simple mode parameters to default
2018-08-06 09:30:42 -03:00
self . set_parameter ( " SUPER_SIMPLE " , 0 )
2018-03-05 11:14:34 -04:00
# hover in place
self . hover ( )
2018-12-17 02:42:21 -04:00
self . do_RTL ( )
2018-03-05 11:14:34 -04:00
# fly_circle - flies a circle with 20m radius
2018-07-31 06:47:49 -03:00
def fly_circle ( self , holdtime = 36 ) :
2020-07-01 23:12:40 -03:00
# the following should not be required. But there appears to
# be a physics failure in the simulation which is causing CI
# to fall over a lot. -pb 202007021209
self . reboot_sitl ( )
2018-12-17 02:42:21 -04:00
self . takeoff ( 10 , mode = " LOITER " )
2018-03-05 11:14:34 -04:00
# face west
2018-03-14 08:08:53 -03:00
self . progress ( " turn west " )
2018-03-05 11:14:34 -04:00
self . set_rc ( 4 , 1580 )
2018-04-27 15:21:53 -03:00
self . wait_heading ( 270 )
2018-03-05 11:14:34 -04:00
self . set_rc ( 4 , 1500 )
# set CIRCLE radius
2018-08-06 09:30:42 -03:00
self . set_parameter ( " CIRCLE_RADIUS " , 3000 )
2018-03-05 11:14:34 -04:00
# fly forward (east) at least 100m
self . set_rc ( 2 , 1100 )
2018-04-27 15:21:53 -03:00
self . wait_distance ( 100 )
2018-03-05 11:14:34 -04:00
# return pitch stick back to middle
self . set_rc ( 2 , 1500 )
# set CIRCLE mode
self . mavproxy . send ( ' switch 1 \n ' ) # circle mode
self . wait_mode ( ' CIRCLE ' )
# wait
m = self . mav . recv_match ( type = ' VFR_HUD ' , blocking = True )
start_altitude = m . alt
tstart = self . get_sim_time ( )
2018-03-14 08:08:53 -03:00
self . progress ( " Circle at %u meters for %u seconds " %
( start_altitude , holdtime ) )
2019-03-07 18:34:09 -04:00
while self . get_sim_time_cached ( ) < tstart + holdtime :
2018-03-05 11:14:34 -04:00
m = self . mav . recv_match ( type = ' VFR_HUD ' , blocking = True )
2018-07-02 00:22:54 -03:00
self . progress ( " heading %d " % m . heading )
2018-03-05 11:14:34 -04:00
2018-03-14 08:08:53 -03:00
self . progress ( " CIRCLE OK for %u seconds " % holdtime )
2018-03-05 11:14:34 -04:00
2018-12-17 02:42:21 -04:00
self . do_RTL ( )
2019-02-02 00:19:18 -04:00
def wait_attitude ( self , desroll = None , despitch = None , timeout = 2 , tolerance = 10 ) :
''' wait for an attitude (degrees) '''
if desroll is None and despitch is None :
raise ValueError ( " despitch or desroll must be supplied " )
tstart = self . get_sim_time ( )
while True :
if self . get_sim_time_cached ( ) - tstart > 2 :
raise AutoTestTimeoutException ( " Failed to achieve attitude " )
m = self . mav . recv_match ( type = ' ATTITUDE ' , blocking = True )
roll_deg = math . degrees ( m . roll )
pitch_deg = math . degrees ( m . pitch )
self . progress ( " wait_att: roll= %f desroll= %s pitch= %f despitch= %s " %
( roll_deg , desroll , pitch_deg , despitch ) )
if desroll is not None and abs ( roll_deg - desroll ) > tolerance :
continue
if despitch is not None and abs ( pitch_deg - despitch ) > tolerance :
continue
return
def fly_flip ( self ) :
ex = None
try :
self . mavproxy . send ( " set streamrate -1 \n " )
self . set_message_rate_hz ( mavutil . mavlink . MAVLINK_MSG_ID_ATTITUDE , 100 )
self . takeoff ( 20 )
self . hover ( )
old_speedup = self . get_parameter ( " SIM_SPEEDUP " )
self . set_parameter ( ' SIM_SPEEDUP ' , 1 )
self . progress ( " Flipping in roll " )
self . set_rc ( 1 , 1700 )
self . mavproxy . send ( ' mode FLIP \n ' ) # don't wait for heartbeat!
self . wait_attitude ( despitch = 0 , desroll = 45 , tolerance = 30 )
self . wait_attitude ( despitch = 0 , desroll = 90 , tolerance = 30 )
self . wait_attitude ( despitch = 0 , desroll = - 45 , tolerance = 30 )
self . progress ( " Waiting for level " )
self . set_rc ( 1 , 1500 ) # can't change quickly enough!
self . wait_attitude ( despitch = 0 , desroll = 0 , tolerance = 5 )
self . progress ( " Regaining altitude " )
self . change_mode ( ' STABILIZE ' )
self . set_rc ( 3 , 1800 )
self . wait_for_alt ( 20 )
self . hover ( )
self . progress ( " Flipping in pitch " )
self . set_rc ( 2 , 1700 )
self . mavproxy . send ( ' mode FLIP \n ' ) # don't wait for heartbeat!
self . wait_attitude ( despitch = 45 , desroll = 0 , tolerance = 30 )
# can't check roll here as it flips from 0 to -180..
self . wait_attitude ( despitch = 90 , tolerance = 30 )
self . wait_attitude ( despitch = - 45 , tolerance = 30 )
self . progress ( " Waiting for level " )
self . set_rc ( 1 , 1500 ) # can't change quickly enough!
self . wait_attitude ( despitch = 0 , desroll = 0 , tolerance = 5 )
self . set_parameter ( ' SIM_SPEEDUP ' , old_speedup )
self . change_mode ( ' RTL ' )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2019-02-02 00:19:18 -04:00
except Exception as e :
ex = e
self . set_message_rate_hz ( mavutil . mavlink . MAVLINK_MSG_ID_ATTITUDE , 0 )
sr = self . sitl_streamrate ( )
self . mavproxy . send ( " set streamrate %u \n " % sr )
if ex is not None :
raise ex
2018-10-02 19:28:21 -03:00
# fly_optical_flow_limits - test EKF navigation limiting
def fly_optical_flow_limits ( self ) :
ex = None
self . context_push ( )
try :
2018-12-17 02:42:21 -04:00
2018-10-02 19:28:21 -03:00
self . set_parameter ( " SIM_FLOW_ENABLE " , 1 )
2019-04-04 02:11:14 -03:00
self . set_parameter ( " FLOW_TYPE " , 10 )
2018-10-02 19:28:21 -03:00
2019-07-10 10:57:46 -03:00
self . set_analog_rangefinder_parameters ( )
2018-10-02 19:28:21 -03:00
self . set_parameter ( " SIM_GPS_DISABLE " , 1 )
2018-11-15 04:05:24 -04:00
self . set_parameter ( " SIM_TERRAIN " , 0 )
2018-10-02 19:28:21 -03:00
self . reboot_sitl ( )
self . takeoff ( alt_min = 2 , require_absolute = False )
self . mavproxy . send ( ' mode loiter \n ' )
self . wait_mode ( ' LOITER ' )
# speed should be limited to <10m/s
self . set_rc ( 2 , 1000 )
tstart = self . get_sim_time ( )
timeout = 60
while self . get_sim_time_cached ( ) - tstart < timeout :
m = self . mav . recv_match ( type = ' VFR_HUD ' , blocking = True )
spd = m . groundspeed
2018-11-06 04:55:55 -04:00
max_speed = 8
self . progress ( " %0.1f : Low Speed: %f (want <= %u ) " %
( self . get_sim_time_cached ( ) - tstart ,
spd ,
max_speed ) )
if spd > max_speed :
2018-10-17 23:55:16 -03:00
raise NotAchievedException ( ( " Speed should be limited by "
" EKF optical flow limits " ) )
2018-10-02 19:28:21 -03:00
self . progress ( " Moving higher " )
self . change_alt ( 60 )
self . wait_groundspeed ( 10 , 100 , timeout = 60 )
except Exception as e :
ex = e
self . set_rc ( 2 , 1500 )
self . context_pop ( )
2019-02-26 00:29:12 -04:00
self . disarm_vehicle ( force = True )
2018-10-02 19:28:21 -03:00
self . reboot_sitl ( )
if ex is not None :
raise ex
2018-04-17 23:47:31 -03:00
def fly_autotune ( self ) :
2020-06-27 07:49:17 -03:00
""" Test autotune mode """
2018-04-17 23:47:31 -03:00
2020-06-27 07:49:17 -03:00
rlld = self . get_parameter ( " ATC_RAT_RLL_D " )
rlli = self . get_parameter ( " ATC_RAT_RLL_I " )
rllp = self . get_parameter ( " ATC_RAT_RLL_P " )
2018-12-17 02:42:21 -04:00
self . takeoff ( 10 )
2018-04-17 23:47:31 -03:00
# hold position in loiter
self . mavproxy . send ( ' mode autotune \n ' )
self . wait_mode ( ' AUTOTUNE ' )
tstart = self . get_sim_time ( )
sim_time_expected = 5000
deadline = tstart + sim_time_expected
while self . get_sim_time_cached ( ) < deadline :
now = self . get_sim_time_cached ( )
m = self . mav . recv_match ( type = ' STATUSTEXT ' ,
blocking = True ,
timeout = 1 )
if m is None :
continue
self . progress ( " STATUSTEXT ( %u < %u ): %s " % ( now , deadline , m . text ) )
if " AutoTune: Success " in m . text :
self . progress ( " AUTOTUNE OK ( %u seconds) " % ( now - tstart ) )
# near enough for now:
2018-12-17 02:42:21 -04:00
self . change_mode ( ' LAND ' )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2020-06-27 07:49:17 -03:00
# check the original gains have been re-instated
if ( rlld != self . get_parameter ( " ATC_RAT_RLL_D " )
or rlli != self . get_parameter ( " ATC_RAT_RLL_I " )
or rllp != self . get_parameter ( " ATC_RAT_RLL_P " ) ) :
raise NotAchievedException ( " AUTOTUNE gains still present " )
2018-04-17 23:47:31 -03:00
return
2018-10-17 23:55:16 -03:00
raise NotAchievedException ( " AUTOTUNE failed ( %u seconds) " %
2020-06-27 07:49:17 -03:00
( self . get_sim_time ( ) - tstart ) )
def fly_autotune_switch ( self ) :
""" Test autotune on a switch with gains being saved """
self . set_parameter ( " RC8_OPTION " , 17 )
self . set_parameter ( " ATC_RAT_RLL_FLTT " , 20 )
rlld = self . get_parameter ( " ATC_RAT_RLL_D " )
rlli = self . get_parameter ( " ATC_RAT_RLL_I " )
rllp = self . get_parameter ( " ATC_RAT_RLL_P " )
rllt = self . get_parameter ( " ATC_RAT_RLL_FLTT " )
self . progress ( " AUTOTUNE pre-gains are P: %f I: %f D: %f " % ( ( self . get_parameter ( " ATC_RAT_RLL_P " ) ) ,
self . get_parameter ( " ATC_RAT_RLL_I " ) , self . get_parameter ( " ATC_RAT_RLL_D " ) ) )
self . takeoff ( 10 , mode = ' LOITER ' )
# hold position in loiter and run autotune
self . set_rc ( 8 , 1850 )
self . wait_mode ( ' AUTOTUNE ' )
tstart = self . get_sim_time ( )
sim_time_expected = 5000
deadline = tstart + sim_time_expected
while self . get_sim_time_cached ( ) < deadline :
now = self . get_sim_time_cached ( )
m = self . mav . recv_match ( type = ' STATUSTEXT ' ,
blocking = True ,
timeout = 1 )
if m is None :
continue
self . progress ( " STATUSTEXT ( %u < %u ): %s " % ( now , deadline , m . text ) )
if " AutoTune: Success " in m . text :
self . progress ( " AUTOTUNE OK ( %u seconds) " % ( now - tstart ) )
# Check original gains are re-instated
self . set_rc ( 8 , 1100 )
self . delay_sim_time ( 1 )
self . progress ( " AUTOTUNE original gains are P: %f I: %f D: %f " % \
( self . get_parameter ( " ATC_RAT_RLL_P " ) , self . get_parameter ( " ATC_RAT_RLL_I " ) , \
self . get_parameter ( " ATC_RAT_RLL_D " ) ) )
if ( rlld != self . get_parameter ( " ATC_RAT_RLL_D " )
or rlli != self . get_parameter ( " ATC_RAT_RLL_I " )
or rllp != self . get_parameter ( " ATC_RAT_RLL_P " ) ) :
raise NotAchievedException ( " AUTOTUNE gains still present " )
# Use autotuned gains
self . set_rc ( 8 , 1850 )
self . delay_sim_time ( 1 )
self . progress ( " AUTOTUNE testing gains are P: %f I: %f D: %f " % \
( self . get_parameter ( " ATC_RAT_RLL_P " ) , self . get_parameter ( " ATC_RAT_RLL_I " ) , \
self . get_parameter ( " ATC_RAT_RLL_D " ) ) )
if ( rlld == self . get_parameter ( " ATC_RAT_RLL_D " )
or rlli == self . get_parameter ( " ATC_RAT_RLL_I " )
or rllp == self . get_parameter ( " ATC_RAT_RLL_P " ) ) :
raise NotAchievedException ( " AUTOTUNE gains not present in pilot testing " )
# land without changing mode
self . set_rc ( 3 , 1000 )
self . wait_for_alt ( 0 )
self . wait_disarmed ( )
# Check gains are still there after disarm
if ( rlld == self . get_parameter ( " ATC_RAT_RLL_D " )
or rlli == self . get_parameter ( " ATC_RAT_RLL_I " )
or rllp == self . get_parameter ( " ATC_RAT_RLL_P " ) ) :
raise NotAchievedException ( " AUTOTUNE gains not present on disarm " )
self . reboot_sitl ( )
# Check gains are still there after reboot
if ( rlld == self . get_parameter ( " ATC_RAT_RLL_D " ) or
rlli == self . get_parameter ( " ATC_RAT_RLL_I " ) or
rllp == self . get_parameter ( " ATC_RAT_RLL_P " ) ) :
raise NotAchievedException ( " AUTOTUNE gains not present on reboot " )
# Check FLTT is unchanged
if rllt != self . get_parameter ( " ATC_RAT_RLL_FLTT " ) :
raise NotAchievedException ( " AUTOTUNE FLTT was modified " )
return
raise NotAchievedException ( " AUTOTUNE failed ( %u seconds) " %
( self . get_sim_time ( ) - tstart ) )
2018-04-17 23:47:31 -03:00
2018-03-05 11:14:34 -04:00
# fly_auto_test - fly mission which tests a significant number of commands
def fly_auto_test ( self ) :
# Fly mission #1
2018-03-14 08:08:53 -03:00
self . progress ( " # Load copter_mission " )
2018-03-05 11:14:34 -04:00
# load the waypoint count
2018-03-14 08:08:53 -03:00
num_wp = self . load_mission ( " copter_mission.txt " )
2018-03-05 11:14:34 -04:00
if not num_wp :
2018-10-17 23:55:16 -03:00
raise NotAchievedException ( " load copter_mission failed " )
2013-05-19 04:19:59 -03:00
2018-03-14 08:08:53 -03:00
self . progress ( " test: Fly a mission from 1 to %u " % num_wp )
2018-03-05 11:14:34 -04:00
self . mavproxy . send ( ' wp set 1 \n ' )
2013-09-22 23:45:30 -03:00
2018-12-17 02:42:21 -04:00
self . change_mode ( " LOITER " )
self . wait_ready_to_arm ( )
self . arm_vehicle ( )
2018-03-05 11:14:34 -04:00
# switch into AUTO mode and raise throttle
2018-12-17 02:42:21 -04:00
self . change_mode ( " AUTO " )
2018-03-05 11:14:34 -04:00
self . set_rc ( 3 , 1500 )
2013-09-22 23:45:30 -03:00
2018-03-05 11:14:34 -04:00
# fly the mission
2018-05-09 01:22:13 -03:00
self . wait_waypoint ( 0 , num_wp - 1 , timeout = 500 )
2013-09-22 23:45:30 -03:00
2018-03-05 11:14:34 -04:00
# set throttle to minimum
2018-10-10 10:07:21 -03:00
self . zero_throttle ( )
2011-12-16 00:39:05 -04:00
2018-03-05 11:14:34 -04:00
# wait for disarm
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2018-03-14 08:08:53 -03:00
self . progress ( " MOTORS DISARMED OK " )
2011-12-16 00:39:05 -04:00
2018-04-27 15:21:53 -03:00
self . progress ( " Auto mission completed: passed! " )
2011-11-13 04:24:25 -04:00
2017-04-10 14:48:48 -03:00
def fly_motor_fail ( self , fail_servo = 0 , fail_mul = 0.0 , holdtime = 30 ) :
""" Test flight with reduced motor efficiency """
# we only expect an octocopter to survive ATM:
servo_counts = {
2018-07-31 06:50:02 -03:00
# 2: 6, # hexa
3 : 8 , # octa
# 5: 6, # Y6
2017-04-10 14:48:48 -03:00
}
frame_class = int ( self . get_parameter ( " FRAME_CLASS " ) )
if frame_class not in servo_counts :
self . progress ( " Test not relevant for frame_class %u " % frame_class )
return
servo_count = servo_counts [ frame_class ]
if fail_servo < 0 or fail_servo > servo_count :
raise ValueError ( ' fail_servo outside range for frame class ' )
2018-12-17 02:42:21 -04:00
self . takeoff ( 10 , mode = " LOITER " )
2017-04-10 14:48:48 -03:00
self . change_alt ( alt_min = 50 )
# Get initial values
start_hud = self . mav . recv_match ( type = ' VFR_HUD ' , blocking = True )
start_attitude = self . mav . recv_match ( type = ' ATTITUDE ' , blocking = True )
hover_time = 5
try :
tstart = self . get_sim_time ( )
int_error_alt = 0
int_error_yaw_rate = 0
int_error_yaw = 0
self . progress ( " Hovering for %u seconds " % hover_time )
failed = False
2019-03-07 18:34:09 -04:00
while True :
now = self . get_sim_time_cached ( )
if now - tstart > holdtime + hover_time :
break
2017-04-10 14:48:48 -03:00
servo = self . mav . recv_match ( type = ' SERVO_OUTPUT_RAW ' ,
blocking = True )
hud = self . mav . recv_match ( type = ' VFR_HUD ' , blocking = True )
attitude = self . mav . recv_match ( type = ' ATTITUDE ' , blocking = True )
2019-03-07 18:34:09 -04:00
if not failed and now - tstart > hover_time :
2017-04-10 14:48:48 -03:00
self . progress ( " Killing motor %u ( %u %% ) " %
( fail_servo + 1 , fail_mul ) )
self . set_parameter ( " SIM_ENGINE_FAIL " , fail_servo )
self . set_parameter ( " SIM_ENGINE_MUL " , fail_mul )
failed = True
if failed :
2019-03-07 18:34:09 -04:00
self . progress ( " Hold Time: %f / %f " % ( now - tstart , holdtime ) )
2017-04-10 14:48:48 -03:00
servo_pwm = [ servo . servo1_raw ,
servo . servo2_raw ,
servo . servo3_raw ,
servo . servo4_raw ,
servo . servo5_raw ,
servo . servo6_raw ,
servo . servo7_raw ,
servo . servo8_raw ]
self . progress ( " PWM output per motor " )
for i , pwm in enumerate ( servo_pwm [ 0 : servo_count ] ) :
if pwm > 1900 :
state = " oversaturated "
elif pwm < 1200 :
state = " undersaturated "
else :
state = " OK "
2018-07-31 06:50:02 -03:00
if failed and i == fail_servo :
2017-04-10 14:48:48 -03:00
state + = " (failed) "
self . progress ( " servo %u [pwm= %u ] [ %s ] " % ( i + 1 , pwm , state ) )
alt_delta = hud . alt - start_hud . alt
yawrate_delta = attitude . yawspeed - start_attitude . yawspeed
yaw_delta = attitude . yaw - start_attitude . yaw
self . progress ( " Alt= %f m (delta= %f m) " % ( hud . alt , alt_delta ) )
self . progress ( " Yaw rate= %f (delta= %f ) (rad/s) " %
( attitude . yawspeed , yawrate_delta ) )
self . progress ( " Yaw= %f (delta= %f ) (deg) " %
( attitude . yaw , yaw_delta ) )
2019-03-07 18:34:09 -04:00
dt = self . get_sim_time ( ) - now
2017-04-10 14:48:48 -03:00
int_error_alt + = abs ( alt_delta / dt )
int_error_yaw_rate + = abs ( yawrate_delta / dt )
int_error_yaw + = abs ( yaw_delta / dt )
self . progress ( " ## Error Integration ## " )
self . progress ( " Altitude: %f m " % int_error_alt )
self . progress ( " Yaw rate: %f rad/s " % int_error_yaw_rate )
self . progress ( " Yaw: %f deg " % int_error_yaw )
self . progress ( " ---- " )
2019-03-25 00:56:02 -03:00
if int_error_yaw_rate > 0.1 :
raise NotAchievedException ( " Vehicle is spinning " )
2017-04-10 14:48:48 -03:00
if alt_delta < - 20 :
2018-10-17 23:55:16 -03:00
raise NotAchievedException ( " Vehicle is descending " )
2017-04-10 14:48:48 -03:00
self . set_parameter ( " SIM_ENGINE_FAIL " , 0 )
self . set_parameter ( " SIM_ENGINE_MUL " , 1.0 )
except Exception as e :
self . set_parameter ( " SIM_ENGINE_FAIL " , 0 )
self . set_parameter ( " SIM_ENGINE_MUL " , 1.0 )
raise e
2018-12-17 02:42:21 -04:00
self . do_RTL ( )
2017-04-10 14:48:48 -03:00
2019-10-03 13:12:34 -03:00
def fly_motor_vibration ( self ) :
""" Test flight with motor vibration """
self . context_push ( )
ex = None
try :
self . set_rc_default ( )
2019-07-20 11:41:39 -03:00
# magic tridge EKF type that dramatically speeds up the test
self . set_parameter ( " AHRS_EKF_TYPE " , 10 )
2019-10-03 13:12:34 -03:00
self . set_parameter ( " INS_LOG_BAT_MASK " , 3 )
2019-12-24 17:20:58 -04:00
self . set_parameter ( " INS_LOG_BAT_OPT " , 0 )
2019-10-03 13:12:34 -03:00
self . set_parameter ( " LOG_BITMASK " , 958 )
self . set_parameter ( " LOG_DISARMED " , 0 )
self . set_parameter ( " SIM_VIB_MOT_MAX " , 350 )
2019-12-24 17:20:58 -04:00
# these are real values taken from a 180mm Quad
self . set_parameter ( " SIM_GYR_RND " , 20 )
self . set_parameter ( " SIM_ACC_RND " , 5 )
self . set_parameter ( " SIM_ACC2_RND " , 5 )
self . set_parameter ( " SIM_INS_THR_MIN " , 0.1 )
2019-10-03 13:12:34 -03:00
self . reboot_sitl ( )
2019-12-24 17:20:58 -04:00
self . takeoff ( 15 , mode = " ALT_HOLD " )
2019-10-03 13:12:34 -03:00
2019-12-24 17:20:58 -04:00
hover_time = 15
2019-10-03 13:12:34 -03:00
tstart = self . get_sim_time ( )
self . progress ( " Hovering for %u seconds " % hover_time )
while self . get_sim_time_cached ( ) < tstart + hover_time :
attitude = self . mav . recv_match ( type = ' ATTITUDE ' , blocking = True )
tend = self . get_sim_time ( )
self . do_RTL ( )
psd = self . mavfft_fttd ( 1 , 0 , tstart * 1.0e6 , tend * 1.0e6 )
2019-12-24 17:20:58 -04:00
# ignore the first 20Hz and look for a peak at -15dB or more
2019-10-03 13:12:34 -03:00
ignore_bins = 20
freq = psd [ " F " ] [ numpy . argmax ( psd [ " X " ] [ ignore_bins : ] ) + ignore_bins ]
2019-12-24 17:20:58 -04:00
if numpy . amax ( psd [ " X " ] [ ignore_bins : ] ) < - 15 or freq < 200 or freq > 300 :
2019-10-03 13:12:34 -03:00
raise NotAchievedException ( " Did not detect a motor peak, found %f at %f dB " % ( freq , numpy . amax ( psd [ " X " ] [ ignore_bins : ] ) ) )
else :
self . progress ( " Detected motor peak at %f Hz " % freq )
2019-12-24 17:20:58 -04:00
# now add a notch and check that post-filter the peak is squashed below 40dB
self . set_parameter ( " INS_LOG_BAT_OPT " , 2 )
2019-10-03 13:12:34 -03:00
self . set_parameter ( " INS_NOTCH_ENABLE " , 1 )
self . set_parameter ( " INS_NOTCH_FREQ " , freq )
self . set_parameter ( " INS_NOTCH_ATT " , 50 )
self . set_parameter ( " INS_NOTCH_BW " , freq / 2 )
self . reboot_sitl ( )
2019-12-24 17:20:58 -04:00
self . takeoff ( 15 , mode = " ALT_HOLD " )
2019-10-03 13:12:34 -03:00
tstart = self . get_sim_time ( )
self . progress ( " Hovering for %u seconds " % hover_time )
while self . get_sim_time_cached ( ) < tstart + hover_time :
attitude = self . mav . recv_match ( type = ' ATTITUDE ' , blocking = True )
tend = self . get_sim_time ( )
self . do_RTL ( )
psd = self . mavfft_fttd ( 1 , 0 , tstart * 1.0e6 , tend * 1.0e6 )
freq = psd [ " F " ] [ numpy . argmax ( psd [ " X " ] [ ignore_bins : ] ) + ignore_bins ]
2019-12-24 17:20:58 -04:00
if numpy . amax ( psd [ " X " ] [ ignore_bins : ] ) < - 30 :
2019-10-03 13:12:34 -03:00
self . progress ( " Did not detect a motor peak, found %f at %f dB " % ( freq , numpy . amax ( psd [ " X " ] [ ignore_bins : ] ) ) )
else :
raise NotAchievedException ( " Detected motor peak at %f Hz " % ( freq ) )
except Exception as e :
ex = e
self . context_pop ( )
if ex is not None :
raise ex
2018-06-29 07:38:10 -03:00
def fly_vision_position ( self ) :
2018-07-31 06:50:02 -03:00
""" Disable GPS navigation, enable Vicon input. """
2018-06-29 07:38:10 -03:00
# scribble down a location we can set origin to:
self . progress ( " Waiting for location " )
self . mavproxy . send ( ' switch 6 \n ' ) # stabilize mode
2018-12-13 18:17:02 -04:00
self . wait_heartbeat ( )
2018-06-29 07:38:10 -03:00
self . wait_mode ( ' STABILIZE ' )
self . wait_ready_to_arm ( )
old_pos = self . mav . recv_match ( type = ' GLOBAL_POSITION_INT ' , blocking = True )
print ( " old_pos= %s " % str ( old_pos ) )
2018-07-31 06:50:02 -03:00
self . context_push ( )
2018-06-29 07:38:10 -03:00
ex = None
try :
self . set_parameter ( " GPS_TYPE " , 0 )
self . set_parameter ( " EK2_GPS_TYPE " , 3 )
2020-03-30 06:11:01 -03:00
self . set_parameter ( " VISO_TYPE " , 1 )
2018-06-29 07:38:10 -03:00
self . set_parameter ( " SERIAL5_PROTOCOL " , 1 )
self . reboot_sitl ( )
# without a GPS or some sort of external prompting, AP
# doesn't send system_time messages. So prompt it:
2019-03-13 03:12:28 -03:00
self . mav . mav . system_time_send ( int ( time . time ( ) * 1000000 ) , 0 )
2018-06-29 07:38:10 -03:00
self . progress ( " Waiting for non-zero-lat " )
2018-07-12 06:44:03 -03:00
tstart = self . get_sim_time ( )
2018-06-29 07:38:10 -03:00
while True :
2019-06-28 05:51:24 -03:00
self . mav . mav . set_gps_global_origin_send ( 1 ,
old_pos . lat ,
old_pos . lon ,
old_pos . alt )
2018-06-29 07:38:10 -03:00
gpi = self . mav . recv_match ( type = ' GLOBAL_POSITION_INT ' ,
blocking = True )
2019-06-28 05:51:24 -03:00
self . progress ( " gpi= %s " % str ( gpi ) )
2018-06-29 07:38:10 -03:00
if gpi . lat != 0 :
break
2019-06-28 05:51:24 -03:00
if self . get_sim_time_cached ( ) - tstart > 60 :
2018-10-17 23:55:16 -03:00
raise AutoTestTimeoutException ( " Did not get non-zero lat " )
2018-06-29 07:38:10 -03:00
2018-08-23 07:24:34 -03:00
self . takeoff ( )
2018-06-29 07:38:10 -03:00
self . set_rc ( 1 , 1600 )
2018-07-12 06:44:03 -03:00
tstart = self . get_sim_time ( )
2018-06-29 07:38:10 -03:00
while True :
2019-08-04 22:38:38 -03:00
vicon_pos = self . mav . recv_match ( type = ' VISION_POSITION_ESTIMATE ' ,
2018-06-29 07:38:10 -03:00
blocking = True )
2018-07-31 06:50:02 -03:00
# print("vpe=%s" % str(vicon_pos))
self . mav . recv_match ( type = ' GLOBAL_POSITION_INT ' ,
blocking = True )
# self.progress("gpi=%s" % str(gpi))
2018-06-29 07:38:10 -03:00
if vicon_pos . x > 40 :
break
2019-03-07 18:34:09 -04:00
if self . get_sim_time_cached ( ) - tstart > 100 :
2018-10-17 23:55:16 -03:00
raise AutoTestTimeoutException ( " Vicon showed no movement " )
2018-06-29 07:38:10 -03:00
# recenter controls:
self . set_rc ( 1 , 1500 )
self . progress ( " # Enter RTL " )
self . mavproxy . send ( ' switch 3 \n ' )
self . set_rc ( 3 , 1500 )
tstart = self . get_sim_time ( )
while True :
2019-03-07 18:34:09 -04:00
if self . get_sim_time_cached ( ) - tstart > 200 :
2018-10-17 23:55:16 -03:00
raise NotAchievedException ( " Did not disarm " )
2018-07-31 06:50:02 -03:00
self . mav . recv_match ( type = ' GLOBAL_POSITION_INT ' ,
blocking = True )
# print("gpi=%s" % str(gpi))
self . mav . recv_match ( type = ' SIMSTATE ' ,
blocking = True )
# print("ss=%s" % str(ss))
2018-06-29 07:38:10 -03:00
# wait for RTL disarm:
if not self . armed ( ) :
break
except Exception as e :
2019-06-18 00:13:49 -03:00
self . progress ( " Exception caught: %s " % (
self . get_exception_stacktrace ( e ) ) )
2018-06-29 07:38:10 -03:00
ex = e
2018-07-31 06:50:02 -03:00
self . context_pop ( )
2018-10-10 10:07:21 -03:00
self . zero_throttle ( )
2018-06-29 07:38:10 -03:00
self . reboot_sitl ( )
if ex is not None :
raise ex
2019-01-31 01:01:33 -04:00
def fly_rtl_speed ( self ) :
""" Test RTL Speed parameters """
rtl_speed_ms = 7
wpnav_speed_ms = 4
wpnav_accel_mss = 3
tolerance = 0.5
self . load_mission ( " copter_rtl_speed.txt " )
self . set_parameter ( ' WPNAV_ACCEL ' , wpnav_accel_mss * 100 )
self . set_parameter ( ' RTL_SPEED ' , rtl_speed_ms * 100 )
self . set_parameter ( ' WPNAV_SPEED ' , wpnav_speed_ms * 100 )
self . change_mode ( ' LOITER ' )
self . wait_ready_to_arm ( )
self . arm_vehicle ( )
self . change_mode ( ' AUTO ' )
self . set_rc ( 3 , 1600 )
self . wait_altitude ( 19 , 25 , relative = True )
self . wait_groundspeed ( wpnav_speed_ms - tolerance , wpnav_speed_ms + tolerance )
self . monitor_groundspeed ( wpnav_speed_ms , timeout = 20 )
self . change_mode ( ' RTL ' )
self . wait_groundspeed ( rtl_speed_ms - tolerance , rtl_speed_ms + tolerance )
self . monitor_groundspeed ( rtl_speed_ms , timeout = 5 )
self . change_mode ( ' AUTO ' )
self . wait_groundspeed ( 0 - tolerance , 0 + tolerance )
self . wait_groundspeed ( wpnav_speed_ms - tolerance , wpnav_speed_ms + tolerance )
self . monitor_groundspeed ( wpnav_speed_ms , timeout = 5 )
self . change_mode ( ' RTL ' )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2019-01-31 01:01:33 -04:00
2018-08-06 22:25:23 -03:00
def fly_nav_delay ( self ) :
2018-07-31 06:50:02 -03:00
""" Fly a simple mission that has a delay in it. """
2018-08-06 22:25:23 -03:00
2018-07-31 06:50:02 -03:00
self . load_mission ( " copter_nav_delay.txt " )
2018-08-06 22:25:23 -03:00
2019-03-07 18:30:09 -04:00
self . set_parameter ( " DISARM_DELAY " , 0 )
2018-08-06 22:25:23 -03:00
2019-03-07 18:30:09 -04:00
self . change_mode ( " LOITER " )
self . wait_ready_to_arm ( )
2018-08-06 22:25:23 -03:00
2019-03-07 18:30:09 -04:00
self . arm_vehicle ( )
self . change_mode ( " AUTO " )
self . set_rc ( 3 , 1600 )
count_start = - 1
count_stop = - 1
tstart = self . get_sim_time ( )
last_mission_current_msg = 0
last_seq = None
while self . armed ( ) : # we RTL at end of mission
now = self . get_sim_time_cached ( )
if now - tstart > 120 :
raise AutoTestTimeoutException ( " Did not disarm as expected " )
m = self . mav . recv_match ( type = ' MISSION_CURRENT ' , blocking = True )
at_delay_item = " "
if m . seq == 3 :
at_delay_item = " (At delay item) "
if count_start == - 1 :
count_start = now
if ( ( now - last_mission_current_msg ) > 1 or m . seq != last_seq ) :
dist = None
x = self . mav . messages . get ( " NAV_CONTROLLER_OUTPUT " , None )
if x is not None :
dist = x . wp_dist
self . progress ( " MISSION_CURRENT.seq= %u dist= %s %s " %
( m . seq , dist , at_delay_item ) )
last_mission_current_msg = self . get_sim_time_cached ( )
last_seq = m . seq
if m . seq > 3 :
if count_stop == - 1 :
count_stop = now
calculated_delay = count_stop - count_start
want_delay = 59 # should reflect what's in the mission file
self . progress ( " Stopped for %u seconds (want >= %u seconds) " %
( calculated_delay , want_delay ) )
if calculated_delay < want_delay :
raise NotAchievedException ( " Did not delay for long enough " )
2018-08-06 22:25:23 -03:00
2019-01-07 18:51:36 -04:00
def test_rangefinder ( self ) :
ex = None
self . context_push ( )
self . progress ( " Making sure we don ' t ordinarily get RANGEFINDER " )
try :
m = self . mav . recv_match ( type = ' RANGEFINDER ' ,
blocking = True ,
timeout = 5 )
except Exception as e :
2019-08-07 01:58:25 -03:00
self . progress ( " Caught exception: %s " %
self . get_exception_stacktrace ( e ) )
2019-01-07 18:51:36 -04:00
if m is not None :
raise NotAchievedException ( " Received unexpected RANGEFINDER msg " )
2019-04-08 23:26:18 -03:00
# may need to force a rotation if some other test has used the
# rangefinder...
self . progress ( " Ensure no RFND messages in log " )
if self . current_onboard_log_contains_message ( " RFND " ) :
raise NotAchievedException ( " Found unexpected RFND message " )
2019-01-07 18:51:36 -04:00
try :
2019-07-10 10:57:46 -03:00
self . set_analog_rangefinder_parameters ( )
2019-01-07 18:51:36 -04:00
self . set_parameter ( " RC9_OPTION " , 10 ) # rangefinder
self . set_rc ( 9 , 2000 )
self . reboot_sitl ( )
self . progress ( " Making sure we now get RANGEFINDER messages " )
m = self . mav . recv_match ( type = ' RANGEFINDER ' ,
blocking = True ,
timeout = 10 )
if m is None :
raise NotAchievedException ( " Did not get expected RANGEFINDER msg " )
self . progress ( " Checking RangeFinder is marked as enabled in mavlink " )
m = self . mav . recv_match ( type = ' SYS_STATUS ' ,
blocking = True ,
timeout = 10 )
flags = m . onboard_control_sensors_enabled
if not flags & mavutil . mavlink . MAV_SYS_STATUS_SENSOR_LASER_POSITION :
raise NotAchievedException ( " Laser not enabled in SYS_STATUS " )
self . progress ( " Disabling laser using switch " )
self . set_rc ( 9 , 1000 )
self . delay_sim_time ( 1 )
self . progress ( " Checking RangeFinder is marked as disabled in mavlink " )
m = self . mav . recv_match ( type = ' SYS_STATUS ' ,
blocking = True ,
timeout = 10 )
flags = m . onboard_control_sensors_enabled
if flags & mavutil . mavlink . MAV_SYS_STATUS_SENSOR_LASER_POSITION :
raise NotAchievedException ( " Laser enabled in SYS_STATUS " )
2018-06-05 22:31:14 -03:00
self . progress ( " Re-enabling rangefinder " )
self . set_rc ( 9 , 2000 )
self . delay_sim_time ( 1 )
m = self . mav . recv_match ( type = ' SYS_STATUS ' ,
blocking = True ,
timeout = 10 )
flags = m . onboard_control_sensors_enabled
if not flags & mavutil . mavlink . MAV_SYS_STATUS_SENSOR_LASER_POSITION :
raise NotAchievedException ( " Laser not enabled in SYS_STATUS " )
self . takeoff ( 10 , mode = " LOITER " )
m_r = self . mav . recv_match ( type = ' RANGEFINDER ' ,
blocking = True )
m_p = self . mav . recv_match ( type = ' GLOBAL_POSITION_INT ' ,
blocking = True )
if abs ( m_r . distance - m_p . relative_alt / 1000 ) > 1 :
2019-07-10 12:11:46 -03:00
raise NotAchievedException ( " rangefinder/global position int mismatch %0.2f vs %0.2f " , ( m_r . distance , m_p . relative_alt / 1000 ) )
2018-06-05 22:31:14 -03:00
2019-04-08 23:26:18 -03:00
self . land_and_disarm ( )
if not self . current_onboard_log_contains_message ( " RFND " ) :
raise NotAchievedException ( " Did not see expected RFND message " )
2018-06-05 22:31:14 -03:00
2019-01-07 18:51:36 -04:00
except Exception as e :
2019-06-18 00:13:49 -03:00
self . progress ( " Exception caught: %s " % (
self . get_exception_stacktrace ( e ) ) )
ex = e
self . context_pop ( )
self . reboot_sitl ( )
if ex is not None :
raise ex
def test_surface_tracking ( self ) :
ex = None
self . context_push ( )
try :
2019-07-10 10:57:46 -03:00
self . set_analog_rangefinder_parameters ( )
2019-06-18 00:13:49 -03:00
self . set_parameter ( " RC9_OPTION " , 10 ) # rangefinder
self . set_rc ( 9 , 2000 )
self . reboot_sitl ( ) # needed for both rangefinder and initial position
self . assert_vehicle_location_is_at_startup_location ( )
self . takeoff ( 10 , mode = " LOITER " )
lower_surface_pos = mavutil . location ( - 35.362421 , 149.164534 , 584 , 270 )
here = self . mav . location ( )
bearing = self . get_bearing ( here , lower_surface_pos )
self . change_mode ( " GUIDED " )
self . guided_achieve_heading ( bearing )
self . change_mode ( " LOITER " )
self . delay_sim_time ( 2 )
m = self . mav . recv_match ( type = ' GLOBAL_POSITION_INT ' , blocking = True )
orig_absolute_alt_mm = m . alt
self . progress ( " Original alt: absolute= %f " % orig_absolute_alt_mm )
self . progress ( " Flying somewhere which surface is known lower compared to takeoff point " )
self . set_rc ( 2 , 1450 )
tstart = self . get_sim_time ( )
while True :
if self . get_sim_time ( ) - tstart > 200 :
raise NotAchievedException ( " Did not reach lower point " )
m = self . mav . recv_match ( type = ' GLOBAL_POSITION_INT ' , blocking = True )
x = mavutil . location ( m . lat / 1e7 , m . lon / 1e7 , m . alt / 1e3 , 0 )
dist = self . get_distance ( x , lower_surface_pos )
delta = ( orig_absolute_alt_mm - m . alt ) / 1000.0
self . progress ( " Distance: %f m abs-alt-delta: %f m " %
( dist , delta ) )
2019-07-01 05:55:40 -03:00
if dist < 15 :
2019-06-18 00:13:49 -03:00
if delta < 0.8 :
raise NotAchievedException ( " Did not dip in altitude as expected " )
break
self . set_rc ( 2 , 1500 )
self . do_RTL ( )
except Exception as e :
self . progress ( " Exception caught: %s " % (
self . get_exception_stacktrace ( e ) ) )
self . disarm_vehicle ( force = True )
2019-01-07 18:51:36 -04:00
ex = e
self . context_pop ( )
self . reboot_sitl ( )
if ex is not None :
raise ex
2020-05-14 15:40:33 -03:00
def test_rangefinder_switchover ( self ) :
""" test that the EKF correctly handles the switchover between baro and rangefinder """
ex = None
self . context_push ( )
try :
self . set_analog_rangefinder_parameters ( )
self . set_parameter ( " RNGFND1_MAX_CM " , 1500 )
self . set_parameter ( " EK2_RNG_USE_HGT " , 70 )
self . set_parameter ( " EK2_ENABLE " , 1 )
self . set_parameter ( " AHRS_EKF_TYPE " , 2 )
self . reboot_sitl ( ) # needed for both rangefinder and initial position
self . assert_vehicle_location_is_at_startup_location ( )
self . change_mode ( " LOITER " )
self . wait_ready_to_arm ( )
self . arm_vehicle ( )
self . set_rc ( 3 , 1800 )
self . set_rc ( 2 , 1200 )
# wait till we get to 50m
self . wait_altitude ( 50 , 52 , True , 60 )
self . change_mode ( " RTL " )
# wait till we get to 25m
self . wait_altitude ( 25 , 27 , True , 120 )
# level up
self . set_rc ( 2 , 1500 )
self . wait_altitude ( 14 , 15 , relative = True )
tstart = self . get_sim_time ( )
while self . get_sim_time_cached ( ) < tstart + 200 :
m = self . mav . recv_match ( type = ' GLOBAL_POSITION_INT ' , blocking = True )
alt = m . relative_alt / 1000.0 # mm -> m
home_distance = self . distance_to_home ( use_cached_home = True )
home = " "
if alt < = 1 :
home = " HOME "
self . progress ( " Alt: %.02f HomeDist: %.02f %s " %
( alt , home_distance , home ) )
# our post-condition is that we are disarmed:
if not self . armed ( ) :
break
if home == " " :
raise NotAchievedException ( " Did not get home and disarm " )
except Exception as e :
self . progress ( " Exception caught: %s " % (
self . get_exception_stacktrace ( e ) ) )
self . disarm_vehicle ( force = True )
ex = e
self . context_pop ( )
self . reboot_sitl ( )
if ex is not None :
raise ex
2019-01-08 21:40:49 -04:00
def test_parachute ( self ) :
self . set_rc ( 9 , 1000 )
self . set_parameter ( " CHUTE_ENABLED " , 1 )
self . set_parameter ( " CHUTE_TYPE " , 10 )
self . set_parameter ( " SERVO9_FUNCTION " , 27 )
self . set_parameter ( " SIM_PARA_ENABLE " , 1 )
self . set_parameter ( " SIM_PARA_PIN " , 9 )
2019-01-31 19:11:25 -04:00
self . progress ( " Test triggering parachute in mission " )
self . load_mission ( " copter_parachute_mission.txt " )
self . change_mode ( ' LOITER ' )
self . wait_ready_to_arm ( )
self . arm_vehicle ( )
self . change_mode ( ' AUTO ' )
self . set_rc ( 3 , 1600 )
self . mavproxy . expect ( ' BANG ' )
2019-12-23 21:27:45 -04:00
self . disarm_vehicle ( force = True )
2019-01-31 19:11:25 -04:00
self . reboot_sitl ( )
self . progress ( " Test triggering with mavlink message " )
self . takeoff ( 20 )
self . run_cmd ( mavutil . mavlink . MAV_CMD_DO_PARACHUTE ,
2 , # release
0 ,
0 ,
0 ,
0 ,
0 ,
0 )
self . mavproxy . expect ( ' BANG ' )
2019-12-23 21:27:45 -04:00
self . disarm_vehicle ( force = True )
2019-01-31 19:11:25 -04:00
self . reboot_sitl ( )
2019-01-08 21:40:49 -04:00
self . progress ( " Testing three-position switch " )
self . set_parameter ( " RC9_OPTION " , 23 ) # parachute 3pos
self . progress ( " Test manual triggering " )
self . takeoff ( 20 )
self . set_rc ( 9 , 2000 )
self . mavproxy . expect ( ' BANG ' )
self . set_rc ( 9 , 1000 )
2019-12-23 21:27:45 -04:00
self . disarm_vehicle ( force = True )
2019-01-08 21:40:49 -04:00
self . reboot_sitl ( )
self . context_push ( )
self . progress ( " Crashing with 3pos switch in enable position " )
self . takeoff ( 40 )
self . set_rc ( 9 , 1500 )
self . set_parameter ( " SIM_ENGINE_MUL " , 0 )
self . set_parameter ( " SIM_ENGINE_FAIL " , 1 )
self . mavproxy . expect ( ' BANG ' )
self . set_rc ( 9 , 1000 )
2019-12-23 21:27:45 -04:00
self . disarm_vehicle ( force = True )
2019-01-08 21:40:49 -04:00
self . reboot_sitl ( )
2019-07-08 12:59:02 -03:00
self . context_pop ( )
2019-01-08 21:40:49 -04:00
self . progress ( " Crashing with 3pos switch in disable position " )
loiter_alt = 10
self . takeoff ( loiter_alt , mode = ' LOITER ' )
self . set_rc ( 9 , 1100 )
self . set_parameter ( " SIM_ENGINE_MUL " , 0 )
self . set_parameter ( " SIM_ENGINE_FAIL " , 1 )
tstart = self . get_sim_time ( )
while self . get_sim_time_cached ( ) < tstart + 5 :
m = self . mav . recv_match ( type = ' STATUSTEXT ' , blocking = True , timeout = 1 )
if m is None :
continue
if " BANG " in m . text :
self . set_rc ( 9 , 1000 )
self . reboot_sitl ( )
raise NotAchievedException ( " Parachute deployed when disabled " )
self . set_rc ( 9 , 1000 )
2019-12-23 21:27:45 -04:00
self . disarm_vehicle ( force = True )
2019-01-08 21:40:49 -04:00
self . reboot_sitl ( )
2018-07-15 20:54:00 -03:00
def fly_precision_sitl ( self ) :
""" Use SITL PrecLand backend precision messages to land aircraft. """
self . context_push ( )
ex = None
try :
self . set_parameter ( " PLND_ENABLED " , 1 )
self . fetch_parameters ( )
self . set_parameter ( " PLND_TYPE " , 4 )
2019-07-10 10:57:46 -03:00
self . set_analog_rangefinder_parameters ( )
2019-04-04 04:37:07 -03:00
self . set_parameter ( " SIM_SONAR_SCALE " , 12 )
start = self . mav . location ( )
target = start
( target . lat , target . lng ) = mavextra . gps_offset ( start . lat , start . lng , 4 , - 4 )
self . progress ( " Setting target to %f %f " % ( target . lat , target . lng ) )
self . set_parameter ( " SIM_PLD_ENABLE " , 1 )
self . set_parameter ( " SIM_PLD_LAT " , target . lat )
self . set_parameter ( " SIM_PLD_LON " , target . lng )
self . set_parameter ( " SIM_PLD_HEIGHT " , 0 )
self . set_parameter ( " SIM_PLD_ALT_LMT " , 15 )
self . set_parameter ( " SIM_PLD_DIST_LMT " , 10 )
2018-07-15 20:54:00 -03:00
self . reboot_sitl ( )
self . progress ( " Waiting for location " )
2018-10-10 10:07:21 -03:00
self . zero_throttle ( )
2018-07-15 20:54:00 -03:00
self . takeoff ( 10 , 1800 )
2019-03-06 20:23:58 -04:00
self . change_mode ( " LAND " )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2018-07-15 20:54:00 -03:00
self . mav . recv_match ( type = ' GLOBAL_POSITION_INT ' , blocking = True )
new_pos = self . mav . location ( )
2019-04-04 04:37:07 -03:00
delta = self . get_distance ( target , new_pos )
self . progress ( " Landed %f metres from target position " % delta )
2019-03-06 20:23:58 -04:00
max_delta = 1
if delta > max_delta :
2019-04-04 04:37:07 -03:00
raise NotAchievedException ( " Did not land close enough to target position ( %f m > %f m " % ( delta , max_delta ) )
2018-07-15 20:54:00 -03:00
2019-04-02 23:53:56 -03:00
if not self . current_onboard_log_contains_message ( " PL " ) :
raise NotAchievedException ( " Did not see expected PL message " )
2018-07-15 20:54:00 -03:00
except Exception as e :
2019-04-02 23:53:56 -03:00
self . progress ( " Exception caught ( %s ) " % str ( e ) )
2018-07-15 20:54:00 -03:00
ex = e
2018-10-10 10:07:21 -03:00
self . zero_throttle ( )
2018-07-15 20:54:00 -03:00
self . context_pop ( )
self . reboot_sitl ( )
self . progress ( " All done " )
if ex is not None :
raise ex
2018-07-31 06:50:02 -03:00
def get_system_clock_utc ( self , time_seconds ) :
2018-08-07 06:58:40 -03:00
# this is a copy of ArduPilot's AP_RTC function!
# separate time into ms, sec, min, hour and days but all expressed
# in milliseconds
2018-07-31 06:50:02 -03:00
time_ms = time_seconds * 1000
ms = time_ms % 1000
sec_ms = ( time_ms % ( 60 * 1000 ) ) - ms
min_ms = ( time_ms % ( 60 * 60 * 1000 ) ) - sec_ms - ms
hour_ms = ( time_ms % ( 24 * 60 * 60 * 1000 ) ) - min_ms - sec_ms - ms
2018-08-07 06:58:40 -03:00
# convert times as milliseconds into appropriate units
2018-07-31 06:50:02 -03:00
secs = sec_ms / 1000
mins = min_ms / ( 60 * 1000 )
hours = hour_ms / ( 60 * 60 * 1000 )
return ( hours , mins , secs , 0 )
2018-08-07 06:58:40 -03:00
2019-08-15 07:14:52 -03:00
def calc_delay ( self , seconds , delay_for_seconds ) :
2018-08-07 06:58:40 -03:00
# delay-for-seconds has to be long enough that we're at the
# waypoint before that time. Otherwise we'll try to wait a
# day....
2019-08-15 07:14:52 -03:00
if delay_for_seconds > = 3600 :
raise ValueError ( " Won ' t handle large delays " )
2018-08-07 06:58:40 -03:00
( hours ,
mins ,
secs ,
ms ) = self . get_system_clock_utc ( seconds )
self . progress ( " Now is %u h %u m %u s " % ( hours , mins , secs ) )
2019-08-15 07:14:52 -03:00
secs + = delay_for_seconds # add seventeen seconds
mins + = int ( secs / 60 )
secs % = 60
hours + = int ( mins / 60 )
mins % = 60
if hours > 24 :
raise ValueError ( " Way too big a delay " )
self . progress ( " Delay until %u h %u m %u s " %
( hours , mins , secs ) )
2018-08-07 06:58:40 -03:00
return ( hours , mins , secs , 0 )
2019-08-15 07:14:52 -03:00
def reset_delay_item ( self , seq , seconds_in_future ) :
2018-08-07 06:58:40 -03:00
while True :
self . progress ( " Requesting request for seq %u " % ( seq , ) )
self . mav . mav . mission_write_partial_list_send ( 1 , # target system
1 , # target component
seq , # start index
seq )
req = self . mav . recv_match ( type = ' MISSION_REQUEST ' ,
blocking = True ,
timeout = 1 )
if req is not None and req . seq == seq :
if req . get_srcSystem ( ) == 255 :
self . progress ( " Shutup MAVProxy " )
continue
# notionally this might be in the message cache before
# we prompt for it... *shrug*
break
# we have received a request for the item. Supply it:
frame = mavutil . mavlink . MAV_FRAME_GLOBAL_RELATIVE_ALT_INT
command = mavutil . mavlink . MAV_CMD_NAV_DELAY
# retrieve mission item and check it:
tried_set = False
2018-11-22 23:51:58 -04:00
hours = None
mins = None
secs = None
2018-08-07 06:58:40 -03:00
while True :
2018-08-15 20:44:32 -03:00
self . progress ( " Requesting item " )
self . mav . mav . mission_request_send ( 1 ,
1 ,
2018-11-22 23:51:58 -04:00
seq )
2018-08-07 06:58:40 -03:00
st = self . mav . recv_match ( type = ' MISSION_ITEM ' ,
blocking = True ,
timeout = 1 )
2018-08-15 20:44:32 -03:00
if st is None :
continue
2018-08-07 06:58:40 -03:00
2018-08-15 20:44:32 -03:00
print ( " Item: %s " % str ( st ) )
2018-11-22 23:51:58 -04:00
have_match = ( tried_set and
st . seq == seq and
st . command == command and
st . param2 == hours and
st . param3 == mins and
st . param4 == secs )
if have_match :
2018-08-15 20:44:32 -03:00
return
2018-11-22 23:51:58 -04:00
2018-08-15 20:44:32 -03:00
self . progress ( " Mission mismatch " )
m = None
tstart = self . get_sim_time ( )
while True :
if self . get_sim_time_cached ( ) - tstart > 3 :
2018-10-17 23:55:16 -03:00
raise NotAchievedException (
" Did not receive MISSION_REQUEST " )
2018-08-07 06:58:40 -03:00
self . mav . mav . mission_write_partial_list_send ( 1 ,
1 ,
2018-08-15 20:44:32 -03:00
seq ,
seq )
m = self . mav . recv_match ( type = ' MISSION_REQUEST ' ,
blocking = True ,
timeout = 1 )
if m is None :
continue
if m . seq != st . seq :
continue
break
self . progress ( " Sending absolute-time mission item " )
# we have to change out the delay time...
now = self . mav . messages [ " SYSTEM_TIME " ]
if now is None :
2018-10-17 23:55:16 -03:00
raise PreconditionFailedException ( " Never got SYSTEM_TIME " )
2018-08-15 20:44:32 -03:00
if now . time_unix_usec == 0 :
2018-10-17 23:55:16 -03:00
raise PreconditionFailedException ( " system time is zero " )
2019-08-15 07:14:52 -03:00
( hours , mins , secs , ms ) = self . calc_delay ( now . time_unix_usec / 1000000 , seconds_in_future )
2018-08-15 20:44:32 -03:00
self . mav . mav . mission_item_send (
1 , # target system
1 , # target component
seq , # seq
frame , # frame
command , # command
0 , # current
1 , # autocontinue
0 , # p1 (relative seconds)
hours , # p2
mins , # p3
secs , # p4
0 , # p5
0 , # p6
0 ) # p7
tried_set = True
ack = self . mav . recv_match ( type = ' MISSION_ACK ' ,
blocking = True ,
timeout = 1 )
self . progress ( " Received ack: %s " % str ( ack ) )
2018-08-07 06:58:40 -03:00
def fly_nav_delay_abstime ( self ) :
2018-08-08 06:09:48 -03:00
""" fly a simple mission that has a delay in it """
2019-08-15 07:14:52 -03:00
self . fly_nav_delay_abstime_x ( 87 )
def fly_nav_delay_abstime_x ( self , delay_for , expected_delay = None ) :
""" fly a simple mission that has a delay in it, expect a delay """
if expected_delay is None :
expected_delay = delay_for
2018-08-07 06:58:40 -03:00
2018-07-31 06:50:02 -03:00
self . load_mission ( " copter_nav_delay.txt " )
2018-08-07 06:58:40 -03:00
2019-03-07 18:30:09 -04:00
self . change_mode ( " LOITER " )
2018-08-07 06:58:40 -03:00
self . wait_ready_to_arm ( )
2018-08-08 06:09:48 -03:00
delay_item_seq = 3
2019-08-15 07:14:52 -03:00
self . reset_delay_item ( delay_item_seq , delay_for )
delay_for_seconds = delay_for
2018-08-07 06:58:40 -03:00
reset_at_m = self . mav . recv_match ( type = ' SYSTEM_TIME ' , blocking = True )
reset_at = reset_at_m . time_unix_usec / 1000000
2019-03-07 18:30:09 -04:00
self . arm_vehicle ( )
self . change_mode ( " AUTO " )
self . set_rc ( 3 , 1600 )
count_stop = - 1
tstart = self . get_sim_time ( )
while self . armed ( ) : # we RTL at end of mission
2019-03-07 18:34:09 -04:00
now = self . get_sim_time_cached ( )
2019-08-15 07:14:52 -03:00
if now - tstart > 240 :
2019-03-07 18:30:09 -04:00
raise AutoTestTimeoutException ( " Did not disarm as expected " )
m = self . mav . recv_match ( type = ' MISSION_CURRENT ' , blocking = True )
at_delay_item = " "
if m . seq == delay_item_seq :
at_delay_item = " (delay item) "
self . progress ( " MISSION_CURRENT.seq= %u %s " % ( m . seq , at_delay_item ) )
if m . seq > delay_item_seq :
if count_stop == - 1 :
count_stop_m = self . mav . recv_match ( type = ' SYSTEM_TIME ' ,
blocking = True )
count_stop = count_stop_m . time_unix_usec / 1000000
calculated_delay = count_stop - reset_at
2019-08-15 07:14:52 -03:00
error = abs ( calculated_delay - expected_delay )
2019-03-07 18:30:09 -04:00
self . progress ( " Stopped for %u seconds (want >= %u seconds) " %
( calculated_delay , delay_for_seconds ) )
if error > 2 :
raise NotAchievedException ( " delay outside expectations " )
2018-08-07 06:58:40 -03:00
2018-08-08 06:09:48 -03:00
def fly_nav_takeoff_delay_abstime ( self ) :
""" make sure taking off at a specific time works """
2018-12-20 08:05:30 -04:00
self . load_mission ( " copter_nav_delay_takeoff.txt " )
2018-08-08 06:09:48 -03:00
2019-03-07 18:30:09 -04:00
self . change_mode ( " LOITER " )
2018-08-08 06:09:48 -03:00
self . wait_ready_to_arm ( )
delay_item_seq = 2
delay_for_seconds = 77
2019-08-15 07:14:52 -03:00
self . reset_delay_item ( delay_item_seq , delay_for_seconds )
2018-11-22 23:51:58 -04:00
reset_at = self . get_sim_time_cached ( )
2018-08-08 06:09:48 -03:00
2019-03-07 18:30:09 -04:00
self . arm_vehicle ( )
self . change_mode ( " AUTO " )
2018-08-08 06:09:48 -03:00
2019-03-07 18:30:09 -04:00
self . set_rc ( 3 , 1600 )
2018-08-08 06:09:48 -03:00
2019-03-07 18:30:09 -04:00
# should not take off for about least 77 seconds
tstart = self . get_sim_time ( )
took_off = False
while self . armed ( ) :
now = self . get_sim_time_cached ( )
if now - tstart > 200 :
# timeout
break
m = self . mav . recv_match ( type = ' MISSION_CURRENT ' , blocking = True )
now = self . get_sim_time_cached ( )
self . progress ( " %s " % str ( m ) )
if m . seq > delay_item_seq :
if not took_off :
took_off = True
delta_time = now - reset_at
if abs ( delta_time - delay_for_seconds ) > 2 :
raise NotAchievedException ( (
" Did not take off on time "
" measured= %f want= %f " %
( delta_time , delay_for_seconds ) ) )
if not took_off :
raise NotAchievedException ( " Did not take off " )
2018-08-08 06:09:48 -03:00
2018-09-07 01:20:23 -03:00
def fly_zigzag_mode ( self ) :
''' test zigzag mode '''
# set channel 8 for zigzag savewp and recentre it
self . set_parameter ( " RC8_OPTION " , 61 )
self . takeoff ( alt_min = 5 , mode = ' LOITER ' )
ZIGZAG = 24
2020-02-27 05:21:18 -04:00
j = 0
2020-04-01 18:22:14 -03:00
slowdown_speed = 0.3 # because Copter takes a long time to actually stop
2020-02-27 05:21:18 -04:00
self . start_subtest ( " Conduct ZigZag test for all 4 directions " )
while j < 4 :
self . progress ( " ## Align heading with the run-way (j= %d )## " % j )
2018-09-07 01:20:23 -03:00
self . set_rc ( 8 , 1500 )
self . set_rc ( 4 , 1420 )
2020-02-27 05:21:18 -04:00
self . wait_heading ( 352 - j * 90 )
2018-09-07 01:20:23 -03:00
self . set_rc ( 4 , 1500 )
self . change_mode ( ZIGZAG )
2020-02-27 05:21:18 -04:00
self . progress ( " ## Record Point A ## " )
2018-09-07 01:20:23 -03:00
self . set_rc ( 8 , 1100 ) # record point A
self . set_rc ( 1 , 1700 ) # fly side-way for 20m
self . wait_distance ( 20 )
self . set_rc ( 1 , 1500 )
2020-04-01 18:22:14 -03:00
self . wait_groundspeed ( 0 , slowdown_speed ) # wait until the copter slows down
2020-02-27 05:21:18 -04:00
self . progress ( " ## Record Point A ## " )
2018-09-07 01:20:23 -03:00
self . set_rc ( 8 , 1500 ) # pilot always have to cross mid position when changing for low to high position
self . set_rc ( 8 , 1900 ) # record point B
i = 1
2020-02-27 05:21:18 -04:00
while i < 2 :
self . start_subtest ( " Run zigzag A->B and B->A (i= %d ) " % i )
self . progress ( " ## fly forward for 10 meter ## " )
self . set_rc ( 2 , 1300 )
2018-09-07 01:20:23 -03:00
self . wait_distance ( 10 )
self . set_rc ( 2 , 1500 ) # re-centre pitch rc control
2020-04-01 18:22:14 -03:00
self . wait_groundspeed ( 0 , slowdown_speed ) # wait until the copter slows down
2018-09-07 01:20:23 -03:00
self . set_rc ( 8 , 1500 ) # switch to mid position
2020-02-27 05:21:18 -04:00
self . progress ( " ## auto execute vector BA ## " )
self . set_rc ( 8 , 1100 )
2018-09-07 01:20:23 -03:00
self . wait_distance ( 17 ) # wait for it to finish
2020-04-01 18:22:14 -03:00
self . wait_groundspeed ( 0 , slowdown_speed ) # wait until the copter slows down
2018-09-07 01:20:23 -03:00
2020-02-27 05:21:18 -04:00
self . progress ( " ## fly forward for 10 meter ## " )
2018-09-07 01:20:23 -03:00
self . set_rc ( 2 , 1300 ) # fly forward for 10 meter
self . wait_distance ( 10 )
self . set_rc ( 2 , 1500 ) # re-centre pitch rc control
2020-04-01 18:22:14 -03:00
self . wait_groundspeed ( 0 , slowdown_speed ) # wait until the copter slows down
2018-09-07 01:20:23 -03:00
self . set_rc ( 8 , 1500 ) # switch to mid position
2020-02-27 05:21:18 -04:00
self . progress ( " ## auto execute vector AB ## " )
self . set_rc ( 8 , 1900 )
2018-09-07 01:20:23 -03:00
self . wait_distance ( 17 ) # wait for it to finish
2020-04-01 18:22:14 -03:00
self . wait_groundspeed ( 0 , slowdown_speed ) # wait until the copter slows down
2018-09-07 01:20:23 -03:00
i = i + 1
# test the case when pilot switch to manual control during the auto flight
2020-02-27 05:21:18 -04:00
self . start_subtest ( " test the case when pilot switch to manual control during the auto flight " )
self . progress ( " ## fly forward for 10 meter ## " )
2018-09-07 01:20:23 -03:00
self . set_rc ( 2 , 1300 ) # fly forward for 10 meter
self . wait_distance ( 10 )
self . set_rc ( 2 , 1500 ) # re-centre pitch rc control
2020-03-10 09:35:21 -03:00
self . wait_groundspeed ( 0 , 0.3 ) # wait until the copter slows down
2018-09-07 01:20:23 -03:00
self . set_rc ( 8 , 1500 ) # switch to mid position
2020-02-27 05:21:18 -04:00
self . progress ( " ## auto execute vector BA ## " )
2018-09-07 01:20:23 -03:00
self . set_rc ( 8 , 1100 ) # switch to low position, auto execute vector BA
self . wait_distance ( 8 ) # purposely switch to manual halfway
self . set_rc ( 8 , 1500 )
2020-04-01 18:22:14 -03:00
self . wait_groundspeed ( 0 , slowdown_speed ) # copter should slow down here
2020-02-27 05:21:18 -04:00
self . progress ( " ## Manual control to fly forward ## " )
2018-09-07 01:20:23 -03:00
self . set_rc ( 2 , 1300 ) # manual control to fly forward
self . wait_distance ( 8 )
self . set_rc ( 2 , 1500 ) # re-centre pitch rc control
2020-04-01 18:22:14 -03:00
self . wait_groundspeed ( 0 , slowdown_speed ) # wait until the copter slows down
2020-02-27 05:21:18 -04:00
self . progress ( " ## continue vector BA ## " )
2018-09-07 01:20:23 -03:00
self . set_rc ( 8 , 1100 ) # copter should continue mission here
self . wait_distance ( 8 ) # wait for it to finish rest of BA
2020-04-01 18:22:14 -03:00
self . wait_groundspeed ( 0 , slowdown_speed ) #wait until the copter slows down
2018-09-07 01:20:23 -03:00
self . set_rc ( 8 , 1500 ) # switch to mid position
2020-02-27 05:21:18 -04:00
self . progress ( " ## auto execute vector AB ## " )
2018-09-07 01:20:23 -03:00
self . set_rc ( 8 , 1900 ) # switch to execute AB again
self . wait_distance ( 17 ) # wait for it to finish
2020-04-01 18:22:14 -03:00
self . wait_groundspeed ( 0 , slowdown_speed ) # wait until the copter slows down
2018-09-07 01:20:23 -03:00
self . change_mode ( ' LOITER ' )
j = j + 1
self . do_RTL ( )
2018-04-14 08:31:22 -03:00
def test_setting_modes_via_modeswitch ( self ) :
2018-07-31 06:50:02 -03:00
self . context_push ( )
2018-04-14 08:31:22 -03:00
ex = None
try :
fltmode_ch = 5
self . set_parameter ( " FLTMODE_CH " , fltmode_ch )
self . set_rc ( fltmode_ch , 1000 ) # PWM for mode1
testmodes = [ ( " FLTMODE1 " , 4 , " GUIDED " , 1165 ) ,
( " FLTMODE2 " , 13 , " SPORT " , 1295 ) ,
( " FLTMODE3 " , 6 , " RTL " , 1425 ) ,
( " FLTMODE4 " , 7 , " CIRCLE " , 1555 ) ,
( " FLTMODE5 " , 1 , " ACRO " , 1685 ) ,
( " FLTMODE6 " , 17 , " BRAKE " , 1815 ) ,
2018-07-31 06:50:02 -03:00
]
2018-04-14 08:31:22 -03:00
for mode in testmodes :
( parm , parm_value , name , pwm ) = mode
self . set_parameter ( parm , parm_value )
for mode in reversed ( testmodes ) :
( parm , parm_value , name , pwm ) = mode
self . set_rc ( fltmode_ch , pwm )
self . wait_mode ( name )
for mode in testmodes :
( parm , parm_value , name , pwm ) = mode
self . set_rc ( fltmode_ch , pwm )
self . wait_mode ( name )
for mode in reversed ( testmodes ) :
( parm , parm_value , name , pwm ) = mode
self . set_rc ( fltmode_ch , pwm )
self . wait_mode ( name )
self . mavproxy . send ( ' switch 6 \n ' )
self . wait_mode ( " BRAKE " )
self . mavproxy . send ( ' switch 5 \n ' )
self . wait_mode ( " ACRO " )
except Exception as e :
2019-06-18 00:13:49 -03:00
self . progress ( " Exception caught: %s " % (
self . get_exception_stacktrace ( e ) ) )
2018-04-14 08:31:22 -03:00
ex = e
self . context_pop ( )
if ex is not None :
raise ex
def test_setting_modes_via_auxswitch ( self ) :
2018-07-31 06:50:02 -03:00
self . context_push ( )
2018-04-14 08:31:22 -03:00
ex = None
try :
fltmode_ch = int ( self . get_parameter ( " FLTMODE_CH " ) )
self . set_rc ( fltmode_ch , 1000 )
self . wait_mode ( " CIRCLE " )
self . set_rc ( 9 , 1000 )
self . set_rc ( 10 , 1000 )
self . set_parameter ( " RC9_OPTION " , 18 ) # land
self . set_parameter ( " RC10_OPTION " , 55 ) # guided
self . set_rc ( 9 , 1900 )
self . wait_mode ( " LAND " )
self . set_rc ( 10 , 1900 )
self . wait_mode ( " GUIDED " )
self . set_rc ( 10 , 1000 ) # this re-polls the mode switch
self . wait_mode ( " CIRCLE " )
except Exception as e :
2019-06-18 00:13:49 -03:00
self . progress ( " Exception caught: %s " % (
self . get_exception_stacktrace ( e ) ) )
2018-04-14 08:31:22 -03:00
ex = e
2018-07-31 06:50:02 -03:00
self . context_pop ( )
2018-04-14 08:31:22 -03:00
if ex is not None :
raise ex
2019-02-22 01:29:13 -04:00
def fly_guided_stop ( self ,
timeout = 20 ,
groundspeed_tolerance = 0.05 ,
climb_tolerance = 0.001 ) :
""" stop the vehicle moving in guided mode """
self . progress ( " Stopping vehicle " )
tstart = self . get_sim_time ( )
while True :
if self . get_sim_time_cached ( ) - tstart > timeout :
raise NotAchievedException ( " Vehicle did not stop " )
# send a position-control command
self . mav . mav . set_position_target_local_ned_send (
0 , # timestamp
1 , # target system_id
1 , # target component id
mavutil . mavlink . MAV_FRAME_BODY_NED ,
0b1111111111111000 , # mask specifying use-only-x-y-z
0 , # x
0 , # y
0 , # z
0 , # vx
0 , # vy
0 , # vz
0 , # afx
0 , # afy
0 , # afz
0 , # yaw
0 , # yawrate
)
m = self . mav . recv_match ( type = ' VFR_HUD ' , blocking = True )
print ( " %s " % str ( m ) )
if ( m . groundspeed < groundspeed_tolerance and
m . climb < climb_tolerance ) :
break
2019-03-14 22:00:34 -03:00
def fly_guided_move_global_relative_alt ( self , lat , lon , alt ) :
2018-10-12 18:20:13 -03:00
startpos = self . mav . recv_match ( type = ' GLOBAL_POSITION_INT ' ,
blocking = True )
tstart = self . get_sim_time ( )
while True :
2019-03-07 18:34:09 -04:00
if self . get_sim_time_cached ( ) - tstart > 200 :
2018-10-12 18:20:13 -03:00
raise NotAchievedException ( " Did not move far enough " )
# 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
)
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
2019-02-22 01:29:13 -04:00
def fly_guided_move_local ( self , x , y , z_up , timeout = 100 ) :
""" move the vehicle using MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED """
startpos = self . mav . recv_match ( type = ' LOCAL_POSITION_NED ' , blocking = True )
self . progress ( " startpos= %s " % str ( startpos ) )
tstart = self . get_sim_time ( )
while True :
2019-03-07 18:34:09 -04:00
if self . get_sim_time_cached ( ) - tstart > timeout :
2019-02-22 01:29:13 -04:00
raise NotAchievedException ( " Did not start to move " )
# send a position-control command
self . mav . mav . set_position_target_local_ned_send (
0 , # timestamp
1 , # target system_id
1 , # target component id
mavutil . mavlink . MAV_FRAME_LOCAL_NED ,
0b1111111111111000 , # mask specifying use-only-x-y-z
x , # x
y , # y
- z_up , # z
0 , # vx
0 , # vy
0 , # vz
0 , # afx
0 , # afy
0 , # afz
0 , # yaw
0 , # yawrate
)
m = self . mav . recv_match ( type = ' VFR_HUD ' , blocking = True )
print ( " %s " % m )
if m . groundspeed > 0.5 :
break
self . progress ( " Waiting for vehicle to stop... " )
self . wait_groundspeed ( 1 , 100 , timeout = timeout )
stoppos = self . mav . recv_match ( type = ' LOCAL_POSITION_NED ' , blocking = True )
self . progress ( " stop_pos= %s " % str ( stoppos ) )
x_achieved = stoppos . x - startpos . x
if x_achieved - x > 1 :
raise NotAchievedException ( " Did not achieve x position: want= %f got= %f " % ( x , x_achieved ) )
y_achieved = stoppos . y - startpos . y
if y_achieved - y > 1 :
raise NotAchievedException ( " Did not achieve y position: want= %f got= %f " % ( y , y_achieved ) )
z_achieved = stoppos . z - startpos . z
if z_achieved - z_up > 1 :
raise NotAchievedException ( " Did not achieve z position: want= %f got= %f " % ( z_up , z_achieved ) )
2019-03-30 21:57:32 -03:00
def test_guided_local_position_target ( self , x , y , z_up ) :
""" Check target position being received by vehicle """
# set POSITION_TARGET_LOCAL_NED message rate using SET_MESSAGE_INTERVAL
self . progress ( " Setting local target in NED: ( %f , %f , %f ) " % ( x , y , - z_up ) )
self . progress ( " Setting rate to 1 Hz " )
self . set_message_rate_hz ( mavutil . mavlink . MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED , 1 )
# set position target
self . mav . mav . set_position_target_local_ned_send (
0 , # timestamp
1 , # target system_id
1 , # target component id
mavutil . mavlink . MAV_FRAME_LOCAL_NED ,
0b1111111111111000 , # mask specifying use only xyz
x , # x
y , # y
- z_up , # z
0 , # vx
0 , # vy
0 , # vz
0 , # afx
0 , # afy
0 , # afz
0 , # yaw
0 , # yawrate
)
m = self . mav . recv_match ( type = ' POSITION_TARGET_LOCAL_NED ' , blocking = True , timeout = 2 )
self . progress ( " Received local target: %s " % str ( m ) )
if not ( m . type_mask == 0xFFF8 or m . type_mask == 0x0FF8 ) :
raise NotAchievedException ( " Did not receive proper mask: expected=65528 or 4088, got= %u " % m . type_mask )
if x - m . x > 0.1 :
raise NotAchievedException ( " Did not receive proper target position x: wanted= %f got= %f " % ( x , m . x ) )
if y - m . y > 0.1 :
raise NotAchievedException ( " Did not receive proper target position y: wanted= %f got= %f " % ( y , m . y ) )
if z_up - ( - m . z ) > 0.1 :
raise NotAchievedException ( " Did not receive proper target position z: wanted= %f got= %f " % ( z_up , - m . z ) )
def test_guided_local_velocity_target ( self , vx , vy , vz_up , timeout = 3 ) :
" Check local target velocity being recieved by vehicle "
self . progress ( " Setting local NED velocity target: ( %f , %f , %f ) " % ( vx , vy , - vz_up ) )
self . progress ( " Setting POSITION_TARGET_LOCAL_NED message rate to 10Hz " )
self . set_message_rate_hz ( mavutil . mavlink . MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED , 10 )
# Drain old messages and ignore the ramp-up to the required target velocity
tstart = self . get_sim_time ( )
while self . get_sim_time_cached ( ) - tstart < timeout :
# send velocity-control command
self . mav . mav . set_position_target_local_ned_send (
0 , # timestamp
1 , # target system_id
1 , # target component id
mavutil . mavlink . MAV_FRAME_LOCAL_NED ,
0b1111111111000111 , # mask specifying use only vx,vy,vz
0 , # x
0 , # y
0 , # z
vx , # vx
vy , # vy
- vz_up , # vz
0 , # afx
0 , # afy
0 , # afz
0 , # yaw
0 , # yawrate
)
m = self . mav . recv_match ( type = ' POSITION_TARGET_LOCAL_NED ' , blocking = True , timeout = 1 )
if m is None :
raise NotAchievedException ( " Did not receive any message for 1 sec " )
self . progress ( " Received local target: %s " % str ( m ) )
# Check the last received message
if not ( m . type_mask == 0xFFC7 or m . type_mask == 0x0FC7 ) :
raise NotAchievedException ( " Did not receive proper mask: expected=65479 or 4039, got= %u " % m . type_mask )
if vx - m . vx > 0.1 :
raise NotAchievedException ( " Did not receive proper target velocity vx: wanted= %f got= %f " % ( vx , m . vx ) )
if vy - m . vy > 0.1 :
raise NotAchievedException ( " Did not receive proper target velocity vy: wanted= %f got= %f " % ( vy , m . vy ) )
if vz_up - ( - m . vz ) > 0.1 :
raise NotAchievedException ( " Did not receive proper target velocity vz: wanted= %f got= %f " % ( vz_up , - m . vz ) )
self . progress ( " Received proper target velocity commands " )
def test_position_target_message_mode ( self ) :
" Ensure that POSITION_TARGET_LOCAL_NED messages are sent in Guided Mode only "
2019-04-09 02:35:19 -03:00
self . hover ( )
2019-03-30 21:57:32 -03:00
self . change_mode ( ' LOITER ' )
self . progress ( " Setting POSITION_TARGET_LOCAL_NED message rate to 10Hz " )
self . set_message_rate_hz ( mavutil . mavlink . MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED , 10 )
tstart = self . get_sim_time ( )
while self . get_sim_time_cached ( ) < tstart + 5 :
m = self . mav . recv_match ( type = ' POSITION_TARGET_LOCAL_NED ' , blocking = True , timeout = 1 )
if m is None :
continue
raise NotAchievedException ( " Received POSITION_TARGET message in LOITER mode: %s " % str ( m ) )
self . progress ( " Did not receive any POSITION_TARGET_LOCAL_NED message in LOITER mode. Success " )
2018-07-15 20:54:00 -03:00
def earth_to_body ( self , vector ) :
m = self . mav . messages [ " ATTITUDE " ]
x = rotmat . Vector3 ( m . roll , m . pitch , m . yaw )
# print('r=%f p=%f y=%f' % (m.roll, m.pitch, m.yaw))
return vector - x
2020-04-01 05:48:26 -03:00
def loiter_to_ne ( self , x , y , z , timeout = 40 ) :
2018-07-15 20:54:00 -03:00
dest = rotmat . Vector3 ( x , y , z )
2019-03-07 18:34:09 -04:00
tstart = self . get_sim_time ( )
2018-07-15 20:54:00 -03:00
while True :
2019-03-07 18:34:09 -04:00
if self . get_sim_time_cached ( ) - tstart > timeout :
raise NotAchievedException ( " Did not loiter to ne! " )
2018-07-15 20:54:00 -03:00
m_pos = self . mav . recv_match ( type = ' LOCAL_POSITION_NED ' ,
blocking = True )
pos = rotmat . Vector3 ( m_pos . x , m_pos . y , m_pos . z )
delta_ef = pos - dest
dist = math . sqrt ( delta_ef . x * delta_ef . x + delta_ef . y * delta_ef . y )
2019-03-12 23:53:33 -03:00
dist_max = 2
self . progress ( " dist= %f want < %f " % ( dist , dist_max ) )
if dist < dist_max :
2018-07-15 20:54:00 -03:00
break
delta_bf = self . earth_to_body ( delta_ef )
angle_x = math . atan2 ( delta_bf . x , delta_bf . z )
angle_y = math . atan2 ( delta_bf . y , delta_bf . z )
distance = math . sqrt ( delta_bf . x * delta_bf . x +
delta_bf . y * delta_bf . y +
delta_bf . z * delta_bf . z )
self . mav . mav . landing_target_send (
0 , # time_usec
1 , # target_num
mavutil . mavlink . MAV_FRAME_GLOBAL , # frame; AP ignores
angle_x , # angle x (radians)
angle_y , # angle y (radians)
distance , # distance to target
0.01 , # size of target in radians, X-axis
0.01 # size of target in radians, Y-axis
)
tstart = self . get_sim_time ( )
2019-03-07 18:34:09 -04:00
while self . get_sim_time_cached ( ) - tstart < 10 :
2018-07-15 20:54:00 -03:00
m_pos = self . mav . recv_match ( type = ' LOCAL_POSITION_NED ' ,
blocking = True )
pos = rotmat . Vector3 ( m_pos . x , m_pos . y , m_pos . z )
delta_ef = pos - dest
dist = math . sqrt ( delta_ef . x * delta_ef . x + delta_ef . y * delta_ef . y )
self . progress ( " dist= %f " % ( dist , ) )
def fly_payload_place_mission ( self ) :
""" Test payload placing in auto. """
self . context_push ( )
ex = None
try :
2019-07-10 10:57:46 -03:00
self . set_analog_rangefinder_parameters ( )
2018-07-15 20:54:00 -03:00
self . set_parameter ( " GRIP_ENABLE " , 1 )
self . set_parameter ( " GRIP_TYPE " , 1 )
self . set_parameter ( " SIM_GRPS_ENABLE " , 1 )
self . set_parameter ( " SIM_GRPS_PIN " , 8 )
self . set_parameter ( " SERVO8_FUNCTION " , 28 )
self . set_parameter ( " RC9_OPTION " , 19 )
self . reboot_sitl ( )
self . set_rc ( 9 , 2000 )
# load the mission:
2018-12-20 08:05:30 -04:00
self . load_mission ( " copter_payload_place.txt " )
2018-07-15 20:54:00 -03:00
self . progress ( " Waiting for location " )
self . mav . location ( )
2018-10-10 10:07:21 -03:00
self . zero_throttle ( )
2018-07-15 20:54:00 -03:00
self . mavproxy . send ( ' switch 6 \n ' ) # stabilize mode
2018-12-13 18:17:02 -04:00
self . wait_heartbeat ( )
2018-07-15 20:54:00 -03:00
self . wait_mode ( ' STABILIZE ' )
self . wait_ready_to_arm ( )
self . arm_vehicle ( )
self . mavproxy . send ( ' switch 4 \n ' ) # auto mode
2018-12-13 18:17:02 -04:00
self . wait_heartbeat ( )
2018-07-15 20:54:00 -03:00
self . wait_mode ( ' AUTO ' )
self . set_rc ( 3 , 1500 )
self . wait_text ( " Gripper load releas " , timeout = 90 )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2018-07-15 20:54:00 -03:00
except Exception as e :
2019-06-18 00:13:49 -03:00
self . progress ( " Exception caught: %s " % (
self . get_exception_stacktrace ( e ) ) )
2018-07-15 20:54:00 -03:00
ex = e
self . context_pop ( )
self . reboot_sitl ( )
self . progress ( " All done " )
if ex is not None :
raise ex
2018-07-30 08:19:14 -03:00
def fly_guided_change_submode ( self ) :
2018-07-31 06:50:02 -03:00
""" " Ensure we can move around in guided after a takeoff command. """
2018-07-30 08:19:14 -03:00
2019-03-10 23:58:26 -03:00
''' start by disabling GCS failsafe, otherwise we immediately disarm
due to ( apparently ) not receiving traffic from the GCS for
too long . This is probably a function of - - speedup '''
self . set_parameter ( " FS_GCS_ENABLE " , 0 )
self . set_parameter ( " DISARM_DELAY " , 0 ) # until traffic problems are fixed
self . change_mode ( " GUIDED " )
self . wait_ready_to_arm ( )
self . arm_vehicle ( )
2018-07-30 08:19:14 -03:00
2019-03-10 23:58:26 -03:00
self . user_takeoff ( alt_min = 10 )
2019-02-22 01:29:13 -04:00
2019-07-10 05:55:49 -03:00
self . start_subtest ( " yaw through absolute angles using MAV_CMD_CONDITION_YAW " )
2019-03-10 23:58:26 -03:00
self . guided_achieve_heading ( 45 )
self . guided_achieve_heading ( 135 )
2018-07-30 08:19:14 -03:00
2019-07-10 05:55:49 -03:00
self . start_subtest ( " move the vehicle using set_position_target_global_int " )
2019-03-14 22:00:34 -03:00
self . fly_guided_move_global_relative_alt ( 5 , 5 , 10 )
2018-07-30 08:19:14 -03:00
2019-07-10 05:55:49 -03:00
self . start_subtest ( " move the vehicle using MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED " )
2019-03-10 23:58:26 -03:00
self . fly_guided_stop ( )
self . fly_guided_move_local ( 5 , 5 , 10 )
2018-07-30 08:19:14 -03:00
2019-07-10 05:55:49 -03:00
self . start_subtest ( " Check target position received by vehicle using SET_MESSAGE_INTERVAL " )
2019-03-30 21:57:32 -03:00
self . test_guided_local_position_target ( 5 , 5 , 10 )
self . test_guided_local_velocity_target ( 2 , 2 , 1 )
self . test_position_target_message_mode ( )
2019-03-10 23:58:26 -03:00
self . do_RTL ( )
2018-07-30 08:19:14 -03:00
2018-10-23 01:56:36 -03:00
def test_gripper_mission ( self ) :
self . context_push ( )
ex = None
try :
self . load_mission ( " copter-gripper-mission.txt " )
self . mavproxy . send ( ' mode loiter \n ' )
self . wait_ready_to_arm ( )
2019-06-18 00:13:49 -03:00
self . assert_vehicle_location_is_at_startup_location ( )
2018-10-23 01:56:36 -03:00
self . arm_vehicle ( )
self . mavproxy . send ( ' mode auto \n ' )
self . wait_mode ( ' AUTO ' )
self . set_rc ( 3 , 1500 )
self . mavproxy . expect ( " Gripper Grabbed " )
self . mavproxy . expect ( " Gripper Released " )
except Exception as e :
2019-06-18 00:13:49 -03:00
self . progress ( " Exception caught: %s " % (
self . get_exception_stacktrace ( e ) ) )
2018-10-23 01:56:36 -03:00
self . mavproxy . send ( ' mode land \n ' )
ex = e
self . context_pop ( )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2018-10-23 01:56:36 -03:00
if ex is not None :
raise ex
2019-09-10 01:52:56 -03:00
def test_spline_last_waypoint ( self ) :
self . context_push ( )
ex = None
try :
self . load_mission ( " copter-spline-last-waypoint.txt " )
self . mavproxy . send ( ' mode loiter \n ' )
self . wait_ready_to_arm ( )
self . arm_vehicle ( )
self . mavproxy . send ( ' mode auto \n ' )
self . wait_mode ( ' AUTO ' )
self . set_rc ( 3 , 1500 )
self . wait_altitude ( 10 , 3000 , relative = True )
except Exception as e :
self . progress ( " Exception caught: %s " % (
self . get_exception_stacktrace ( e ) ) )
ex = e
self . context_pop ( )
self . do_RTL ( )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2019-09-10 01:52:56 -03:00
if ex is not None :
raise ex
2019-03-06 02:37:26 -04:00
def fly_manual_throttle_mode_change ( self ) :
self . set_parameter ( " FS_GCS_ENABLE " , 0 ) # avoid GUIDED instant disarm
self . change_mode ( " STABILIZE " )
self . wait_ready_to_arm ( )
self . arm_vehicle ( )
self . change_mode ( " ACRO " )
self . change_mode ( " STABILIZE " )
self . change_mode ( " GUIDED " )
self . set_rc ( 3 , 1700 )
self . watch_altitude_maintained ( - 1 , 0.2 ) # should not take off in guided
self . run_cmd_do_set_mode (
" ACRO " ,
want_result = mavutil . mavlink . MAV_RESULT_UNSUPPORTED ) # should fix this result code!
2019-03-07 05:08:28 -04:00
self . run_cmd_do_set_mode (
" STABILIZE " ,
want_result = mavutil . mavlink . MAV_RESULT_UNSUPPORTED ) # should fix this result code!
self . run_cmd_do_set_mode (
" DRIFT " ,
want_result = mavutil . mavlink . MAV_RESULT_UNSUPPORTED ) # should fix this result code!
2019-03-06 02:37:26 -04:00
self . set_rc ( 3 , 1000 )
self . run_cmd_do_set_mode ( " ACRO " )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2019-03-06 02:37:26 -04:00
2019-03-16 22:19:32 -03:00
def test_mount_pitch ( self , despitch , despitch_tolerance , timeout = 10 , hold = 0 ) :
2018-10-12 18:20:13 -03:00
tstart = self . get_sim_time ( )
2019-03-16 22:19:32 -03:00
success_start = 0
2018-10-12 18:20:13 -03:00
while True :
2019-03-16 22:19:32 -03:00
now = self . get_sim_time_cached ( )
if now - tstart > timeout :
2019-03-05 21:02:15 -04:00
raise NotAchievedException ( " Mount pitch not achieved " )
2018-10-12 18:20:13 -03:00
m = self . mav . recv_match ( type = ' MOUNT_STATUS ' ,
blocking = True ,
timeout = 5 )
# self.progress("pitch=%f roll=%f yaw=%f" %
# (m.pointing_a, m.pointing_b, m.pointing_c))
mount_pitch = m . pointing_a / 100.0 # centidegrees to degrees
if abs ( despitch - mount_pitch ) > despitch_tolerance :
2020-01-22 21:43:06 -04:00
self . progress ( " Mount pitch incorrect: got= %f want= %f (+/- %f ) " %
( mount_pitch , despitch , despitch_tolerance ) )
2019-03-16 22:19:32 -03:00
success_start = 0
2018-10-12 18:20:13 -03:00
continue
self . progress ( " Mount pitch correct: %f degrees == %f " %
( mount_pitch , despitch ) )
2019-03-16 22:19:32 -03:00
if success_start == 0 :
success_start = now
continue
if now - success_start > hold :
self . progress ( " Mount pitch achieved " )
return
2018-10-12 18:20:13 -03:00
def do_pitch ( self , pitch ) :
''' pitch aircraft in guided/angle mode '''
self . mav . mav . set_attitude_target_send (
0 , # time_boot_ms
1 , # target sysid
1 , # target compid
0 , # bitmask of things to ignore
mavextra . euler_to_quat ( [ 0 , math . radians ( pitch ) , 0 ] ) , # att
0 , # roll rate (rad/s)
1 , # pitch rate
0 , # yaw rate
0.5 ) # thrust, 0 to 1, translated to a climb/descent rate
def test_mount ( self ) :
ex = None
self . context_push ( )
2019-03-16 22:19:32 -03:00
old_srcSystem = self . mav . mav . srcSystem
self . mav . mav . srcSystem = 250
self . set_parameter ( " DISARM_DELAY " , 0 )
2018-10-12 18:20:13 -03:00
try :
''' start by disabling GCS failsafe, otherwise we immediately disarm
due to ( apparently ) not receiving traffic from the GCS for
too long . This is probably a function of - - speedup '''
self . set_parameter ( " FS_GCS_ENABLE " , 0 )
self . progress ( " Setting up servo mount " )
roll_servo = 5
pitch_servo = 6
yaw_servo = 7
self . set_parameter ( " MNT_TYPE " , 1 )
self . set_parameter ( " SERVO %u _FUNCTION " % roll_servo , 8 ) # roll
self . set_parameter ( " SERVO %u _FUNCTION " % pitch_servo , 7 ) # pitch
self . set_parameter ( " SERVO %u _FUNCTION " % yaw_servo , 6 ) # yaw
self . reboot_sitl ( ) # to handle MNT_TYPE changing
# make sure we're getting mount status and gimbal reports
self . mav . recv_match ( type = ' MOUNT_STATUS ' ,
blocking = True ,
timeout = 5 )
self . mav . recv_match ( type = ' GIMBAL_REPORT ' ,
blocking = True ,
timeout = 5 )
# test pitch isn't stabilising:
m = self . mav . recv_match ( type = ' MOUNT_STATUS ' ,
blocking = True ,
timeout = 5 )
if m . pointing_a != 0 or m . pointing_b != 0 or m . pointing_c != 0 :
2019-03-05 21:02:15 -04:00
raise NotAchievedException ( " Mount stabilising when not requested " )
2018-10-12 18:20:13 -03:00
self . mavproxy . send ( ' mode guided \n ' )
self . wait_mode ( ' GUIDED ' )
self . wait_ready_to_arm ( )
self . arm_vehicle ( )
self . user_takeoff ( )
despitch = 10
despitch_tolerance = 3
self . progress ( " Pitching vehicle " )
self . do_pitch ( despitch ) # will time out!
self . wait_pitch ( despitch , despitch_tolerance )
# check we haven't modified:
m = self . mav . recv_match ( type = ' MOUNT_STATUS ' ,
blocking = True ,
timeout = 5 )
if m . pointing_a != 0 or m . pointing_b != 0 or m . pointing_c != 0 :
2019-03-05 21:02:15 -04:00
raise NotAchievedException ( " Mount stabilising when not requested " )
2018-10-12 18:20:13 -03:00
self . progress ( " Enable pitch stabilization using MOUNT_CONFIGURE " )
self . mav . mav . mount_configure_send (
1 , # target system
1 , # target component
mavutil . mavlink . MAV_MOUNT_MODE_RC_TARGETING ,
0 , # stab-roll
1 , # stab-pitch
0 )
2019-03-10 21:59:12 -03:00
self . do_pitch ( despitch )
2018-10-12 18:20:13 -03:00
self . test_mount_pitch ( - despitch , 1 )
self . progress ( " Disable pitch using MAV_CMD_DO_MOUNT_CONFIGURE " )
self . do_pitch ( despitch )
self . run_cmd ( mavutil . mavlink . MAV_CMD_DO_MOUNT_CONFIGURE ,
mavutil . mavlink . MAV_MOUNT_MODE_RC_TARGETING ,
0 ,
0 ,
0 ,
0 ,
0 ,
0 ,
)
self . test_mount_pitch ( 0 , 0 )
self . progress ( " Point somewhere using MOUNT_CONTROL (ANGLE) " )
self . do_pitch ( despitch )
self . run_cmd ( mavutil . mavlink . MAV_CMD_DO_MOUNT_CONFIGURE ,
mavutil . mavlink . MAV_MOUNT_MODE_MAVLINK_TARGETING ,
0 ,
0 ,
0 ,
0 ,
0 ,
0 ,
)
self . mav . mav . mount_control_send (
1 , # target system
1 , # target component
2018-11-22 23:51:58 -04:00
20 * 100 , # pitch
20 * 100 , # roll (centidegrees)
2018-10-12 18:20:13 -03:00
0 , # yaw
0 # save position
)
self . test_mount_pitch ( 20 , 1 )
self . progress ( " Point somewhere using MOUNT_CONTROL (GPS) " )
self . do_pitch ( despitch )
self . run_cmd ( mavutil . mavlink . MAV_CMD_DO_MOUNT_CONFIGURE ,
mavutil . mavlink . MAV_MOUNT_MODE_GPS_POINT ,
0 ,
0 ,
0 ,
0 ,
0 ,
0 ,
)
start = self . mav . location ( )
self . progress ( " start= %s " % str ( start ) )
( t_lat , t_lon ) = mavextra . gps_offset ( start . lat , start . lng , 10 , 20 )
t_alt = 0
self . progress ( " loc %f %f %f " % ( start . lat , start . lng , start . alt ) )
self . progress ( " targetting %f %f %f " % ( t_lat , t_lon , t_alt ) )
self . do_pitch ( despitch )
self . mav . mav . mount_control_send (
1 , # target system
1 , # target component
2019-03-13 03:12:28 -03:00
int ( t_lat * 1e7 ) , # lat
int ( t_lon * 1e7 ) , # lon
2018-10-12 18:20:13 -03:00
t_alt * 100 , # alt
0 # save position
)
self . test_mount_pitch ( - 52 , 5 )
# now test RC targetting
self . progress ( " Testing mount RC targetting " )
# this is a one-off; ArduCopter *will* time out this directive!
self . progress ( " Levelling aircraft " )
self . mav . mav . set_attitude_target_send (
0 , # time_boot_ms
1 , # target sysid
1 , # target compid
0 , # bitmask of things to ignore
mavextra . euler_to_quat ( [ 0 , 0 , 0 ] ) , # att
1 , # roll rate (rad/s)
1 , # pitch rate
1 , # yaw rate
0.5 ) # thrust, 0 to 1, translated to a climb/descent rate
self . run_cmd ( mavutil . mavlink . MAV_CMD_DO_MOUNT_CONFIGURE ,
mavutil . mavlink . MAV_MOUNT_MODE_RC_TARGETING ,
0 ,
0 ,
0 ,
0 ,
0 ,
0 ,
)
try :
self . context_push ( )
self . set_parameter ( ' MNT_RC_IN_ROLL ' , 11 )
self . set_parameter ( ' MNT_RC_IN_TILT ' , 12 )
self . set_parameter ( ' MNT_RC_IN_PAN ' , 13 )
self . progress ( " Testing RC angular control " )
2020-01-29 22:49:48 -04:00
# default RC min=1100 max=1900
2018-10-12 18:20:13 -03:00
self . set_rc ( 11 , 1500 )
self . set_rc ( 12 , 1500 )
self . set_rc ( 13 , 1500 )
self . test_mount_pitch ( 0 , 1 )
2020-01-29 22:49:48 -04:00
self . progress ( " Testing RC input down 1/4 of its range in the output, should be down 1/4 range in output " )
rc12_in = 1400.0
rc12_min = 1100.0 # default
rc12_max = 1900.0 # default
angmin_tilt = - 45.0 # default
angmax_tilt = 45.0 # default
expected_pitch = ( ( rc12_in - rc12_min ) / ( rc12_max - rc12_min ) * ( angmax_tilt - angmin_tilt ) ) + angmin_tilt
self . progress ( " expected mount pitch: %f " % expected_pitch )
if expected_pitch != - 11.25 :
raise NotAchievedException ( " Calculation wrong - defaults changed?! " )
self . set_rc ( 12 , rc12_in )
2018-10-12 18:20:13 -03:00
self . test_mount_pitch ( - 11.25 , 0.01 )
self . set_rc ( 12 , 1800 )
self . test_mount_pitch ( 33.75 , 0.01 )
self . set_rc ( 11 , 1500 )
self . set_rc ( 12 , 1500 )
self . set_rc ( 13 , 1500 )
2020-01-29 22:52:23 -04:00
try :
self . progress ( " Issue https://discuss.ardupilot.org/t/gimbal-limits-with-storm32-backend-mavlink-not-applied-correctly/51438 " )
self . context_push ( )
self . set_parameter ( " RC12_MIN " , 1000 )
self . set_parameter ( " RC12_MAX " , 2000 )
self . set_parameter ( " MNT_ANGMIN_TIL " , - 9000 )
self . set_parameter ( " MNT_ANGMAX_TIL " , 1000 )
self . set_rc ( 12 , 1000 )
self . test_mount_pitch ( - 90.00 , 0.01 )
self . set_rc ( 12 , 2000 )
self . test_mount_pitch ( 10.00 , 0.01 )
self . set_rc ( 12 , 1500 )
self . test_mount_pitch ( - 40.00 , 0.01 )
finally :
self . context_pop ( )
self . set_rc ( 12 , 1500 )
2018-10-12 18:20:13 -03:00
self . progress ( " Testing RC rate control " )
self . set_parameter ( ' MNT_JSTICK_SPD ' , 10 )
self . test_mount_pitch ( 0 , 1 )
self . set_rc ( 12 , 1300 )
self . test_mount_pitch ( - 5 , 1 )
self . test_mount_pitch ( - 10 , 1 )
self . test_mount_pitch ( - 15 , 1 )
self . test_mount_pitch ( - 20 , 1 )
self . set_rc ( 12 , 1700 )
self . test_mount_pitch ( - 15 , 1 )
self . test_mount_pitch ( - 10 , 1 )
self . test_mount_pitch ( - 5 , 1 )
self . test_mount_pitch ( 0 , 1 )
self . test_mount_pitch ( 5 , 1 )
self . progress ( " Reverting to angle mode " )
self . set_parameter ( ' MNT_JSTICK_SPD ' , 0 )
self . set_rc ( 12 , 1500 )
self . test_mount_pitch ( 0 , 0.1 )
self . context_pop ( )
2019-03-16 22:19:32 -03:00
except Exception as e :
self . progress ( " Exception caught: %s " % (
self . get_exception_stacktrace ( e ) ) )
2018-10-12 18:20:13 -03:00
self . context_pop ( )
2019-03-16 22:19:32 -03:00
raise e
2018-10-12 18:20:13 -03:00
self . progress ( " Testing mount ROI behaviour " )
2019-11-03 23:49:07 -04:00
self . drain_mav_unparsed ( )
2018-10-12 18:20:13 -03:00
self . test_mount_pitch ( 0 , 0.1 )
start = self . mav . location ( )
self . progress ( " start= %s " % str ( start ) )
( roi_lat , roi_lon ) = mavextra . gps_offset ( start . lat ,
start . lng ,
10 ,
20 )
roi_alt = 0
self . progress ( " Using MAV_CMD_DO_SET_ROI_LOCATION " )
self . run_cmd ( mavutil . mavlink . MAV_CMD_DO_SET_ROI_LOCATION ,
0 ,
0 ,
0 ,
0 ,
roi_lat ,
roi_lon ,
roi_alt ,
)
self . test_mount_pitch ( - 52 , 5 )
start = self . mav . location ( )
( roi_lat , roi_lon ) = mavextra . gps_offset ( start . lat ,
start . lng ,
- 100 ,
- 200 )
roi_alt = 0
self . progress ( " Using MAV_CMD_DO_SET_ROI " )
self . run_cmd ( mavutil . mavlink . MAV_CMD_DO_SET_ROI ,
0 ,
0 ,
0 ,
0 ,
roi_lat ,
roi_lon ,
roi_alt ,
)
self . test_mount_pitch ( - 7.5 , 1 )
2019-11-03 23:49:07 -04:00
start = self . mav . location ( )
( roi_lat , roi_lon ) = mavextra . gps_offset ( start . lat ,
start . lng ,
- 100 ,
- 200 )
roi_alt = 0
self . progress ( " Using MAV_CMD_DO_SET_ROI (COMMAND_INT) " )
self . run_cmd_int ( mavutil . mavlink . MAV_CMD_DO_SET_ROI ,
0 ,
0 ,
0 ,
0 ,
2020-01-17 18:10:50 -04:00
int ( roi_lat * 1e7 ) ,
int ( roi_lon * 1e7 ) ,
2019-11-03 23:49:07 -04:00
roi_alt ,
frame = mavutil . mavlink . MAV_FRAME_GLOBAL_RELATIVE_ALT_INT ,
)
self . test_mount_pitch ( - 7.5 , 1 )
self . progress ( " Using MAV_CMD_DO_SET_ROI (COMMAND_INT), absolute-alt-frame " )
# this is pointing essentially straight down
self . run_cmd_int ( mavutil . mavlink . MAV_CMD_DO_SET_ROI ,
0 ,
0 ,
0 ,
0 ,
2020-01-17 18:10:50 -04:00
int ( roi_lat * 1e7 ) ,
int ( roi_lon * 1e7 ) ,
2019-11-03 23:49:07 -04:00
roi_alt ,
frame = mavutil . mavlink . MAV_FRAME_GLOBAL ,
)
2019-03-16 22:19:32 -03:00
self . test_mount_pitch ( - 70 , 1 , hold = 2 )
self . run_cmd ( mavutil . mavlink . MAV_CMD_DO_MOUNT_CONFIGURE ,
mavutil . mavlink . MAV_MOUNT_MODE_NEUTRAL ,
0 ,
0 ,
0 ,
0 ,
0 ,
0 ,
)
self . test_mount_pitch ( 0 , 0.1 )
self . progress ( " Testing mount roi-sysid behaviour " )
self . test_mount_pitch ( 0 , 0.1 )
start = self . mav . location ( )
self . progress ( " start= %s " % str ( start ) )
( roi_lat , roi_lon ) = mavextra . gps_offset ( start . lat ,
start . lng ,
10 ,
20 )
roi_alt = 0
self . progress ( " Using MAV_CMD_DO_SET_ROI_SYSID " )
self . run_cmd ( mavutil . mavlink . MAV_CMD_DO_SET_ROI_SYSID ,
250 ,
0 ,
0 ,
0 ,
0 ,
0 ,
0 ,
)
self . mav . mav . global_position_int_send (
0 , # time boot ms
2020-01-17 18:10:50 -04:00
int ( roi_lat * 1e7 ) ,
int ( roi_lon * 1e7 ) ,
2019-03-16 22:19:32 -03:00
0 * 1000 , # mm alt amsl
0 * 1000 , # relalt mm UP!
0 , # vx
0 , # vy
0 , # vz
0 # heading
) ;
self . test_mount_pitch ( - 89 , 5 , hold = 2 )
self . mav . mav . global_position_int_send (
0 , # time boot ms
2020-01-17 18:10:50 -04:00
int ( roi_lat * 1e7 ) ,
int ( roi_lon * 1e7 ) ,
2019-03-16 22:19:32 -03:00
670 * 1000 , # mm alt amsl
100 * 1000 , # mm UP!
0 , # vx
0 , # vy
0 , # vz
0 # heading
) ;
self . test_mount_pitch ( 68 , 5 , hold = 2 )
self . run_cmd ( mavutil . mavlink . MAV_CMD_DO_MOUNT_CONFIGURE ,
mavutil . mavlink . MAV_MOUNT_MODE_NEUTRAL ,
0 ,
0 ,
0 ,
0 ,
0 ,
0 ,
)
self . test_mount_pitch ( 0 , 0.1 )
2019-11-03 23:49:07 -04:00
2018-10-12 18:20:13 -03:00
self . progress ( " checking ArduCopter yaw-aircraft-for-roi " )
try :
self . context_push ( )
m = self . mav . recv_match ( type = ' VFR_HUD ' , blocking = True )
self . progress ( " current heading %u " % m . heading )
self . set_parameter ( " SERVO %u _FUNCTION " % yaw_servo , 0 ) # yaw
self . progress ( " Waiting for check_servo_map to do its job " )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 5 )
2018-10-12 18:20:13 -03:00
start = self . mav . location ( )
self . progress ( " Moving to guided/position controller " )
2019-03-14 22:00:34 -03:00
self . fly_guided_move_global_relative_alt ( 1 , 0 , 0 )
2018-10-12 18:20:13 -03:00
self . guided_achieve_heading ( 0 )
( roi_lat , roi_lon ) = mavextra . gps_offset ( start . lat ,
start . lng ,
- 100 ,
- 200 )
roi_alt = 0
self . progress ( " Using MAV_CMD_DO_SET_ROI " )
self . run_cmd ( mavutil . mavlink . MAV_CMD_DO_SET_ROI ,
0 ,
0 ,
0 ,
0 ,
roi_lat ,
roi_lon ,
roi_alt ,
)
self . wait_heading ( 110 , timeout = 600 )
self . context_pop ( )
except Exception :
self . context_pop ( )
raise
except Exception as e :
ex = e
self . context_pop ( )
2019-03-16 22:19:32 -03:00
self . mav . mav . srcSystem = old_srcSystem
2019-02-26 00:29:12 -04:00
self . disarm_vehicle ( force = True )
2018-10-12 18:20:13 -03:00
self . reboot_sitl ( ) # to handle MNT_TYPE changing
2018-11-22 23:51:58 -04:00
if ex is not None :
raise ex
2018-03-22 22:31:09 -03:00
def fly_throw_mode ( self ) :
# test boomerang mode:
self . progress ( " Throwing vehicle away " )
self . set_parameter ( " THROW_NEXTMODE " , 6 )
self . set_parameter ( " SIM_SHOVE_Z " , - 30 )
self . set_parameter ( " SIM_SHOVE_X " , - 20 )
self . change_mode ( ' THROW ' )
self . wait_ready_to_arm ( )
self . arm_vehicle ( )
2019-03-11 22:29:42 -03:00
try :
2020-03-12 20:51:17 -03:00
self . set_parameter ( " SIM_SHOVE_TIME " , 500 , retries = 3 )
2019-03-11 22:29:42 -03:00
except ValueError as e :
# the shove resets this to zero
pass
2018-03-22 22:31:09 -03:00
tstart = self . get_sim_time ( )
self . wait_mode ( ' RTL ' )
2019-03-11 22:29:42 -03:00
max_good_tdelta = 15
2018-03-22 22:31:09 -03:00
tdelta = self . get_sim_time ( ) - tstart
self . progress ( " Vehicle in RTL " )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2018-03-22 22:31:09 -03:00
self . progress ( " Vehicle disarmed " )
if tdelta > max_good_tdelta :
2019-03-11 22:29:42 -03:00
raise NotAchievedException ( " Took too long to enter RTL: %f s > %f s " %
( tdelta , max_good_tdelta ) )
2018-03-22 22:31:09 -03:00
self . progress ( " Vehicle returned " )
2019-12-27 08:56:03 -04:00
def hover_and_check_matched_frequency_with_fft ( self , dblevel = - 15 , minhz = 200 , maxhz = 300 , peakhz = None , reverse = None ) :
# find a motor peak
self . takeoff ( 10 , mode = " ALT_HOLD " )
hover_time = 15
tstart = self . get_sim_time ( )
self . progress ( " Hovering for %u seconds " % hover_time )
while self . get_sim_time_cached ( ) < tstart + hover_time :
attitude = self . mav . recv_match ( type = ' ATTITUDE ' , blocking = True )
vfr_hud = self . mav . recv_match ( type = ' VFR_HUD ' , blocking = True )
tend = self . get_sim_time ( )
self . do_RTL ( )
psd = self . mavfft_fttd ( 1 , 0 , tstart * 1.0e6 , tend * 1.0e6 )
# batch sampler defaults give 1024 fft and sample rate of 1kz so roughly 1hz/bin
freq = psd [ " F " ] [ numpy . argmax ( psd [ " X " ] [ minhz : maxhz ] ) + minhz ] * ( 1000. / 1024. )
peakdb = numpy . amax ( psd [ " X " ] [ minhz : maxhz ] )
if peakdb < dblevel or ( peakhz is not None and abs ( freq - peakhz ) / peakhz > 0.05 ) :
if reverse is not None :
self . progress ( " Did not detect a motor peak, found %f Hz at %f dB " % ( freq , peakdb ) )
else :
raise NotAchievedException ( " Did not detect a motor peak, found %f Hz at %f dB " % ( freq , peakdb ) )
else :
if reverse is not None :
raise NotAchievedException ( " Detected motor peak at %f Hz, throttle %f %% , %f dB " % ( freq , vfr_hud . throttle , peakdb ) )
else :
self . progress ( " Detected motor peak at %f Hz, throttle %f %% , %f dB " % ( freq , vfr_hud . throttle , peakdb ) )
return freq , vfr_hud , peakdb
2019-07-15 14:53:20 -03:00
def fly_dynamic_notches ( self ) :
2019-12-27 08:56:03 -04:00
""" Use dynamic harmonic notch to control motor noise. """
2019-07-15 14:53:20 -03:00
self . progress ( " Flying with dynamic notches " )
2019-12-27 08:56:03 -04:00
self . context_push ( )
ex = None
try :
self . set_rc_default ( )
self . set_parameter ( " AHRS_EKF_TYPE " , 10 )
self . set_parameter ( " INS_LOG_BAT_MASK " , 3 )
self . set_parameter ( " INS_LOG_BAT_OPT " , 0 )
# set the gyro filter high so we can observe behaviour
self . set_parameter ( " INS_GYRO_FILTER " , 100 )
self . set_parameter ( " LOG_BITMASK " , 958 )
self . set_parameter ( " LOG_DISARMED " , 0 )
self . set_parameter ( " SIM_VIB_MOT_MAX " , 350 )
self . set_parameter ( " SIM_GYR_RND " , 20 )
self . reboot_sitl ( )
2019-07-15 14:53:20 -03:00
2019-12-27 08:56:03 -04:00
self . takeoff ( 10 , mode = " ALT_HOLD " )
2019-07-20 11:41:39 -03:00
2019-12-27 08:56:03 -04:00
# find a motor peak
freq , vfr_hud , peakdb = self . hover_and_check_matched_frequency_with_fft ( - 15 , 200 , 300 )
2019-07-15 14:53:20 -03:00
2019-12-27 08:56:03 -04:00
# now add a dynamic notch and check that the peak is squashed
self . set_parameter ( " INS_LOG_BAT_OPT " , 2 )
self . set_parameter ( " INS_HNTCH_ENABLE " , 1 )
self . set_parameter ( " INS_HNTCH_FREQ " , freq )
self . set_parameter ( " INS_HNTCH_REF " , vfr_hud . throttle / 100. )
# first and third harmonic
self . set_parameter ( " INS_HNTCH_HMNCS " , 5 )
self . set_parameter ( " INS_HNTCH_ATT " , 50 )
self . set_parameter ( " INS_HNTCH_BW " , freq / 2 )
self . reboot_sitl ( )
2019-07-15 14:53:20 -03:00
2019-12-27 08:56:03 -04:00
freq , vfr_hud , peakdb1 = self . hover_and_check_matched_frequency_with_fft ( - 10 , 20 , 350 , reverse = True )
# now add double dynamic notches and check that the peak is squashed
self . set_parameter ( " INS_HNTCH_OPTS " , 1 )
self . reboot_sitl ( )
freq , vfr_hud , peakdb2 = self . hover_and_check_matched_frequency_with_fft ( - 15 , 20 , 350 , reverse = True )
# double-notch should do better
if peakdb2 > peakdb1 :
raise NotAchievedException ( " Double-notch peak was higher than single-notch peak %f dB < %f dB " % ( peakdb2 , peakdb1 ) )
except Exception as e :
ex = e
self . context_pop ( )
if ex is not None :
raise ex
2019-07-15 14:53:20 -03:00
2020-02-24 17:56:36 -04:00
def hover_and_check_matched_frequency ( self , dblevel = - 15 , minhz = 200 , maxhz = 300 , fftLength = 32 , peakhz = None ) :
2019-07-20 11:41:39 -03:00
# find a motor peak
self . takeoff ( 10 , mode = " ALT_HOLD " )
hover_time = 15
tstart = self . get_sim_time ( )
self . progress ( " Hovering for %u seconds " % hover_time )
while self . get_sim_time_cached ( ) < tstart + hover_time :
attitude = self . mav . recv_match ( type = ' ATTITUDE ' , blocking = True )
vfr_hud = self . mav . recv_match ( type = ' VFR_HUD ' , blocking = True )
tend = self . get_sim_time ( )
self . do_RTL ( )
psd = self . mavfft_fttd ( 1 , 0 , tstart * 1.0e6 , tend * 1.0e6 )
# batch sampler defaults give 1024 fft and sample rate of 1kz so roughly 1hz/bin
2020-03-13 13:13:54 -03:00
scale = 1000. / 1024.
sminhz = int ( minhz * scale )
smaxhz = int ( maxhz * scale )
freq = psd [ " F " ] [ numpy . argmax ( psd [ " X " ] [ sminhz : smaxhz ] ) + sminhz ]
peakdb = numpy . amax ( psd [ " X " ] [ sminhz : smaxhz ] )
2020-03-13 13:13:54 -03:00
if peakdb < dblevel :
2019-07-20 11:41:39 -03:00
raise NotAchievedException ( " Did not detect a motor peak, found %f Hz at %f dB " % ( freq , peakdb ) )
2020-03-13 13:13:54 -03:00
elif peakhz is not None and abs ( freq - peakhz ) / peakhz > 0.05 :
raise NotAchievedException ( " Did not detect a motor peak at %f Hz, found %f Hz at %f dB " % ( peakhz , freq , peakdb ) )
2019-07-20 11:41:39 -03:00
else :
self . progress ( " Detected motor peak at %f Hz, throttle %f %% , %f dB " % ( freq , vfr_hud . throttle , peakdb ) )
# we have a peak make sure that the FFT detected something close
# logging is at 10Hz
mlog = self . dfreader_for_current_onboard_log ( )
2020-02-24 17:56:36 -04:00
# accuracy is determined by sample rate and fft length, given our use of quinn we could probably use half of this
freqDelta = 1000. / fftLength
2019-07-20 11:41:39 -03:00
pkAvg = freq
nmessages = 1
m = mlog . recv_match ( type = ' FTN1 ' , blocking = False ,
condition = " FTN1.TimeUS> %u and FTN1.TimeUS< %u " % ( tstart * 1.0e6 , tend * 1.0e6 ) )
2020-03-13 13:13:54 -03:00
freqs = [ ]
2019-07-20 11:41:39 -03:00
while m is not None :
nmessages = nmessages + 1
2020-03-13 13:13:54 -03:00
freqs . append ( m . PkAvg )
2019-07-20 11:41:39 -03:00
m = mlog . recv_match ( type = ' FTN1 ' , blocking = False ,
condition = " FTN1.TimeUS> %u and FTN1.TimeUS< %u " % ( tstart * 1.0e6 , tend * 1.0e6 ) )
2020-03-13 13:13:54 -03:00
# peak within resolution of FFT length
pkAvg = numpy . median ( numpy . asarray ( freqs ) )
2019-07-20 11:41:39 -03:00
self . progress ( " Detected motor peak at %f Hz processing %d messages " % ( pkAvg , nmessages ) )
# peak within 5%
2020-02-24 17:56:36 -04:00
if abs ( pkAvg - freq ) > freqDelta :
2019-07-20 11:41:39 -03:00
raise NotAchievedException ( " FFT did not detect a motor peak at %f , found %f , wanted %f " % ( dblevel , pkAvg , freq ) )
return freq
2020-03-05 03:52:24 -04:00
def fly_gyro_fft_harmonic ( self ) :
2020-02-24 17:56:36 -04:00
""" Use dynamic harmonic notch to control motor noise with harmonic matching of the first harmonic. """
2020-03-05 03:52:24 -04:00
# basic gyro sample rate test
2020-02-24 17:56:36 -04:00
self . progress ( " Flying with gyro FFT harmonic - Gyro sample rate " )
2020-03-05 03:52:24 -04:00
self . context_push ( )
ex = None
2020-03-13 13:13:54 -03:00
# we are dealing with probabalistic scenarios involving threads, have two bites at the cherry
for loop in [ " first " , " second " ] :
try :
self . start_subtest ( " Hover to calculate approximate hover frequency " )
self . set_rc_default ( )
# magic tridge EKF type that dramatically speeds up the test
self . set_parameter ( " AHRS_EKF_TYPE " , 10 )
self . set_parameter ( " EK2_ENABLE " , 0 )
self . set_parameter ( " INS_LOG_BAT_MASK " , 3 )
self . set_parameter ( " INS_LOG_BAT_OPT " , 0 )
self . set_parameter ( " INS_GYRO_FILTER " , 100 )
self . set_parameter ( " INS_FAST_SAMPLE " , 0 )
self . set_parameter ( " LOG_BITMASK " , 958 )
self . set_parameter ( " LOG_DISARMED " , 0 )
self . set_parameter ( " SIM_DRIFT_SPEED " , 0 )
self . set_parameter ( " SIM_DRIFT_TIME " , 0 )
self . set_parameter ( " FFT_THR_REF " , self . get_parameter ( " MOT_THST_HOVER " ) )
# enable a noisy motor peak
self . set_parameter ( " SIM_GYR_RND " , 20 )
# enabling FFT will also enable the arming check, self-testing the functionality
self . set_parameter ( " FFT_ENABLE " , 1 )
self . set_parameter ( " FFT_MINHZ " , 50 )
self . set_parameter ( " FFT_MAXHZ " , 450 )
self . set_parameter ( " FFT_SNR_REF " , 10 )
# Step 1: inject actual motor noise and use the FFT to track it
self . set_parameter ( " SIM_VIB_MOT_MAX " , 250 ) # gives a motor peak at about 175Hz
self . set_parameter ( " FFT_WINDOW_SIZE " , 64 )
self . set_parameter ( " FFT_WINDOW_OLAP " , 0.75 )
self . reboot_sitl ( )
freq = self . hover_and_check_matched_frequency ( - 15 , 100 , 250 , 64 )
# Step 2: add a second harmonic and check the first is still tracked
self . start_subtest ( " Add a fixed frequency harmonic at twice the hover frequency and check the right harmonic is found " )
self . set_parameter ( " SIM_VIB_FREQ_X " , freq * 2 )
self . set_parameter ( " SIM_VIB_FREQ_Y " , freq * 2 )
self . set_parameter ( " SIM_VIB_FREQ_Z " , freq * 2 )
self . set_parameter ( " SIM_VIB_MOT_MULT " , 0.25 ) # halve the motor noise so that the higher harmonic dominates
self . reboot_sitl ( )
2020-03-05 03:52:24 -04:00
2020-03-13 13:13:54 -03:00
self . hover_and_check_matched_frequency ( - 15 , 100 , 250 , 64 , None )
2020-03-05 03:52:24 -04:00
2020-03-13 13:13:54 -03:00
# Step 3: switch harmonics mid flight and check for tracking
2020-05-29 13:29:55 -03:00
self . start_subtest ( " Switch harmonics mid flight and check the right harmonic is found " )
2020-03-13 13:13:54 -03:00
self . set_parameter ( " FFT_HMNC_PEAK " , 0 )
self . reboot_sitl ( )
2020-03-05 03:52:24 -04:00
2020-03-13 13:13:54 -03:00
self . takeoff ( 10 , mode = " ALT_HOLD " )
2020-03-13 13:13:54 -03:00
2020-03-13 13:13:54 -03:00
hover_time = 10
tstart = self . get_sim_time ( )
self . progress ( " Hovering for %u seconds " % hover_time )
while self . get_sim_time_cached ( ) < tstart + hover_time :
attitude = self . mav . recv_match ( type = ' ATTITUDE ' , blocking = True )
vfr_hud = self . mav . recv_match ( type = ' VFR_HUD ' , blocking = True )
2020-03-05 03:52:24 -04:00
2020-03-13 13:13:54 -03:00
self . set_parameter ( " SIM_VIB_MOT_MULT " , 5.0 )
self . progress ( " Hovering for %u seconds " % hover_time )
while self . get_sim_time_cached ( ) < tstart + hover_time :
attitude = self . mav . recv_match ( type = ' ATTITUDE ' , blocking = True )
vfr_hud = self . mav . recv_match ( type = ' VFR_HUD ' , blocking = True )
tend = self . get_sim_time ( )
self . do_RTL ( )
mlog = self . dfreader_for_current_onboard_log ( )
m = mlog . recv_match ( type = ' FTN1 ' , blocking = False ,
condition = " FTN1.TimeUS> %u and FTN1.TimeUS< %u " % ( tstart * 1.0e6 , tend * 1.0e6 ) )
freqs = [ ]
while m is not None :
freqs . append ( m . PkAvg )
m = mlog . recv_match ( type = ' FTN1 ' , blocking = False ,
condition = " FTN1.TimeUS> %u and FTN1.TimeUS< %u " % ( tstart * 1.0e6 , tend * 1.0e6 ) )
# peak within resolution of FFT length, the highest energy peak switched but our detection should not
pkAvg = numpy . median ( numpy . asarray ( freqs ) )
freqDelta = 1000. / self . get_parameter ( " FFT_WINDOW_SIZE " )
if abs ( pkAvg - freq ) > freqDelta :
raise NotAchievedException ( " FFT did not detect a harmonic motor peak, found %f , wanted %f " % ( pkAvg , freq ) )
2020-05-29 13:29:55 -03:00
# Step 4: dynamic harmonic
self . start_subtest ( " Enable dynamic harmonics and make sure both frequency peaks are attenuated " )
# find a motor peak
freq , vfr_hud , peakdb = self . hover_and_check_matched_frequency_with_fft ( - 15 , 100 , 350 )
# now add a dynamic notch and check that the peak is squashed
self . set_parameter ( " INS_LOG_BAT_OPT " , 2 )
self . set_parameter ( " INS_HNTCH_ENABLE " , 1 )
self . set_parameter ( " INS_HNTCH_HMNCS " , 3 )
self . set_parameter ( " INS_HNTCH_MODE " , 4 )
self . set_parameter ( " INS_HNTCH_FREQ " , freq )
#self.set_parameter("INS_HNTCH_REF", 1.0)
self . set_parameter ( " INS_HNTCH_REF " , vfr_hud . throttle / 100. )
self . set_parameter ( " INS_HNTCH_ATT " , 100 )
self . set_parameter ( " INS_HNTCH_BW " , freq / 2 )
self . set_parameter ( " INS_HNTCH_OPTS " , 3 )
self . reboot_sitl ( )
self . hover_and_check_matched_frequency_with_fft ( - 1 , 100 , 350 , reverse = True )
2020-03-13 13:13:54 -03:00
self . set_parameter ( " SIM_VIB_FREQ_X " , 0 )
self . set_parameter ( " SIM_VIB_FREQ_Y " , 0 )
self . set_parameter ( " SIM_VIB_FREQ_Z " , 0 )
self . set_parameter ( " SIM_VIB_MOT_MULT " , 1. )
# prevent update parameters from messing with the settings when we pop the context
self . set_parameter ( " FFT_ENABLE " , 0 )
self . reboot_sitl ( )
except Exception as e :
self . progress ( " Exception caught in %s loop: %s " % ( loop , self . get_exception_stacktrace ( e ) ) )
autotest: Use ==/!= to compare constant literals (str, bytes, int, float, tuple)
Avoid SyntaxWarnings on Python >= 3.8
% `python3.8`
```
>>> "second" is "second"
<stdin>:1: SyntaxWarning: "is" with a literal. Did you mean "=="?
```
`flake8 . --count --select=E9,F63,F7,F82 --show-source --statistics`
```
./Tools/autotest/arducopter.py:3899:20: F632 use ==/!= to compare constant literals (str, bytes, int, float, tuple)
if loop is not "second":
^
./Tools/autotest/arducopter.py:4047:20: F632 use ==/!= to compare constant literals (str, bytes, int, float, tuple)
if loop is not "second":
^
2 F632 use ==/!= to compare constant literals (str, bytes, int, float, tuple)
2
```
2020-05-25 08:59:57 -03:00
if loop != " second " :
2020-03-13 13:13:54 -03:00
continue
ex = e
break
2020-03-05 03:52:24 -04:00
self . context_pop ( )
if ex is not None :
raise ex
2019-07-20 11:41:39 -03:00
def fly_gyro_fft ( self ) :
""" Use dynamic harmonic notch to control motor noise. """
# basic gyro sample rate test
self . progress ( " Flying with gyro FFT - Gyro sample rate " )
self . context_push ( )
ex = None
2020-03-13 13:13:54 -03:00
# we are dealing with probabalistic scenarios involving threads, have two bites at the cherry
for loop in [ " first " " second " ] :
try :
self . set_rc_default ( )
# magic tridge EKF type that dramatically speeds up the test
self . set_parameter ( " AHRS_EKF_TYPE " , 10 )
self . set_parameter ( " EK2_ENABLE " , 0 )
self . set_parameter ( " INS_LOG_BAT_MASK " , 3 )
self . set_parameter ( " INS_LOG_BAT_OPT " , 0 )
self . set_parameter ( " INS_GYRO_FILTER " , 100 )
self . set_parameter ( " INS_FAST_SAMPLE " , 0 )
self . set_parameter ( " LOG_BITMASK " , 958 )
self . set_parameter ( " LOG_DISARMED " , 0 )
self . set_parameter ( " SIM_DRIFT_SPEED " , 0 )
self . set_parameter ( " SIM_DRIFT_TIME " , 0 )
# enable a noisy motor peak
self . set_parameter ( " SIM_GYR_RND " , 20 )
# enabling FFT will also enable the arming check, self-testing the functionality
self . set_parameter ( " FFT_ENABLE " , 1 )
self . set_parameter ( " FFT_MINHZ " , 50 )
self . set_parameter ( " FFT_MAXHZ " , 450 )
self . set_parameter ( " FFT_SNR_REF " , 10 )
self . set_parameter ( " FFT_WINDOW_SIZE " , 128 )
self . set_parameter ( " FFT_WINDOW_OLAP " , 0.75 )
self . set_parameter ( " FFT_SAMPLE_MODE " , 0 )
# Step 1: inject a very precise noise peak at 250hz and make sure the in-flight fft
# can detect it really accurately. For a 128 FFT the frequency resolution is 8Hz so
# a 250Hz peak should be detectable within 5%
self . start_subtest ( " Inject noise at 250Hz and check the FFT can find the noise " )
self . set_parameter ( " SIM_VIB_FREQ_X " , 250 )
self . set_parameter ( " SIM_VIB_FREQ_Y " , 250 )
self . set_parameter ( " SIM_VIB_FREQ_Z " , 250 )
2020-03-13 13:13:54 -03:00
2020-03-13 13:13:54 -03:00
self . reboot_sitl ( )
2020-03-13 13:13:54 -03:00
2020-03-13 13:13:54 -03:00
# find a motor peak
self . hover_and_check_matched_frequency ( - 15 , 100 , 350 , 128 , 250 )
2019-07-20 11:41:39 -03:00
2020-03-13 13:13:54 -03:00
# Step 1b: run the same test with an FFT length of 256 which is needed to flush out a
# whole host of bugs related to uint8_t. This also tests very accurately the frequency resolution
self . set_parameter ( " FFT_WINDOW_SIZE " , 256 )
self . start_subtest ( " Inject noise at 250Hz and check the FFT can find the noise " )
2019-07-20 11:41:39 -03:00
2020-03-13 13:13:54 -03:00
self . reboot_sitl ( )
2019-07-20 11:41:39 -03:00
2020-03-13 13:13:54 -03:00
# find a motor peak
self . hover_and_check_matched_frequency ( - 15 , 100 , 350 , 256 , 250 )
self . set_parameter ( " FFT_WINDOW_SIZE " , 128 )
2019-07-20 11:41:39 -03:00
2020-03-13 13:13:54 -03:00
# Step 2: inject actual motor noise and use the standard length FFT to track it
self . start_subtest ( " Hover and check that the FFT can find the motor noise " )
self . set_parameter ( " SIM_VIB_FREQ_X " , 0 )
self . set_parameter ( " SIM_VIB_FREQ_Y " , 0 )
self . set_parameter ( " SIM_VIB_FREQ_Z " , 0 )
self . set_parameter ( " SIM_VIB_MOT_MAX " , 250 ) # gives a motor peak at about 175Hz
self . set_parameter ( " FFT_WINDOW_SIZE " , 32 )
self . set_parameter ( " FFT_WINDOW_OLAP " , 0.5 )
2020-02-24 17:56:36 -04:00
2020-03-13 13:13:54 -03:00
self . reboot_sitl ( )
freq = self . hover_and_check_matched_frequency ( - 15 , 100 , 250 , 32 )
self . set_parameter ( " SIM_VIB_MOT_MULT " , 1. )
# Step 3: add a FFT dynamic notch and check that the peak is squashed
self . start_subtest ( " Add a dynamic notch, hover and check that the noise peak is now gone " )
self . set_parameter ( " INS_LOG_BAT_OPT " , 2 )
self . set_parameter ( " INS_HNTCH_ENABLE " , 1 )
self . set_parameter ( " INS_HNTCH_FREQ " , freq )
self . set_parameter ( " INS_HNTCH_REF " , 1.0 )
self . set_parameter ( " INS_HNTCH_ATT " , 50 )
self . set_parameter ( " INS_HNTCH_BW " , freq / 2 )
self . set_parameter ( " INS_HNTCH_MODE " , 4 )
self . reboot_sitl ( )
2019-07-20 11:41:39 -03:00
2020-03-13 13:13:54 -03:00
self . takeoff ( 10 , mode = " ALT_HOLD " )
hover_time = 15
ignore_bins = 20
self . progress ( " Hovering for %u seconds " % hover_time )
tstart = self . get_sim_time ( )
while self . get_sim_time_cached ( ) < tstart + hover_time :
attitude = self . mav . recv_match ( type = ' ATTITUDE ' , blocking = True )
tend = self . get_sim_time ( )
# fly fast forrest!
self . set_rc ( 3 , 1900 )
self . set_rc ( 2 , 1200 )
self . wait_groundspeed ( 5 , 1000 )
self . set_rc ( 3 , 1500 )
self . set_rc ( 2 , 1500 )
self . do_RTL ( )
psd = self . mavfft_fttd ( 1 , 0 , tstart * 1.0e6 , tend * 1.0e6 )
# batch sampler defaults give 1024 fft and sample rate of 1kz so roughly 1hz/bin
scale = 1000. / 1024.
sminhz = int ( 100 * scale )
smaxhz = int ( 350 * scale )
freq = psd [ " F " ] [ numpy . argmax ( psd [ " X " ] [ sminhz : smaxhz ] ) + sminhz ]
peakdb = numpy . amax ( psd [ " X " ] [ sminhz : smaxhz ] )
if peakdb < - 9 :
self . progress ( " Did not detect a motor peak, found %f Hz at %f dB " % ( freq , peakdb ) )
else :
raise NotAchievedException ( " Detected %f Hz motor peak at %f dB " % ( freq , peakdb ) )
# Step 4: loop sample rate test with larger window
self . start_subtest ( " Hover and check that the FFT can find the motor noise when running at fast loop rate " )
# we are limited to half the loop rate for frequency detection
self . set_parameter ( " FFT_MAXHZ " , 185 )
self . set_parameter ( " INS_LOG_BAT_OPT " , 0 )
self . set_parameter ( " SIM_VIB_MOT_MAX " , 220 )
self . set_parameter ( " FFT_WINDOW_SIZE " , 64 )
self . set_parameter ( " FFT_WINDOW_OLAP " , 0.75 )
self . set_parameter ( " FFT_SAMPLE_MODE " , 1 )
self . reboot_sitl ( )
2019-07-20 11:41:39 -03:00
2020-03-13 13:13:54 -03:00
self . takeoff ( 10 , mode = " ALT_HOLD " )
2019-07-20 11:41:39 -03:00
2020-03-13 13:13:54 -03:00
self . progress ( " Hovering for %u seconds " % hover_time )
tstart = self . get_sim_time ( )
while self . get_sim_time_cached ( ) < tstart + hover_time :
attitude = self . mav . recv_match ( type = ' ATTITUDE ' , blocking = True )
tend = self . get_sim_time ( )
2019-07-20 11:41:39 -03:00
2020-03-13 13:13:54 -03:00
self . do_RTL ( )
# prevent update parameters from messing with the settings when we pop the context
self . set_parameter ( " FFT_ENABLE " , 0 )
self . reboot_sitl ( )
2019-07-20 11:41:39 -03:00
2020-03-13 13:13:54 -03:00
except Exception as e :
self . progress ( " Exception caught in %s loop: %s " % ( loop , self . get_exception_stacktrace ( e ) ) )
autotest: Use ==/!= to compare constant literals (str, bytes, int, float, tuple)
Avoid SyntaxWarnings on Python >= 3.8
% `python3.8`
```
>>> "second" is "second"
<stdin>:1: SyntaxWarning: "is" with a literal. Did you mean "=="?
```
`flake8 . --count --select=E9,F63,F7,F82 --show-source --statistics`
```
./Tools/autotest/arducopter.py:3899:20: F632 use ==/!= to compare constant literals (str, bytes, int, float, tuple)
if loop is not "second":
^
./Tools/autotest/arducopter.py:4047:20: F632 use ==/!= to compare constant literals (str, bytes, int, float, tuple)
if loop is not "second":
^
2 F632 use ==/!= to compare constant literals (str, bytes, int, float, tuple)
2
```
2020-05-25 08:59:57 -03:00
if loop != " second " :
2020-03-13 13:13:54 -03:00
continue
ex = e
break
2019-07-20 11:41:39 -03:00
self . context_pop ( )
if ex is not None :
raise ex
2019-03-23 01:30:34 -03:00
def fly_brake_mode ( self ) :
# test brake mode
self . progress ( " Testing brake mode " )
self . takeoff ( 10 , mode = " LOITER " )
self . progress ( " Ensuring RC inputs have no effect in brake mode " )
self . change_mode ( " STABILIZE " )
self . set_rc ( 3 , 1500 )
self . set_rc ( 2 , 1200 )
self . wait_groundspeed ( 5 , 1000 )
self . change_mode ( " BRAKE " )
self . wait_groundspeed ( 0 , 1 )
self . set_rc ( 2 , 1500 )
self . do_RTL ( )
self . progress ( " Ran brake mode " )
2019-02-23 01:01:27 -04:00
def fly_guided_move_to ( self , destination , timeout = 30 ) :
''' move to mavutil.location location; absolute altitude '''
tstart = self . get_sim_time ( )
while True :
if self . get_sim_time ( ) - tstart > timeout :
raise NotAchievedException ( )
self . mav . mav . set_position_target_global_int_send (
0 , # timestamp
1 , # target system_id
1 , # target component id
mavutil . mavlink . MAV_FRAME_GLOBAL_INT ,
0b1111111111111000 , # mask specifying use-only-lat-lon-alt
2020-06-14 22:24:22 -03:00
int ( destination . lat * 1e7 ) , # lat
int ( destination . lng * 1e7 ) , # lon
2019-02-23 01:01:27 -04:00
destination . alt , # alt
0 , # vx
0 , # vy
0 , # vz
0 , # afx
0 , # afy
0 , # afz
0 , # yaw
0 , # yawrate
)
delta = self . get_distance ( self . mav . location ( ) , destination )
self . progress ( " delta= %f (want <1) " % delta )
if delta < 1 :
break
def test_altitude_types ( self ) :
''' start by disabling GCS failsafe, otherwise we immediately disarm
due to ( apparently ) not receiving traffic from the GCS for
too long . This is probably a function of - - speedup '''
''' this test flies the vehicle somewhere lower than were it started.
It then disarms . It then arms , which should reset home to the
new , lower altitude . This delta should be outside 1 m but
within a few metres of the old one .
'''
self . set_parameter ( " FS_GCS_ENABLE " , 0 )
self . change_mode ( ' GUIDED ' )
self . wait_ready_to_arm ( )
self . arm_vehicle ( )
m = self . mav . recv_match ( type = ' GLOBAL_POSITION_INT ' , blocking = True )
max_initial_home_alt_m = 500
if m . relative_alt > max_initial_home_alt_m :
raise NotAchievedException ( " Initial home alt too high ( %f m > %f m) " %
( m . relative_alt * 1000 , max_initial_home_alt_m * 1000 ) )
orig_home_offset_mm = m . alt - m . relative_alt
self . user_takeoff ( 5 )
self . progress ( " Flying to low position " )
current_alt = self . mav . location ( ) . alt
# 10m delta low_position = mavutil.location(-35.358273, 149.169165, current_alt, 0)
low_position = mavutil . location ( - 35.36200016 , 149.16415599 , current_alt , 0 )
self . fly_guided_move_to ( low_position , timeout = 240 )
self . change_mode ( ' LAND ' )
# expecting home to change when disarmed
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2019-02-23 01:01:27 -04:00
# wait a while for home to move (it shouldn't):
self . delay_sim_time ( 10 )
m = self . mav . recv_match ( type = ' GLOBAL_POSITION_INT ' , blocking = True )
new_home_offset_mm = m . alt - m . relative_alt
home_offset_delta_mm = orig_home_offset_mm - new_home_offset_mm
self . progress ( " new home offset: %f delta= %f " %
( new_home_offset_mm , home_offset_delta_mm ) )
self . progress ( " gpi= %s " % str ( m ) )
max_home_offset_delta_mm = 10
if home_offset_delta_mm > max_home_offset_delta_mm :
raise NotAchievedException ( " Large home offset delta: want< %f got= %f " %
( max_home_offset_delta_mm , home_offset_delta_mm ) )
self . progress ( " Ensuring home moves when we arm " )
self . change_mode ( ' GUIDED ' )
self . wait_ready_to_arm ( )
self . arm_vehicle ( )
m = self . mav . recv_match ( type = ' GLOBAL_POSITION_INT ' , blocking = True )
post_arming_home_offset_mm = m . alt - m . relative_alt
self . progress ( " post-arming home offset: %f " % ( post_arming_home_offset_mm ) )
self . progress ( " gpi= %s " % str ( m ) )
min_post_arming_home_offset_delta_mm = - 3000
max_post_arming_home_offset_delta_mm = - 4000
delta_between_original_home_alt_offset_and_new_home_alt_offset_mm = post_arming_home_offset_mm - orig_home_offset_mm
self . progress ( " delta= %f - %f = %f " % (
post_arming_home_offset_mm ,
orig_home_offset_mm ,
delta_between_original_home_alt_offset_and_new_home_alt_offset_mm ) )
self . progress ( " Home moved %f m vertically " % ( delta_between_original_home_alt_offset_and_new_home_alt_offset_mm / 1000.0 ) )
if delta_between_original_home_alt_offset_and_new_home_alt_offset_mm > min_post_arming_home_offset_delta_mm :
raise NotAchievedException (
" Home did not move vertically on arming: want<= %f got= %f " %
( min_post_arming_home_offset_delta_mm , delta_between_original_home_alt_offset_and_new_home_alt_offset_mm ) )
if delta_between_original_home_alt_offset_and_new_home_alt_offset_mm < max_post_arming_home_offset_delta_mm :
raise NotAchievedException (
" Home moved too far vertically on arming: want>= %f got= %f " %
( max_post_arming_home_offset_delta_mm , delta_between_original_home_alt_offset_and_new_home_alt_offset_mm ) )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2019-02-23 01:01:27 -04:00
2018-07-15 20:54:00 -03:00
def fly_precision_companion ( self ) :
""" Use Companion PrecLand backend precision messages to loiter. """
self . context_push ( )
ex = None
try :
self . set_parameter ( " PLND_ENABLED " , 1 )
self . fetch_parameters ( )
# enable companion backend:
self . set_parameter ( " PLND_TYPE " , 1 )
2019-07-10 10:57:46 -03:00
self . set_analog_rangefinder_parameters ( )
2018-07-15 20:54:00 -03:00
# set up a channel switch to enable precision loiter:
self . set_parameter ( " RC7_OPTION " , 39 )
self . reboot_sitl ( )
self . progress ( " Waiting for location " )
self . mav . location ( )
2018-10-10 10:07:21 -03:00
self . zero_throttle ( )
2018-07-15 20:54:00 -03:00
self . mavproxy . send ( ' switch 6 \n ' ) # stabilize mode
2018-12-13 18:17:02 -04:00
self . wait_heartbeat ( )
2018-07-15 20:54:00 -03:00
self . wait_mode ( ' STABILIZE ' )
self . wait_ready_to_arm ( )
# we should be doing precision loiter at this point
start = self . mav . recv_match ( type = ' LOCAL_POSITION_NED ' ,
blocking = True )
self . arm_vehicle ( )
self . set_rc ( 3 , 1800 )
alt_min = 10
self . wait_altitude ( alt_min ,
( alt_min + 5 ) ,
relative = True )
self . set_rc ( 3 , 1500 )
# move away a little
self . set_rc ( 2 , 1550 )
2019-07-08 13:15:02 -03:00
self . wait_distance ( 5 , accuracy = 1 )
2018-07-15 20:54:00 -03:00
self . set_rc ( 2 , 1500 )
self . mavproxy . send ( ' mode loiter \n ' )
self . wait_mode ( ' LOITER ' )
# turn precision loiter on:
self . set_rc ( 7 , 2000 )
# try to drag aircraft to a position 5 metres north-east-east:
self . loiter_to_ne ( start . x + 5 , start . y + 10 , start . z + 10 )
self . loiter_to_ne ( start . x + 5 , start . y - 10 , start . z + 10 )
except Exception as e :
2019-06-18 00:13:49 -03:00
self . progress ( " Exception caught: %s " % (
self . get_exception_stacktrace ( e ) ) )
2018-07-15 20:54:00 -03:00
ex = e
self . context_pop ( )
2018-10-10 10:07:21 -03:00
self . zero_throttle ( )
2019-02-26 00:29:12 -04:00
self . disarm_vehicle ( force = True )
2018-07-15 20:54:00 -03:00
self . reboot_sitl ( )
self . progress ( " All done " )
2018-10-12 18:20:13 -03:00
if ex is not None :
raise ex
2019-03-10 08:17:21 -03:00
def loiter_requires_position ( self ) :
2019-02-28 20:46:59 -04:00
# ensure we can't switch to LOITER without position
self . progress ( " Ensure we can ' t enter LOITER without position " )
self . context_push ( )
self . set_parameter ( " GPS_TYPE " , 0 )
self . reboot_sitl ( )
self . change_mode ( ' STABILIZE ' )
2019-03-20 21:15:11 -03:00
self . wait_ekf_flags ( ( mavutil . mavlink . ESTIMATOR_ATTITUDE |
mavutil . mavlink . ESTIMATOR_VELOCITY_VERT |
mavutil . mavlink . ESTIMATOR_POS_VERT_ABS |
mavutil . mavlink . ESTIMATOR_CONST_POS_MODE |
mavutil . mavlink . ESTIMATOR_PRED_POS_HORIZ_REL ) ,
0 ,
timeout = 120 )
2019-02-28 20:46:59 -04:00
self . arm_vehicle ( )
self . mavproxy . send ( " mode LOITER \n " )
self . mavproxy . expect ( " requires position " )
self . disarm_vehicle ( )
self . context_pop ( )
self . reboot_sitl ( )
2019-03-10 08:17:21 -03:00
def test_arm_feature ( self ) :
self . loiter_requires_position ( )
2019-02-28 20:46:59 -04:00
super ( AutoTestCopter , self ) . test_arm_feature ( )
2019-01-09 22:26:36 -04:00
def test_parameter_checks ( self ) :
2019-03-04 23:52:24 -04:00
self . test_parameter_checks_poscontrol ( " PSC " )
2019-01-09 22:26:36 -04:00
2018-12-31 00:23:14 -04:00
def fly_poshold_takeoff ( self ) :
""" ensure vehicle stays put until it is ready to fly """
self . context_push ( )
ex = None
try :
self . set_parameter ( " PILOT_TKOFF_ALT " , 700 )
self . mavproxy . send ( ' mode POSHOLD \n ' )
self . wait_mode ( ' POSHOLD ' )
self . set_rc ( 3 , 1000 )
self . wait_ready_to_arm ( )
self . arm_vehicle ( )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 2 )
2018-12-31 00:23:14 -04:00
# check we are still on the ground...
m = self . mav . recv_match ( type = ' GLOBAL_POSITION_INT ' , blocking = True )
if abs ( m . relative_alt ) > 100 :
raise NotAchievedException ( " Took off prematurely " )
self . progress ( " Pushing throttle up " )
self . set_rc ( 3 , 1710 )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 0.5 )
2018-12-31 00:23:14 -04:00
self . progress ( " Bringing back to hover throttle " )
self . set_rc ( 3 , 1500 )
# make sure we haven't already reached alt:
m = self . mav . recv_match ( type = ' GLOBAL_POSITION_INT ' , blocking = True )
max_initial_alt = 500
if abs ( m . relative_alt ) > max_initial_alt :
raise NotAchievedException ( " Took off too fast ( %f > %f " %
( abs ( m . relative_alt ) , max_initial_alt ) )
self . progress ( " Monitoring takeoff-to-alt " )
self . wait_altitude ( 6.9 , 8 , relative = True )
self . progress ( " Making sure we stop at our takeoff altitude " )
tstart = self . get_sim_time ( )
while self . get_sim_time ( ) - tstart < 5 :
m = self . mav . recv_match ( type = ' GLOBAL_POSITION_INT ' , blocking = True )
delta = abs ( 7000 - m . relative_alt )
self . progress ( " alt= %f delta= %f " % ( m . relative_alt / 1000 ,
delta / 1000 ) )
if delta > 1000 :
raise NotAchievedException ( " Failed to maintain takeoff alt " )
self . progress ( " takeoff OK " )
except Exception as e :
ex = e
self . mavproxy . send ( ' mode LAND \n ' )
self . wait_mode ( ' LAND ' )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2018-12-31 00:23:14 -04:00
self . set_rc ( 8 , 1000 )
self . context_pop ( )
if ex is not None :
raise ex
2018-12-26 20:16:30 -04:00
def initial_mode ( self ) :
return " STABILIZE "
def initial_mode_switch_mode ( self ) :
2019-02-03 07:28:42 -04:00
return " STABILIZE "
2018-12-26 20:16:30 -04:00
2018-12-17 02:42:21 -04:00
def default_mode ( self ) :
return " STABILIZE "
2018-04-14 08:31:22 -03:00
2019-02-03 06:43:07 -04:00
def rc_defaults ( self ) :
ret = super ( AutoTestCopter , self ) . rc_defaults ( )
ret [ 3 ] = 1000
2019-02-03 07:28:42 -04:00
ret [ 5 ] = 1800 # mode switch
2019-02-03 06:43:07 -04:00
return ret
2018-08-23 07:24:34 -03:00
2019-03-26 22:31:06 -03:00
def test_manual_control ( self ) :
''' test manual_control mavlink message '''
self . change_mode ( ' STABILIZE ' )
self . takeoff ( 10 )
tstart = self . get_sim_time_cached ( )
want_pitch_degrees = - 20
while True :
if self . get_sim_time_cached ( ) - tstart > 10 :
raise AutoTestTimeoutException ( " Did not reach pitch " )
self . progress ( " Sending pitch-forward " )
self . mav . mav . manual_control_send (
1 , # target system
500 , # x (pitch)
32767 , # y (roll)
32767 , # z (thrust)
32767 , # r (yaw)
0 ) # button mask
m = self . mav . recv_match ( type = ' ATTITUDE ' , blocking = True , timeout = 1 )
print ( " m= %s " % str ( m ) )
if m is None :
continue
p = math . degrees ( m . pitch )
self . progress ( " pitch= %f want<= %f " % ( p , want_pitch_degrees ) )
if p < = want_pitch_degrees :
break
self . mav . mav . manual_control_send (
1 , # target system
32767 , # x (pitch)
32767 , # y (roll)
32767 , # z (thrust)
32767 , # r (yaw)
0 ) # button mask
self . do_RTL ( )
2019-05-29 05:32:22 -03:00
def check_avoidance_corners ( self ) :
self . takeoff ( 10 , mode = " LOITER " )
self . set_rc ( 2 , 1400 )
west_loc = mavutil . location ( - 35.363007 ,
149.164911 ,
0 ,
0 )
self . wait_location ( west_loc , accuracy = 6 )
north_loc = mavutil . location ( - 35.362908 ,
149.165051 ,
0 ,
0 )
self . reach_heading_manual ( 0 ) ;
self . wait_location ( north_loc , accuracy = 6 )
self . reach_heading_manual ( 90 ) ;
east_loc = mavutil . location ( - 35.363013 ,
149.165194 ,
0 ,
0 )
self . wait_location ( east_loc , accuracy = 6 )
self . reach_heading_manual ( 225 ) ;
self . wait_location ( west_loc , accuracy = 6 )
self . set_rc ( 2 , 1500 )
self . do_RTL ( )
def fly_proximity_avoidance_test ( self ) :
self . context_push ( )
ex = None
try :
2019-08-05 01:43:27 -03:00
self . load_fence ( " copter-avoidance-fence.txt " )
2019-05-29 05:32:22 -03:00
self . set_parameter ( " FENCE_ENABLE " , 0 )
self . set_parameter ( " PRX_TYPE " , 10 )
self . set_parameter ( " RC10_OPTION " , 40 ) # proximity-enable
self . reboot_sitl ( )
self . progress ( " Enabling proximity " )
self . set_rc ( 10 , 2000 )
self . check_avoidance_corners ( )
except Exception as e :
2019-08-07 01:58:25 -03:00
self . progress ( " Caught exception: %s " %
self . get_exception_stacktrace ( e ) )
2019-05-29 05:32:22 -03:00
ex = e
self . context_pop ( )
self . mavproxy . send ( " fence clear \n " )
self . disarm_vehicle ( force = True )
self . reboot_sitl ( )
if ex is not None :
raise ex
2019-05-29 06:43:12 -03:00
def fly_fence_avoidance_test ( self ) :
self . context_push ( )
ex = None
try :
2019-08-05 01:43:27 -03:00
self . load_fence ( " copter-avoidance-fence.txt " )
2019-05-29 06:43:12 -03:00
self . set_parameter ( " FENCE_ENABLE " , 1 )
self . check_avoidance_corners ( )
except Exception as e :
2019-08-07 01:58:25 -03:00
self . progress ( " Caught exception: %s " %
self . get_exception_stacktrace ( e ) )
2019-05-29 06:43:12 -03:00
ex = e
self . context_pop ( )
2019-08-07 01:58:25 -03:00
self . clear_fence ( )
2019-05-29 06:43:12 -03:00
self . disarm_vehicle ( force = True )
2019-05-29 07:23:58 -03:00
if ex is not None :
raise ex
2020-02-05 08:27:56 -04:00
def global_position_int_for_location ( self , loc , time_boot , heading = 0 ) :
2019-09-15 20:57:27 -03:00
return self . mav . mav . global_position_int_encode (
2020-02-05 08:27:56 -04:00
int ( time_boot * 1000 ) , # time_boot_ms
2019-09-15 20:57:27 -03:00
int ( loc . lat * 1e7 ) ,
int ( loc . lng * 1e7 ) ,
int ( loc . alt * 1000 ) , # alt in mm
20 , # relative alt - urp.
vx = 0 ,
vy = 0 ,
vz = 0 ,
hdg = heading
)
def fly_follow_mode ( self ) :
self . set_parameter ( " FOLL_ENABLE " , 1 )
self . set_parameter ( " FOLL_SYSID " , 255 )
foll_ofs_x = 30 # metres
self . set_parameter ( " FOLL_OFS_X " , - foll_ofs_x )
self . set_parameter ( " FOLL_OFS_TYPE " , 1 ) # relative to other vehicle heading
self . takeoff ( 10 , mode = " LOITER " )
self . set_parameter ( " SIM_SPEEDUP " , 1 )
self . change_mode ( " FOLLOW " )
new_loc = self . mav . location ( )
new_loc_offset_n = 20
new_loc_offset_e = 30
self . location_offset_ne ( new_loc , new_loc_offset_n , new_loc_offset_e )
self . progress ( " new_loc: %s " % str ( new_loc ) )
heading = 0
self . mavproxy . send ( " map icon %f %f greenplane %f \n " %
( new_loc . lat , new_loc . lng , heading ) )
expected_loc = copy . copy ( new_loc )
self . location_offset_ne ( expected_loc , - foll_ofs_x , 0 )
self . mavproxy . send ( " map icon %f %f hoop \n " %
( expected_loc . lat , expected_loc . lng ) )
self . progress ( " expected_loc: %s " % str ( expected_loc ) )
last_sent = 0
while True :
now = self . get_sim_time_cached ( )
if now - last_sent > 0.5 :
gpi = self . global_position_int_for_location ( new_loc ,
now ,
heading = heading )
gpi . pack ( self . mav . mav )
self . mav . mav . send ( gpi )
2019-09-21 03:04:51 -03:00
self . mav . recv_match ( type = ' GLOBAL_POSITION_INT ' , blocking = True )
2019-09-15 20:57:27 -03:00
pos = self . mav . location ( )
delta = self . get_distance ( expected_loc , pos )
2019-11-04 05:18:34 -04:00
max_delta = 3
2019-09-15 20:57:27 -03:00
self . progress ( " position delta= %f (want < %f ) " % ( delta , max_delta ) )
if delta < max_delta :
break
self . do_RTL ( )
2019-06-28 22:37:16 -03:00
def fly_beacon_position ( self ) :
self . reboot_sitl ( )
self . wait_ready_to_arm ( require_absolute = True )
old_pos = self . mav . location ( )
print ( " old_pos= %s " % str ( old_pos ) )
self . context_push ( )
ex = None
try :
self . set_parameter ( " BCN_TYPE " , 10 )
self . set_parameter ( " BCN_LATITUDE " , SITL_START_LOCATION . lat )
self . set_parameter ( " BCN_LONGITUDE " , SITL_START_LOCATION . lng )
self . set_parameter ( " BCN_ALT " , SITL_START_LOCATION . alt )
self . set_parameter ( " BCN_ORIENT_YAW " , 45 )
self . set_parameter ( " AVOID_ENABLE " , 4 )
self . set_parameter ( " GPS_TYPE " , 0 )
2019-06-28 23:18:21 -03:00
self . set_parameter ( " EK3_GPS_TYPE " , 3 ) # NOGPS
self . set_parameter ( " EK3_ENABLE " , 1 )
self . set_parameter ( " EK2_ENABLE " , 0 )
2019-06-28 22:37:16 -03:00
self . reboot_sitl ( )
# require_absolute=True infers a GPS is present
self . wait_ready_to_arm ( require_absolute = False )
tstart = self . get_sim_time ( )
timeout = 20
while True :
if self . get_sim_time_cached ( ) - tstart > timeout :
raise NotAchievedException ( " Did not get new position like old position " )
self . progress ( " Fetching location " )
new_pos = self . mav . location ( )
pos_delta = self . get_distance ( old_pos , new_pos )
max_delta = 1
self . progress ( " delta= %u want <= %u " % ( pos_delta , max_delta ) )
if pos_delta < = max_delta :
break
self . progress ( " Moving to ensure location is tracked " )
self . takeoff ( 10 , mode = " LOITER " )
self . set_rc ( 2 , 1400 )
self . delay_sim_time ( 5 )
self . set_rc ( 2 , 1500 )
self . wait_groundspeed ( 0 , 0.1 )
pos_delta = self . get_distance ( self . sim_location ( ) , self . mav . location )
if pos_delta > max_delta :
raise NotAchievedException ( " Vehicle location not tracking simulated location ( %u > %u ) " %
( pos_delta , max_delta ) )
except Exception as e :
2019-08-07 01:58:25 -03:00
self . progress ( " Caught exception: %s " %
self . get_exception_stacktrace ( e ) )
2019-06-28 22:37:16 -03:00
ex = e
self . context_pop ( )
self . disarm_vehicle ( force = True )
self . reboot_sitl ( )
if ex is not None :
raise ex
2019-05-29 07:23:58 -03:00
def fly_beacon_avoidance_test ( self ) :
self . context_push ( )
ex = None
try :
self . set_parameter ( " BCN_TYPE " , 10 )
self . set_parameter ( " BCN_LATITUDE " , int ( SITL_START_LOCATION . lat ) )
self . set_parameter ( " BCN_LONGITUDE " , int ( SITL_START_LOCATION . lng ) )
self . set_parameter ( " BCN_ORIENT_YAW " , 45 )
self . set_parameter ( " AVOID_ENABLE " , 4 )
self . reboot_sitl ( )
self . takeoff ( 10 , mode = " LOITER " )
self . set_rc ( 2 , 1400 )
west_loc = mavutil . location ( - 35.362919 , 149.165055 , 0 , 0 )
self . wait_location ( west_loc , accuracy = 7 )
self . reach_heading_manual ( 0 )
north_loc = mavutil . location ( - 35.362881 , 149.165103 , 0 , 0 )
self . wait_location ( north_loc , accuracy = 7 )
self . set_rc ( 2 , 1500 )
self . set_rc ( 1 , 1600 )
east_loc = mavutil . location ( - 35.362986 , 149.165227 , 0 , 0 )
self . wait_location ( east_loc , accuracy = 7 )
self . set_rc ( 1 , 1500 )
self . set_rc ( 2 , 1600 )
south_loc = mavutil . location ( - 35.363025 , 149.165182 , 0 , 0 )
self . wait_location ( south_loc , accuracy = 7 )
self . set_rc ( 2 , 1500 )
self . do_RTL ( )
except Exception as e :
2019-08-07 01:58:25 -03:00
self . progress ( " Caught exception: %s " %
self . get_exception_stacktrace ( e ) )
2019-05-29 07:23:58 -03:00
ex = e
self . context_pop ( )
self . mavproxy . send ( " fence clear \n " )
self . disarm_vehicle ( force = True )
2019-05-29 06:43:12 -03:00
self . reboot_sitl ( )
if ex is not None :
raise ex
2019-12-02 03:30:44 -04:00
def test_richenpower ( self ) :
self . set_parameter ( " SERIAL5_PROTOCOL " , 30 )
self . set_parameter ( " SIM_RICH_ENABLE " , 1 )
self . set_parameter ( " SERVO8_FUNCTION " , 42 )
self . set_parameter ( " SIM_RICH_CTRL " , 8 )
self . set_parameter ( " RC9_OPTION " , 85 )
self . set_parameter ( " LOG_DISARMED " , 1 )
self . customise_SITL_commandline ( [ " --uartF=sim:richenpower " ] )
self . set_rc ( 9 , 1000 ) # remember this is a switch position - stop
self . mavproxy . expect ( " requested state is not RUN " )
2020-07-03 06:32:05 -03:00
messages = [ ]
def my_message_hook ( mav , m ) :
if m . get_type ( ) != ' STATUSTEXT ' :
return
messages . append ( m )
self . install_message_hook ( my_message_hook )
try :
self . set_rc ( 9 , 2000 ) # remember this is a switch position - run
finally :
self . remove_message_hook ( my_message_hook )
if " Generator HIGH " not in [ x . text for x in messages ] :
self . mavproxy . expect ( " Generator HIGH " )
2019-12-02 03:30:44 -04:00
self . set_rc ( 9 , 1000 ) # remember this is a switch position - stop
self . mavproxy . expect ( " requested state is not RUN " , timeout = 200 )
self . set_message_rate_hz ( " GENERATOR_STATUS " , 1 )
self . drain_mav_unparsed ( )
m = self . assert_receive_message ( " GENERATOR_STATUS " , timeout = 10 )
if m . generator_speed == 0 :
raise NotAchievedException ( " Zero GENERATOR_STATUS.generator_speed " )
self . set_message_rate_hz ( " GENERATOR_STATUS " , - 1 )
self . set_parameter ( " LOG_DISARMED " , 0 )
if not self . current_onboard_log_contains_message ( " GEN " ) :
raise NotAchievedException ( " Did not find expected GEN message " )
2019-06-05 20:33:28 -03:00
def get_mission_count ( self ) :
return self . get_parameter ( " MIS_TOTAL " )
def assert_mission_count ( self , expected ) :
count = self . get_mission_count ( )
if count != expected :
raise NotAchievedException ( " Unexpected count got= %u want= %u " %
( count , expected ) )
def test_aux_switch_options ( self ) :
self . set_parameter ( " RC7_OPTION " , 58 ) # clear waypoints
self . load_mission ( " copter_loiter_to_alt.txt " )
self . set_rc ( 7 , 1000 )
self . assert_mission_count ( 5 )
self . progress ( " Clear mission " )
self . set_rc ( 7 , 2000 )
2019-06-13 20:22:59 -03:00
self . delay_sim_time ( 1 ) # allow switch to debounce
2019-06-05 20:33:28 -03:00
self . assert_mission_count ( 0 )
self . set_rc ( 7 , 1000 )
self . set_parameter ( " RC7_OPTION " , 24 ) # reset mission
self . delay_sim_time ( 2 )
self . load_mission ( " copter_loiter_to_alt.txt " )
set_wp = 4
self . mavproxy . send ( " wp set %u \n " % set_wp )
self . delay_sim_time ( 1 )
self . drain_mav ( )
self . wait_current_waypoint ( set_wp , timeout = 10 )
self . progress ( " Reset mission " )
self . set_rc ( 7 , 2000 )
self . delay_sim_time ( 1 )
self . drain_mav ( )
self . wait_current_waypoint ( 0 , timeout = 10 )
self . set_rc ( 7 , 1000 )
2019-12-01 22:01:06 -04:00
def fly_rangefinder_drivers_fly ( self , rangefinders ) :
''' ensure rangefinder gives height-above-ground '''
self . change_mode ( ' GUIDED ' )
self . wait_ready_to_arm ( )
self . arm_vehicle ( )
expected_alt = 5
self . user_takeoff ( alt_min = expected_alt )
rf = self . mav . recv_match ( type = " RANGEFINDER " , timeout = 1 , blocking = True )
if rf is None :
raise NotAchievedException ( " Did not receive rangefinder message " )
gpi = self . mav . recv_match ( type = ' GLOBAL_POSITION_INT ' , blocking = True , timeout = 1 )
if gpi is None :
raise NotAchievedException ( " Did not receive GLOBAL_POSITION_INT message " )
if abs ( rf . distance - gpi . relative_alt / 1000.0 ) > 1 :
raise NotAchievedException ( " rangefinder alt ( %s ) disagrees with global-position-int.relative_alt ( %s ) " % ( rf . distance , gpi . relative_alt / 1000.0 ) )
for i in range ( 0 , len ( rangefinders ) ) :
name = rangefinders [ i ]
self . progress ( " i= %u ( %s ) " % ( i , name ) )
ds = self . mav . recv_match (
type = " DISTANCE_SENSOR " ,
timeout = 2 ,
blocking = True ,
condition = " DISTANCE_SENSOR.id== %u " % i
)
if ds is None :
raise NotAchievedException ( " Did not receive DISTANCE_SENSOR message for id== %u ( %s ) " % ( i , name ) )
self . progress ( " Got: %s " % str ( ds ) )
if abs ( ds . current_distance / 100.0 - gpi . relative_alt / 1000.0 ) > 1 :
raise NotAchievedException (
" distance sensor.current_distance ( %f ) ( %s ) disagrees with global-position-int.relative_alt ( %s ) " %
( ds . current_distance / 100.0 , name , gpi . relative_alt / 1000.0 ) )
self . land_and_disarm ( )
self . progress ( " Ensure RFND messages in log " )
if not self . current_onboard_log_contains_message ( " RFND " ) :
raise NotAchievedException ( " No RFND messages in log " )
2020-04-21 01:05:50 -03:00
def fly_rangefinder_mavlink ( self ) :
# explicit test for the mavlink driver as it doesn't play so nice:
self . set_parameter ( " SERIAL5_PROTOCOL " , 1 )
self . set_parameter ( " RNGFND1_TYPE " , 10 )
self . customise_SITL_commandline ( [ ' --uartF=sim:rf_mavlink ' ] )
self . change_mode ( ' GUIDED ' )
self . wait_ready_to_arm ( )
self . arm_vehicle ( )
expected_alt = 5
self . user_takeoff ( alt_min = expected_alt )
tstart = self . get_sim_time ( )
while True :
if self . get_sim_time ( ) - tstart > 5 :
raise NotAchievedException ( " Mavlink rangefinder not working " )
rf = self . mav . recv_match ( type = " RANGEFINDER " , timeout = 1 , blocking = True )
if rf is None :
raise NotAchievedException ( " Did not receive rangefinder message " )
gpi = self . mav . recv_match ( type = ' GLOBAL_POSITION_INT ' , blocking = True , timeout = 1 )
if gpi is None :
raise NotAchievedException ( " Did not receive GLOBAL_POSITION_INT message " )
if abs ( rf . distance - gpi . relative_alt / 1000.0 ) > 1 :
success = False
print ( " rangefinder alt ( %s ) disagrees with global-position-int.relative_alt ( %s ) " % ( rf . distance , gpi . relative_alt / 1000.0 ) )
continue
ds = self . mav . recv_match (
type = " DISTANCE_SENSOR " ,
timeout = 2 ,
blocking = True ,
)
if ds is None :
raise NotAchievedException ( " Did not receive DISTANCE_SENSOR message " )
self . progress ( " Got: %s " % str ( ds ) )
if abs ( ds . current_distance / 100.0 - gpi . relative_alt / 1000.0 ) > 1 :
print (
" distance sensor.current_distance ( %f ) disagrees with global-position-int.relative_alt ( %s ) " %
( ds . current_distance / 100.0 , gpi . relative_alt / 1000.0 ) )
continue
break
self . progress ( " mavlink rangefinder OK " )
self . land_and_disarm ( )
2019-12-01 22:01:06 -04:00
def fly_rangefinder_drivers ( self ) :
self . set_parameter ( " RTL_ALT " , 500 )
2019-12-12 01:50:10 -04:00
self . set_parameter ( " RTL_ALT_TYPE " , 1 )
2019-12-01 22:01:06 -04:00
drivers = [
2020-06-30 08:10:28 -03:00
( " lightwareserial " , 8 ) , # autodetected between this and -binary
( " lightwareserial-binary " , 8 ) ,
2019-12-01 22:01:06 -04:00
( " ulanding_v0 " , 11 ) ,
( " ulanding_v1 " , 11 ) ,
( " leddarone " , 12 ) ,
( " maxsonarseriallv " , 13 ) ,
( " nmea " , 17 ) ,
( " wasp " , 18 ) ,
( " benewake_tf02 " , 19 ) ,
( " blping " , 23 ) ,
( " benewake_tfmini " , 20 ) ,
( " lanbao " , 26 ) ,
( " benewake_tf03 " , 27 ) ,
2020-06-14 20:38:46 -03:00
( " gyus42v2 " , 31 ) ,
2019-12-01 22:01:06 -04:00
]
while len ( drivers ) :
do_drivers = drivers [ 0 : 3 ]
drivers = drivers [ 3 : ]
command_line_args = [ ]
for ( offs , cmdline_argument , serial_num ) in [ ( 0 , ' --uartE ' , 4 ) ,
( 1 , ' --uartF ' , 5 ) ,
( 2 , ' --uartG ' , 6 ) ] :
if len ( do_drivers ) > offs :
( sim_name , rngfnd_param_value ) = do_drivers [ offs ]
command_line_args . append ( " %s =sim: %s " %
( cmdline_argument , sim_name ) )
serial_param_name = " SERIAL %u _PROTOCOL " % serial_num
self . set_parameter ( serial_param_name , 9 ) # rangefinder
self . set_parameter ( " RNGFND %u _TYPE " % ( offs + 1 ) , rngfnd_param_value )
self . customise_SITL_commandline ( command_line_args )
self . fly_rangefinder_drivers_fly ( [ x [ 0 ] for x in do_drivers ] )
2020-04-21 01:05:50 -03:00
self . fly_rangefinder_mavlink ( )
2020-06-29 07:36:00 -03:00
def fly_ship_takeoff ( self ) :
# test ship takeoff
self . wait_groundspeed ( 0 , 2 )
self . set_parameter ( " SIM_SHIP_ENABLE " , 1 )
self . set_parameter ( " SIM_SHIP_SPEED " , 10 )
self . set_parameter ( " SIM_SHIP_DSIZE " , 2 )
self . wait_ready_to_arm ( )
# we should be moving with the ship
self . wait_groundspeed ( 9 , 11 )
self . takeoff ( 10 )
# above ship our speed drops to 0
self . wait_groundspeed ( 0 , 2 )
self . land_and_disarm ( )
# ship will have moved on, so we land on the water which isn't moving
self . wait_groundspeed ( 0 , 2 )
2020-04-21 01:05:50 -03:00
2020-01-29 21:14:39 -04:00
def test_parameter_validation ( self ) :
self . progress ( " invalid; min must be less than max: " )
self . set_parameter ( " MOT_PWM_MIN " , 100 )
self . set_parameter ( " MOT_PWM_MAX " , 50 )
self . drain_mav ( )
self . assert_prearm_failure ( " Check MOT_PWM_MIN/MAX " )
self . progress ( " invalid; min must be less than max (equal case): " )
self . set_parameter ( " MOT_PWM_MIN " , 100 )
self . set_parameter ( " MOT_PWM_MAX " , 100 )
self . drain_mav ( )
self . assert_prearm_failure ( " Check MOT_PWM_MIN/MAX " )
self . progress ( " invalid; both must be non-zero or both zero (min=0) " )
self . set_parameter ( " MOT_PWM_MIN " , 0 )
self . set_parameter ( " MOT_PWM_MAX " , 100 )
self . drain_mav ( )
self . assert_prearm_failure ( " Check MOT_PWM_MIN/MAX " )
self . progress ( " invalid; both must be non-zero or both zero (max=0) " )
self . set_parameter ( " MOT_PWM_MIN " , 100 )
self . set_parameter ( " MOT_PWM_MAX " , 0 )
self . drain_mav ( )
self . assert_prearm_failure ( " Check MOT_PWM_MIN/MAX " )
2020-02-21 21:32:43 -04:00
def tests1 ( self ) :
2018-12-17 02:42:21 -04:00
''' return list of all tests '''
ret = super ( AutoTestCopter , self ) . tests ( )
ret . extend ( [
2018-03-05 11:14:34 -04:00
2018-12-17 02:42:21 -04:00
( " NavDelayTakeoffAbsTime " ,
" Fly Nav Delay (takeoff) " ,
self . fly_nav_takeoff_delay_abstime ) ,
2018-03-05 11:14:34 -04:00
2018-12-17 02:42:21 -04:00
( " NavDelayAbsTime " ,
" Fly Nav Delay (AbsTime) " ,
self . fly_nav_delay_abstime ) ,
2018-03-05 11:14:34 -04:00
2018-12-17 02:42:21 -04:00
( " NavDelay " ,
" Fly Nav Delay " ,
self . fly_nav_delay ) ,
2018-03-05 11:14:34 -04:00
2018-12-17 02:42:21 -04:00
( " GuidedSubModeChange " ,
" Test submode change " ,
self . fly_guided_change_submode ) ,
2018-05-10 04:15:38 -03:00
2018-12-17 02:42:21 -04:00
( " LoiterToAlt " ,
" Loiter-To-Alt " ,
self . fly_loiter_to_alt ) ,
2018-04-27 15:21:53 -03:00
2018-12-17 02:42:21 -04:00
( " PayLoadPlaceMission " ,
" Payload Place Mission " ,
self . fly_payload_place_mission ) ,
2018-03-05 11:14:34 -04:00
2018-12-17 02:42:21 -04:00
( " PrecisionLoiterCompanion " ,
" Precision Loiter (Companion) " ,
self . fly_precision_companion ) ,
2018-03-05 11:14:34 -04:00
2018-12-17 02:42:21 -04:00
( " PrecisionLandingSITL " ,
" Precision Landing (SITL) " ,
self . fly_precision_sitl ) ,
2018-03-05 11:14:34 -04:00
2018-12-17 02:42:21 -04:00
( " SetModesViaModeSwitch " ,
" Set modes via modeswitch " ,
self . test_setting_modes_via_modeswitch ) ,
2018-03-05 11:14:34 -04:00
2018-12-17 02:42:21 -04:00
( " SetModesViaAuxSwitch " ,
" Set modes via auxswitch " ,
self . test_setting_modes_via_auxswitch ) ,
2018-03-05 11:14:34 -04:00
2019-06-05 20:33:28 -03:00
( " AuxSwitchOptions " ,
" Test random aux mode options " ,
self . test_aux_switch_options ) ,
2018-12-17 02:42:21 -04:00
( " AutoTune " , " Fly AUTOTUNE mode " , self . fly_autotune ) ,
2018-03-05 11:14:34 -04:00
2020-06-27 07:49:17 -03:00
( " AutoTuneSwitch " , " Fly AUTOTUNE on a switch " , self . fly_autotune_switch ) ,
2018-03-22 22:31:09 -03:00
( " ThrowMode " , " Fly Throw Mode " , self . fly_throw_mode ) ,
2019-03-23 01:30:34 -03:00
( " BrakeMode " , " Fly Brake Mode " , self . fly_brake_mode ) ,
2018-12-17 02:42:21 -04:00
( " RecordThenPlayMission " ,
" Use switches to toggle in mission, then fly it " ,
self . fly_square ) ,
2018-03-05 11:14:34 -04:00
2018-12-17 02:42:21 -04:00
( " ThrottleFailsafe " ,
" Test Throttle Failsafe " ,
self . fly_throttle_failsafe ) ,
2018-03-05 11:14:34 -04:00
2019-09-28 02:41:56 -03:00
( " GCSFailsafe " ,
" Test GCS Failsafe " ,
self . fly_gcs_failsafe ) ,
2018-12-17 02:42:21 -04:00
( " BatteryFailsafe " ,
" Fly Battery Failsafe " ,
self . fly_battery_failsafe ) ,
2018-03-05 11:14:34 -04:00
2018-12-17 02:42:21 -04:00
( " StabilityPatch " ,
" Fly stability patch " ,
lambda : self . fly_stability_patch ( 30 ) ) ,
2018-03-05 11:14:34 -04:00
2019-05-29 05:32:22 -03:00
( " AC_Avoidance_Proximity " ,
" Test proximity avoidance slide behaviour " ,
self . fly_proximity_avoidance_test ) ,
2019-05-29 06:43:12 -03:00
( " AC_Avoidance_Fence " ,
" Test fence avoidance slide behaviour " ,
self . fly_fence_avoidance_test ) ,
2019-05-29 07:23:58 -03:00
( " AC_Avoidance_Beacon " ,
" Test beacon avoidance slide behaviour " ,
self . fly_beacon_avoidance_test ) ,
2018-12-17 02:42:21 -04:00
( " HorizontalFence " ,
" Test horizontal fence " ,
2019-03-07 18:28:46 -04:00
self . fly_fence_test ) ,
2018-03-05 11:14:34 -04:00
2019-06-06 21:57:13 -03:00
( " HorizontalAvoidFence " ,
" Test horizontal Avoidance fence " ,
self . fly_fence_avoid_test ) ,
2018-12-17 02:42:21 -04:00
( " MaxAltFence " ,
" Test Max Alt Fence " ,
self . fly_alt_max_fence_test ) ,
2018-03-05 11:14:34 -04:00
2018-12-17 02:42:21 -04:00
( " GPSGlitchLoiter " ,
" GPS Glitch Loiter Test " ,
self . fly_gps_glitch_loiter_test ) ,
2018-10-02 19:28:21 -03:00
2018-12-17 02:42:21 -04:00
( " GPSGlitchAuto " ,
" GPS Glitch Auto Test " ,
self . fly_gps_glitch_auto_test ) ,
2018-04-27 15:21:53 -03:00
2019-03-05 21:29:32 -04:00
( " ModeAltHold " ,
" Test AltHold Mode " ,
self . test_mode_ALT_HOLD ) ,
2018-12-17 02:42:21 -04:00
( " ModeLoiter " ,
" Test Loiter Mode " ,
self . loiter ) ,
2017-04-10 14:48:48 -03:00
2018-12-17 02:42:21 -04:00
( " SimpleMode " ,
" Fly in SIMPLE mode " ,
self . fly_simple ) ,
2017-04-10 14:48:48 -03:00
2018-12-17 02:42:21 -04:00
( " SuperSimpleCircle " ,
" Fly a circle in SUPER SIMPLE mode " ,
self . fly_super_simple ) ,
2017-04-10 14:48:48 -03:00
2018-12-17 02:42:21 -04:00
( " ModeCircle " ,
" Fly CIRCLE mode " ,
self . fly_circle ) ,
2017-04-10 14:48:48 -03:00
2018-12-17 02:42:21 -04:00
( " OpticalFlowLimits " ,
" Fly Optical Flow limits " ,
self . fly_optical_flow_limits ) ,
2018-05-10 04:15:38 -03:00
2018-12-17 02:42:21 -04:00
( " MotorFail " ,
" Fly motor failure test " ,
self . fly_motor_fail ) ,
2011-10-30 23:50:34 -03:00
2019-02-02 00:19:18 -04:00
( " Flip " ,
" Fly Flip Mode " ,
self . fly_flip ) ,
2018-12-17 02:42:21 -04:00
( " CopterMission " ,
" Fly copter mission " ,
self . fly_auto_test ) ,
2011-11-01 08:31:01 -03:00
2019-09-10 01:52:56 -03:00
( " SplineLastWaypoint " ,
" Test Spline as last waypoint " ,
self . test_spline_last_waypoint ) ,
2018-12-17 02:42:21 -04:00
( " Gripper " ,
" Test gripper " ,
self . test_gripper ) ,
( " TestGripperMission " ,
" Test Gripper mission items " ,
self . test_gripper_mission ) ,
( " VisionPosition " ,
" Fly Vision Position " ,
self . fly_vision_position ) ,
2019-06-28 22:37:16 -03:00
( " BeaconPosition " ,
" Fly Beacon Position " ,
self . fly_beacon_position ) ,
2019-01-31 01:01:33 -04:00
( " RTLSpeed " ,
" Fly RTL Speed " ,
self . fly_rtl_speed ) ,
2018-12-17 02:42:21 -04:00
( " Mount " ,
" Test Camera/Antenna Mount " ,
self . test_mount ) ,
2019-11-27 18:34:21 -04:00
( " Button " ,
" Test Buttons " ,
self . test_button ) ,
2020-06-29 07:36:00 -03:00
( " ShipTakeoff " ,
" Fly Simulated Ship Takeoff " ,
self . fly_ship_takeoff ) ,
2019-01-07 18:51:36 -04:00
( " RangeFinder " ,
" Test RangeFinder Basic Functionality " ,
self . test_rangefinder ) ,
2019-06-18 00:13:49 -03:00
( " SurfaceTracking " ,
" Test Surface Tracking " ,
self . test_surface_tracking ) ,
2019-01-08 21:40:49 -04:00
( " Parachute " ,
" Test Parachute Functionality " ,
self . test_parachute ) ,
2019-01-09 22:26:36 -04:00
( " ParameterChecks " ,
" Test Arming Parameter Checks " ,
self . test_parameter_checks ) ,
2019-03-06 02:37:26 -04:00
( " ManualThrottleModeChange " ,
" Check manual throttle mode changes denied on high throttle " ,
self . fly_manual_throttle_mode_change ) ,
2019-03-26 22:31:06 -03:00
( " MANUAL_CONTROL " ,
" Test mavlink MANUAL_CONTROL " ,
self . test_manual_control ) ,
2018-09-07 01:20:23 -03:00
# Zigzag mode test
( " ZigZag " , " Fly ZigZag Mode " , self . fly_zigzag_mode ) ,
2018-12-31 00:23:14 -04:00
( " PosHoldTakeOff " ,
" Fly POSHOLD takeoff " ,
self . fly_poshold_takeoff ) ,
2019-09-15 20:57:27 -03:00
( " FOLLOW " ,
" Fly follow mode " ,
self . fly_follow_mode ) ,
2019-12-01 22:01:06 -04:00
( " RangeFinderDrivers " ,
" Test rangefinder drivers " ,
self . fly_rangefinder_drivers ) ,
2020-01-29 21:14:39 -04:00
( " ParameterValidation " ,
" Test parameters are checked for validity " ,
self . test_parameter_validation ) ,
2019-02-23 01:01:27 -04:00
( " AltTypes " ,
" Test Different Altitude Types " ,
self . test_altitude_types ) ,
2019-12-02 03:30:44 -04:00
( " RichenPower " ,
" Test RichenPower generator " ,
self . test_richenpower ) ,
2020-05-11 20:29:39 -03:00
( " LogUpload " ,
" Log upload " ,
self . log_upload ) ,
2018-12-17 02:42:21 -04:00
] )
return ret
2018-03-05 11:14:34 -04:00
2020-02-21 21:32:43 -04:00
def tests2 ( self ) :
''' return list of all tests '''
ret = ( [
( " MotorVibration " ,
" Fly motor vibration test " ,
self . fly_motor_vibration ) ,
( " DynamicNotches " ,
" Fly Dynamic Notches " ,
self . fly_dynamic_notches ) ,
( " GyroFFT " ,
" Fly Gyro FFT " ,
self . fly_gyro_fft ) ,
2020-02-23 22:12:12 -04:00
( " FixedYawCalibration " ,
" Test Fixed Yaw Calibration " ,
self . test_fixed_yaw_calibration ) ,
2020-03-05 03:52:24 -04:00
( " GyroFFTHarmonic " ,
2020-03-05 07:04:35 -04:00
" Fly Gyro FFT Harmonic Matching " ,
2020-03-05 03:52:24 -04:00
self . fly_gyro_fft_harmonic ) ,
2020-05-13 12:05:26 -03:00
( " SITLCompassCalibration " ,
" Test SITL onboard compass calibration " ,
self . test_mag_calibration ) ,
2020-06-05 21:24:59 -03:00
( " CRSF " ,
" Test RC CRSF " ,
self . test_crsf ) ,
2020-05-11 20:29:39 -03:00
( " LogUpload " ,
" Log upload " ,
self . log_upload ) ,
2020-02-21 21:32:43 -04:00
] )
return ret
def tests ( self ) :
ret = [ ]
ret . extend ( self . tests1 ( ) )
ret . extend ( self . tests2 ( ) )
return ret
2019-01-14 01:06:41 -04:00
def disabled_tests ( self ) :
return {
" Parachute " : " See https://github.com/ArduPilot/ardupilot/issues/4702 " ,
2019-06-07 01:52:13 -03:00
" HorizontalAvoidFence " : " See https://github.com/ArduPilot/ardupilot/issues/11525 " ,
2019-06-30 20:48:08 -03:00
" BeaconPosition " : " See https://github.com/ArduPilot/ardupilot/issues/11689 " ,
2019-01-14 01:06:41 -04:00
}
2018-03-05 11:14:34 -04:00
2018-12-17 02:57:16 -04:00
class AutoTestHeli ( AutoTestCopter ) :
2018-03-05 11:14:34 -04:00
2019-03-09 00:20:36 -04:00
def log_name ( self ) :
return " HeliCopter "
2018-03-05 11:14:34 -04:00
2019-03-09 00:20:36 -04:00
def default_frame ( self ) :
return " heli "
2015-05-27 05:13:55 -03:00
2019-02-15 20:46:33 -04:00
def sitl_start_location ( self ) :
return SITL_START_LOCATION_AVC
2018-10-10 10:07:21 -03:00
def is_heli ( self ) :
return True
2019-02-03 06:43:07 -04:00
def rc_defaults ( self ) :
ret = super ( AutoTestHeli , self ) . rc_defaults ( )
ret [ 8 ] = 1000
ret [ 3 ] = 1000 # collective
return ret
2015-05-27 05:13:55 -03:00
2019-08-06 03:09:40 -03:00
@staticmethod
def get_position_armable_modes_list ( ) :
''' filter THROW mode out of armable modes list; Heli is special-cased '''
ret = AutoTestCopter . get_position_armable_modes_list ( )
ret = filter ( lambda x : x != " THROW " , ret )
return ret
2019-03-10 08:17:21 -03:00
def loiter_requires_position ( self ) :
self . progress ( " Skipping loiter-requires-position for heli; rotor runup issues " )
2018-10-10 19:47:35 -03:00
def get_collective_out ( self ) :
servo = self . mav . recv_match ( type = ' SERVO_OUTPUT_RAW ' , blocking = True )
chan_pwm = ( servo . servo1_raw + servo . servo2_raw + servo . servo3_raw ) / 3.0
return chan_pwm
def rotor_runup_complete_checks ( self ) :
# Takeoff and landing in Loiter
TARGET_RUNUP_TIME = 10
self . zero_throttle ( )
self . change_mode ( ' LOITER ' )
self . wait_ready_to_arm ( )
self . arm_vehicle ( )
servo = self . mav . recv_match ( type = ' SERVO_OUTPUT_RAW ' , blocking = True )
coll = servo . servo1_raw
coll = coll + 50
self . set_parameter ( " H_RSC_RUNUP_TIME " , TARGET_RUNUP_TIME )
self . progress ( " Initiate Runup by putting some throttle " )
self . set_rc ( 8 , 2000 )
self . set_rc ( 3 , 1700 )
self . progress ( " Collective threshold PWM %u " % coll )
tstart = self . get_sim_time ( )
self . progress ( " Wait that collective PWM pass threshold value " )
servo = self . mav . recv_match ( condition = ' SERVO_OUTPUT_RAW.servo1_raw> %u ' % coll , blocking = True )
runup_time = self . get_sim_time ( ) - tstart
self . progress ( " Collective is now at PWM %u " % servo . servo1_raw )
self . mav . wait_heartbeat ( )
if runup_time < TARGET_RUNUP_TIME :
self . zero_throttle ( )
self . set_rc ( 8 , 1000 )
self . disarm_vehicle ( )
self . mav . wait_heartbeat ( )
raise NotAchievedException ( " Takeoff initiated before runup time complete %u " % runup_time )
self . progress ( " Runup time %u " % runup_time )
self . zero_throttle ( )
self . set_rc ( 8 , 1000 )
self . mavproxy . send ( ' mode LAND \n ' )
self . wait_mode ( ' LAND ' )
self . mav . motors_disarmed_wait ( )
self . mav . wait_heartbeat ( )
2019-03-25 23:07:45 -03:00
# fly_avc_test - fly AVC mission
def fly_avc_test ( self ) :
# Arm
self . mavproxy . send ( ' switch 6 \n ' ) # stabilize mode
self . wait_mode ( ' STABILIZE ' )
self . wait_ready_to_arm ( )
self . arm_vehicle ( )
self . progress ( " Raising rotor speed " )
self . set_rc ( 8 , 2000 )
# upload mission from file
self . progress ( " # Load copter_AVC2013_mission " )
# load the waypoint count
num_wp = self . load_mission ( " copter_AVC2013_mission.txt " )
if not num_wp :
raise NotAchievedException ( " load copter_AVC2013_mission failed " )
self . progress ( " Fly AVC mission from 1 to %u " % num_wp )
self . mavproxy . send ( ' wp set 1 \n ' )
# wait for motor runup
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 20 )
2019-03-25 23:07:45 -03:00
# switch into AUTO mode and raise throttle
self . mavproxy . send ( ' switch 4 \n ' ) # auto mode
self . wait_mode ( ' AUTO ' )
self . set_rc ( 3 , 1500 )
# fly the mission
self . wait_waypoint ( 0 , num_wp - 1 , timeout = 500 )
# set throttle to minimum
self . zero_throttle ( )
# wait for disarm
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2019-03-25 23:07:45 -03:00
self . progress ( " MOTORS DISARMED OK " )
self . progress ( " Lowering rotor speed " )
self . set_rc ( 8 , 1000 )
self . progress ( " AVC mission completed: passed! " )
2018-11-27 23:21:29 -04:00
def fly_heli_poshold_takeoff ( self ) :
""" ensure vehicle stays put until it is ready to fly """
self . context_push ( )
ex = None
try :
self . set_parameter ( " PILOT_TKOFF_ALT " , 700 )
2018-10-10 19:47:35 -03:00
self . change_mode ( ' POSHOLD ' )
self . zero_throttle ( )
2018-11-27 23:21:29 -04:00
self . set_rc ( 8 , 1000 )
self . wait_ready_to_arm ( )
2018-10-10 19:47:35 -03:00
# Arm
2018-11-27 23:21:29 -04:00
self . arm_vehicle ( )
2018-10-10 19:47:35 -03:00
self . progress ( " Raising rotor speed " )
2018-11-27 23:21:29 -04:00
self . set_rc ( 8 , 2000 )
self . progress ( " wait for rotor runup to complete " )
2020-05-29 00:21:30 -03:00
self . wait_servo_channel_value ( 8 , 1660 , timeout = 10 )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 20 )
2018-11-27 23:21:29 -04:00
# check we are still on the ground...
m = self . mav . recv_match ( type = ' GLOBAL_POSITION_INT ' , blocking = True )
if abs ( m . relative_alt ) > 100 :
raise NotAchievedException ( " Took off prematurely " )
self . progress ( " Pushing throttle past half-way " )
self . set_rc ( 3 , 1600 )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 0.5 )
2018-11-27 23:21:29 -04:00
self . progress ( " Bringing back to hover throttle " )
self . set_rc ( 3 , 1500 )
# make sure we haven't already reached alt:
m = self . mav . recv_match ( type = ' GLOBAL_POSITION_INT ' , blocking = True )
if abs ( m . relative_alt ) > 500 :
raise NotAchievedException ( " Took off too fast " )
self . progress ( " Monitoring takeoff-to-alt " )
self . wait_altitude ( 6.9 , 8 , relative = True )
self . progress ( " Making sure we stop at our takeoff altitude " )
tstart = self . get_sim_time ( )
while self . get_sim_time ( ) - tstart < 5 :
m = self . mav . recv_match ( type = ' GLOBAL_POSITION_INT ' , blocking = True )
delta = abs ( 7000 - m . relative_alt )
self . progress ( " alt= %f delta= %f " % ( m . relative_alt / 1000 ,
delta / 1000 ) )
if delta > 1000 :
raise NotAchievedException ( " Failed to maintain takeoff alt " )
self . progress ( " takeoff OK " )
except Exception as e :
ex = e
self . mavproxy . send ( ' mode LAND \n ' )
self . wait_mode ( ' LAND ' )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2018-11-27 23:21:29 -04:00
self . set_rc ( 8 , 1000 )
self . context_pop ( )
if ex is not None :
2020-07-02 20:24:09 -03:00
self . progress ( " Caught exception: %s " %
self . get_exception_stacktrace ( ex ) )
2018-11-27 23:21:29 -04:00
raise ex
2020-05-29 00:13:45 -03:00
def fly_heli_stabilize_takeoff ( self ) :
""" """
self . context_push ( )
ex = None
try :
self . change_mode ( ' STABILIZE ' )
self . set_rc ( 3 , 1000 )
self . set_rc ( 8 , 1000 )
self . wait_ready_to_arm ( )
self . arm_vehicle ( )
self . set_rc ( 8 , 2000 )
self . progress ( " wait for rotor runup to complete " )
self . wait_servo_channel_value ( 8 , 1660 , timeout = 10 )
self . delay_sim_time ( 20 )
# check we are still on the ground...
m = self . mav . recv_match ( type = ' GLOBAL_POSITION_INT ' , blocking = True )
if abs ( m . relative_alt ) > 100 :
raise NotAchievedException ( " Took off prematurely " )
self . progress ( " Pushing throttle past half-way " )
self . set_rc ( 3 , 1600 )
self . progress ( " Monitoring takeoff " )
self . wait_altitude ( 6.9 , 8 , relative = True )
self . progress ( " takeoff OK " )
except Exception as e :
ex = e
self . mavproxy . send ( ' mode LAND \n ' )
self . wait_mode ( ' LAND ' )
self . wait_disarmed ( )
self . set_rc ( 8 , 1000 )
self . context_pop ( )
if ex is not None :
raise ex
2019-09-19 05:58:23 -03:00
def fly_spline_waypoint ( self , timeout = 600 ) :
2019-04-15 22:44:29 -03:00
""" ensure basic spline functionality works """
self . load_mission ( " copter_spline_mission.txt " )
self . change_mode ( " LOITER " )
self . wait_ready_to_arm ( )
2019-04-24 23:16:44 -03:00
self . arm_vehicle ( )
2019-04-15 22:44:29 -03:00
self . progress ( " Raising rotor speed " )
self . set_rc ( 8 , 2000 )
2019-11-08 02:22:30 -04:00
self . delay_sim_time ( 20 )
2019-04-15 22:44:29 -03:00
self . change_mode ( " AUTO " )
self . set_rc ( 3 , 1500 )
2019-04-26 02:10:11 -03:00
tstart = self . get_sim_time ( )
while True :
2019-09-19 05:58:23 -03:00
if self . get_sim_time ( ) - tstart > timeout :
raise AutoTestTimeoutException ( " Vehicle did not disarm after mission " )
2019-04-26 02:10:11 -03:00
if not self . armed ( ) :
break
self . delay_sim_time ( 1 )
2019-04-15 22:44:29 -03:00
self . progress ( " Lowering rotor speed " )
self . set_rc ( 8 , 1000 )
2019-12-31 22:49:47 -04:00
def fly_autorotation ( self , timeout = 600 ) :
""" ensure basic spline functionality works """
self . set_parameter ( " AROT_ENABLE " , 1 )
start_alt = 100 # metres
self . set_parameter ( " PILOT_TKOFF_ALT " , start_alt * 100 )
self . mavproxy . send ( ' mode POSHOLD \n ' )
self . wait_mode ( ' POSHOLD ' )
self . set_rc ( 3 , 1000 )
self . set_rc ( 8 , 1000 )
self . wait_ready_to_arm ( )
self . arm_vehicle ( )
self . set_rc ( 8 , 2000 )
self . progress ( " wait for rotor runup to complete " )
2020-05-29 00:21:30 -03:00
self . wait_servo_channel_value ( 8 , 1660 , timeout = 10 )
2019-12-31 22:49:47 -04:00
self . delay_sim_time ( 20 )
self . set_rc ( 3 , 2000 )
self . wait_altitude ( start_alt - 1 ,
( start_alt + 5 ) ,
relative = True ,
timeout = timeout )
self . progress ( " Triggering autorotate by raising interlock " )
self . set_rc ( 8 , 1000 )
self . mavproxy . expect ( " SS Glide Phase " )
self . mavproxy . expect ( " Hit ground at ([0-9.]+) m/s " )
speed = float ( self . mavproxy . match . group ( 1 ) )
if speed > 30 :
raise NotAchievedException ( " Hit too hard " )
2020-03-12 20:51:17 -03:00
self . wait_disarmed ( )
2019-12-31 22:49:47 -04:00
2018-11-27 23:21:29 -04:00
def set_rc_default ( self ) :
2019-09-21 03:04:51 -03:00
super ( AutoTestHeli , self ) . set_rc_default ( )
2018-11-27 23:21:29 -04:00
self . progress ( " Lowering rotor speed " )
self . set_rc ( 8 , 1000 )
2018-12-17 02:57:16 -04:00
def tests ( self ) :
''' return list of all tests '''
2019-09-21 03:04:51 -03:00
ret = AutoTest . tests ( self )
2018-12-17 02:57:16 -04:00
ret . extend ( [
( " AVCMission " , " Fly AVC mission " , self . fly_avc_test ) ,
2013-05-29 02:33:32 -03:00
2018-10-10 19:47:35 -03:00
( " RotorRunUp " ,
" Test rotor runup " ,
self . rotor_runup_complete_checks ) ,
2018-11-27 23:21:29 -04:00
( " PosHoldTakeOff " ,
" Fly POSHOLD takeoff " ,
self . fly_heli_poshold_takeoff ) ,
2020-05-29 00:13:45 -03:00
( " StabilizeTakeOff " ,
" Fly stabilize takeoff " ,
self . fly_heli_stabilize_takeoff ) ,
2019-04-15 22:44:29 -03:00
( " SplineWaypoint " ,
" Fly Spline Waypoints " ,
self . fly_spline_waypoint ) ,
2019-12-31 22:49:47 -04:00
( " AutoRotation " ,
" Fly AutoRotation " ,
self . fly_autorotation ) ,
2020-05-11 20:29:39 -03:00
( " LogUpload " ,
" Log upload " ,
self . log_upload ) ,
2018-12-17 02:57:16 -04:00
] )
return ret
2020-02-21 18:27:18 -04:00
2020-06-26 04:16:40 -03:00
def disabled_tests ( self ) :
return {
" SplineWaypoint " : " See https://github.com/ArduPilot/ardupilot/issues/14593 " ,
}
2020-02-21 18:27:18 -04:00
2020-02-21 21:32:43 -04:00
class AutoTestCopterTests1 ( AutoTestCopter ) :
2020-02-21 18:27:18 -04:00
def tests ( self ) :
2020-02-21 21:32:43 -04:00
return self . tests1 ( )
2020-02-21 18:27:18 -04:00
2020-02-21 21:32:43 -04:00
class AutoTestCopterTests2 ( AutoTestCopter ) :
2020-02-21 18:27:18 -04:00
2020-02-21 21:32:43 -04:00
def tests ( self ) :
return self . tests2 ( )