mirror of https://github.com/ArduPilot/ardupilot
autotest: test Solo mount
This commit is contained in:
Peter Barker
Peter Barker
parent
218724a3fe
commit
aab1ae4018
|
|
@ -5245,6 +5245,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
|
|||
def test_mount_pitch(self, despitch, despitch_tolerance, mount_mode, timeout=10, hold=0, constrained=True):
|
||||
tstart = self.get_sim_time()
|
||||
success_start = 0
|
||||
|
||||
while True:
|
||||
now = self.get_sim_time_cached()
|
||||
if now - tstart > timeout:
|
||||
|
|
@ -5257,7 +5258,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
|
|||
'''retrieve latest angles from GIMBAL_DEVICE_ATTITUDE_STATUS'''
|
||||
mount_roll, mount_pitch, mount_yaw = self.get_mount_roll_pitch_yaw_deg()
|
||||
|
||||
self.progress("despitch=%f roll=%f pitch=%f yaw=%f" % (despitch, mount_roll, mount_pitch, mount_yaw))
|
||||
# self.progress("despitch=%f roll=%f pitch=%f yaw=%f" % (despitch, mount_roll, mount_pitch, mount_yaw))
|
||||
if abs(despitch - mount_pitch) > despitch_tolerance:
|
||||
self.progress("Mount pitch incorrect: got=%f want=%f (+/- %f)" %
|
||||
(mount_pitch, despitch, despitch_tolerance))
|
||||
|
|
@ -5333,9 +5334,9 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
|
|||
p3=0, # stabilize pitch (unsupported)
|
||||
)
|
||||
|
||||
def test_mount_rc_targetting(self):
|
||||
def test_mount_rc_targetting(self, pitch_rc_neutral=1500, do_rate_tests=True):
|
||||
'''called in multipleplaces to make sure that mount RC targetting works'''
|
||||
try:
|
||||
if True:
|
||||
self.context_push()
|
||||
self.set_parameters({
|
||||
'RC6_OPTION': 0,
|
||||
|
|
@ -5395,6 +5396,13 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
|
|||
|
||||
self.set_rc(12, 1500)
|
||||
|
||||
if do_rate_tests:
|
||||
self.test_mount_rc_targetting_rate_control()
|
||||
|
||||
self.context_pop()
|
||||
|
||||
def test_mount_rc_targetting_rate_control(self, pitch_rc_neutral=1500):
|
||||
if True:
|
||||
self.progress("Testing RC rate control")
|
||||
self.set_parameter('MNT1_RC_RATE', 10)
|
||||
self.test_mount_pitch(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)
|
||||
|
|
@ -5417,46 +5425,21 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
|
|||
self.set_rc(12, 1500)
|
||||
self.test_mount_pitch(0, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)
|
||||
|
||||
self.context_pop()
|
||||
|
||||
except Exception as e:
|
||||
self.print_exception_caught(e)
|
||||
self.context_pop()
|
||||
raise e
|
||||
|
||||
def Mount(self):
|
||||
'''Test Camera/Antenna Mount'''
|
||||
ex = None
|
||||
self.context_push()
|
||||
old_srcSystem = self.mav.mav.srcSystem
|
||||
self.mav.mav.srcSystem = 250
|
||||
self.set_parameter("DISARM_DELAY", 0)
|
||||
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)
|
||||
|
||||
# setup mount parameters
|
||||
self.setup_servo_mount()
|
||||
self.reboot_sitl() # to handle MNT_TYPE changing
|
||||
|
||||
def mount_test_body(self, pitch_rc_neutral=1500, do_rate_tests=True, constrain_sysid_target=True):
|
||||
'''Test Camera/Antenna Mount - assumes a camera is set up and ready to go'''
|
||||
if True:
|
||||
# make sure we're getting gimbal device attitude status
|
||||
self.assert_receive_message('GIMBAL_DEVICE_ATTITUDE_STATUS', timeout=5)
|
||||
self.assert_receive_message('GIMBAL_DEVICE_ATTITUDE_STATUS', timeout=5, very_verbose=True)
|
||||
|
||||
# change mount to neutral mode (point forward, not stabilising)
|
||||
self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL)
|
||||
|
||||
# test pitch is not stabilising
|
||||
# test pitch is not neutral to start with
|
||||
mount_roll_deg, mount_pitch_deg, mount_yaw_deg = self.get_mount_roll_pitch_yaw_deg()
|
||||
if mount_roll_deg != 0 or mount_pitch_deg != 0 or mount_yaw_deg != 0:
|
||||
raise NotAchievedException("Mount stabilising when not requested")
|
||||
raise NotAchievedException("Mount not neutral")
|
||||
|
||||
self.change_mode('GUIDED')
|
||||
self.wait_ready_to_arm()
|
||||
self.arm_vehicle()
|
||||
|
||||
self.user_takeoff()
|
||||
self.takeoff(30, mode='GUIDED')
|
||||
|
||||
# pitch vehicle back and confirm gimbal is still not stabilising
|
||||
despitch = 10
|
||||
|
|
@ -5474,7 +5457,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
|
|||
|
||||
# center RC tilt control and change mount to RC_TARGETING mode
|
||||
self.progress("Gimbal to RC Targetting mode")
|
||||
self.set_rc(6, 1500)
|
||||
self.set_rc(6, pitch_rc_neutral)
|
||||
self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)
|
||||
|
||||
# pitch vehicle back and confirm gimbal is stabilising
|
||||
|
|
@ -5520,7 +5503,10 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
|
|||
self.progress("Testing mount RC targetting")
|
||||
|
||||
self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)
|
||||
self.test_mount_rc_targetting()
|
||||
self.test_mount_rc_targetting(
|
||||
pitch_rc_neutral=pitch_rc_neutral,
|
||||
do_rate_tests=do_rate_tests,
|
||||
)
|
||||
|
||||
self.progress("Testing mount ROI behaviour")
|
||||
self.test_mount_pitch(0, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)
|
||||
|
|
@ -5658,24 +5644,39 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
|
|||
0, # vz
|
||||
0 # heading
|
||||
)
|
||||
self.test_mount_pitch(68, 5, mavutil.mavlink.MAV_MOUNT_MODE_SYSID_TARGET, hold=2)
|
||||
self.test_mount_pitch(
|
||||
68,
|
||||
5,
|
||||
mavutil.mavlink.MAV_MOUNT_MODE_SYSID_TARGET,
|
||||
hold=2,
|
||||
constrained=constrain_sysid_target,
|
||||
)
|
||||
|
||||
self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL)
|
||||
self.test_mount_pitch(0, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL)
|
||||
|
||||
except Exception as e:
|
||||
self.print_exception_caught(e)
|
||||
ex = e
|
||||
self.disarm_vehicle(force=True)
|
||||
|
||||
self.mav.mav.srcSystem = old_srcSystem
|
||||
self.disarm_vehicle(force=True)
|
||||
def Mount(self):
|
||||
'''test servo mount'''
|
||||
self.setup_servo_mount()
|
||||
self.reboot_sitl() # to handle MNT_TYPE changing
|
||||
self.mount_test_body()
|
||||
|
||||
self.context_pop()
|
||||
|
||||
self.reboot_sitl() # to handle MNT1_TYPE changing
|
||||
|
||||
if ex is not None:
|
||||
raise ex
|
||||
def MountSolo(self):
|
||||
'''test type=2, a "Solo" mount'''
|
||||
self.set_parameters({
|
||||
"MNT1_TYPE": 2,
|
||||
"RC6_OPTION": 213, # MOUNT1_PITCH
|
||||
})
|
||||
self.customise_SITL_commandline([
|
||||
"--gimbal" # connects on port 5762
|
||||
])
|
||||
self.mount_test_body(
|
||||
pitch_rc_neutral=1818,
|
||||
do_rate_tests=False, # solo can't do rate control (yet?)
|
||||
constrain_sysid_target=False, # not everything constrains all angles
|
||||
)
|
||||
|
||||
def assert_mount_rpy(self, r, p, y, tolerance=1):
|
||||
'''assert mount atttiude in degrees'''
|
||||
|
|
@ -11730,6 +11731,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
|
|||
self.TerrainDBPreArm,
|
||||
self.ThrottleGainBoost,
|
||||
self.ScriptMountPOI,
|
||||
self.MountSolo,
|
||||
self.FlyMissionTwice,
|
||||
self.FlyMissionTwiceWithReset,
|
||||
self.MissionIndexValidity,
|
||||
|
|
|
|||
Loading…
Reference in New Issue