mirror of https://github.com/ArduPilot/ardupilot
Tools: copter autotest uses gimbal-device-attitude-status
add set_mount_mode to reduce duplication remove reliance on GIMBAL_REPORT slightly loosen accuracy of mount pitch test
This commit is contained in:
parent
7d901491a3
commit
9b357a5cd2
|
@ -12,6 +12,7 @@ import shutil
|
||||||
import time
|
import time
|
||||||
import numpy
|
import numpy
|
||||||
|
|
||||||
|
from pymavlink import quaternion
|
||||||
from pymavlink import mavutil
|
from pymavlink import mavutil
|
||||||
from pymavlink import mavextra
|
from pymavlink import mavextra
|
||||||
from pymavlink import rotmat
|
from pymavlink import rotmat
|
||||||
|
@ -4514,16 +4515,10 @@ class AutoTestCopter(AutoTest):
|
||||||
if now - tstart > timeout:
|
if now - tstart > timeout:
|
||||||
raise NotAchievedException("Mount pitch not achieved")
|
raise NotAchievedException("Mount pitch not achieved")
|
||||||
|
|
||||||
m = self.mav.recv_match(type='MOUNT_STATUS',
|
'''retrieve latest angles from GIMBAL_DEVICE_ATTITUDE_STATUS'''
|
||||||
blocking=True,
|
mount_roll, mount_pitch, mount_yaw = self.get_mount_roll_pitch_yaw_deg()
|
||||||
timeout=5)
|
|
||||||
|
|
||||||
if m.mount_mode != mount_mode:
|
self.progress("despitch=%f roll=%f pitch=%f yaw=%f" % (despitch, mount_roll, mount_pitch, mount_yaw))
|
||||||
raise NotAchievedException("MAV_MOUNT_MODE Not: %s" % (mount_mode))
|
|
||||||
|
|
||||||
# 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:
|
if abs(despitch - mount_pitch) > despitch_tolerance:
|
||||||
self.progress("Mount pitch incorrect: got=%f want=%f (+/- %f)" %
|
self.progress("Mount pitch incorrect: got=%f want=%f (+/- %f)" %
|
||||||
(mount_pitch, despitch, despitch_tolerance))
|
(mount_pitch, despitch, despitch_tolerance))
|
||||||
|
@ -4556,11 +4551,34 @@ class AutoTestCopter(AutoTest):
|
||||||
self.progress("Setting up servo mount")
|
self.progress("Setting up servo mount")
|
||||||
self.set_parameters({
|
self.set_parameters({
|
||||||
"MNT_TYPE": 1,
|
"MNT_TYPE": 1,
|
||||||
|
"MNT_STAB_ROLL": 1,
|
||||||
|
"MNT_STAB_TILT": 1,
|
||||||
|
"MNT_RC_IN_TILT": 6,
|
||||||
"SERVO%u_FUNCTION" % roll_servo: 8, # roll
|
"SERVO%u_FUNCTION" % roll_servo: 8, # roll
|
||||||
"SERVO%u_FUNCTION" % pitch_servo: 7, # pitch
|
"SERVO%u_FUNCTION" % pitch_servo: 7, # pitch
|
||||||
"SERVO%u_FUNCTION" % yaw_servo: 6, # yaw
|
"SERVO%u_FUNCTION" % yaw_servo: 6, # yaw
|
||||||
})
|
})
|
||||||
|
|
||||||
|
def get_mount_roll_pitch_yaw_deg(self):
|
||||||
|
'''return mount (aka gimbal) roll, pitch and yaw angles in degrees'''
|
||||||
|
# wait for gimbal attitude message
|
||||||
|
m = self.mav.recv_match(type='GIMBAL_DEVICE_ATTITUDE_STATUS',
|
||||||
|
blocking=True,
|
||||||
|
timeout=5)
|
||||||
|
|
||||||
|
# convert quaternion to euler angles and return
|
||||||
|
q = quaternion.Quaternion(m.q)
|
||||||
|
euler = q.euler
|
||||||
|
return math.degrees(euler[0]), math.degrees(euler[1]), math.degrees(euler[2])
|
||||||
|
|
||||||
|
def set_mount_mode(self, mount_mode):
|
||||||
|
'''set mount mode'''
|
||||||
|
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_MOUNT_CONFIGURE,
|
||||||
|
mount_mode,
|
||||||
|
1, # stabilize roll
|
||||||
|
1, # stabilize pitch
|
||||||
|
0, 0, 0, 0)
|
||||||
|
|
||||||
def test_mount(self):
|
def test_mount(self):
|
||||||
ex = None
|
ex = None
|
||||||
self.context_push()
|
self.context_push()
|
||||||
|
@ -4573,26 +4591,21 @@ class AutoTestCopter(AutoTest):
|
||||||
too long. This is probably a function of --speedup'''
|
too long. This is probably a function of --speedup'''
|
||||||
self.set_parameter("FS_GCS_ENABLE", 0)
|
self.set_parameter("FS_GCS_ENABLE", 0)
|
||||||
|
|
||||||
|
# setup mount parameters
|
||||||
self.setup_servo_mount()
|
self.setup_servo_mount()
|
||||||
self.reboot_sitl() # to handle MNT_TYPE changing
|
self.reboot_sitl() # to handle MNT_TYPE changing
|
||||||
|
|
||||||
# make sure we're getting mount status and gimbal reports
|
# make sure we're getting gimbal device attitude status
|
||||||
self.mav.recv_match(type='MOUNT_STATUS',
|
self.mav.recv_match(type='GIMBAL_DEVICE_ATTITUDE_STATUS',
|
||||||
blocking=True,
|
|
||||||
timeout=5)
|
|
||||||
self.mav.recv_match(type='GIMBAL_REPORT',
|
|
||||||
blocking=True,
|
blocking=True,
|
||||||
timeout=5)
|
timeout=5)
|
||||||
|
|
||||||
# test pitch isn't stabilising:
|
# change mount to neutral mode (point forward, not stabilising)
|
||||||
m = self.mav.recv_match(type='MOUNT_STATUS',
|
self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL)
|
||||||
blocking=True,
|
|
||||||
timeout=5)
|
|
||||||
|
|
||||||
if m.mount_mode != mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING:
|
# test pitch is not stabilising
|
||||||
raise NotAchievedException("Mount_Mode: Default Not MAV_MOUNT_MODE_RC_TARGETING")
|
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:
|
||||||
if m.pointing_a != 0 or m.pointing_b != 0 or m.pointing_c != 0:
|
|
||||||
raise NotAchievedException("Mount stabilising when not requested")
|
raise NotAchievedException("Mount stabilising when not requested")
|
||||||
|
|
||||||
self.change_mode('GUIDED')
|
self.change_mode('GUIDED')
|
||||||
|
@ -4601,6 +4614,7 @@ class AutoTestCopter(AutoTest):
|
||||||
|
|
||||||
self.user_takeoff()
|
self.user_takeoff()
|
||||||
|
|
||||||
|
# pitch vehicle back and confirm gimbal is still not stabilising
|
||||||
despitch = 10
|
despitch = 10
|
||||||
despitch_tolerance = 3
|
despitch_tolerance = 3
|
||||||
|
|
||||||
|
@ -4609,74 +4623,41 @@ class AutoTestCopter(AutoTest):
|
||||||
|
|
||||||
self.wait_pitch(despitch, despitch_tolerance)
|
self.wait_pitch(despitch, despitch_tolerance)
|
||||||
|
|
||||||
# check we haven't modified:
|
# check gimbal is still not stabilising
|
||||||
m = self.mav.recv_match(type='MOUNT_STATUS',
|
mount_roll_deg, mount_pitch_deg, mount_yaw_deg = self.get_mount_roll_pitch_yaw_deg()
|
||||||
blocking=True,
|
if mount_roll_deg != 0 or mount_pitch_deg != 0 or mount_yaw_deg != 0:
|
||||||
timeout=5)
|
|
||||||
|
|
||||||
if m.mount_mode != mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING:
|
|
||||||
raise NotAchievedException("Mount_Mode: changed when not requested")
|
|
||||||
|
|
||||||
if m.pointing_a != 0 or m.pointing_b != 0 or m.pointing_c != 0:
|
|
||||||
raise NotAchievedException("Mount stabilising when not requested")
|
raise NotAchievedException("Mount stabilising when not requested")
|
||||||
|
|
||||||
self.progress("Enable pitch stabilization using MOUNT_CONFIGURE")
|
# center RC tilt control and change mount to RC_TARGETING mode
|
||||||
self.mav.mav.mount_configure_send(
|
self.progress("Gimbal to RC Targetting mode")
|
||||||
1, # target system
|
self.set_rc(6, 1500)
|
||||||
1, # target component
|
self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)
|
||||||
mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING,
|
|
||||||
0, # stab-roll
|
|
||||||
1, # stab-pitch
|
|
||||||
0)
|
|
||||||
|
|
||||||
|
# pitch vehicle back and confirm gimbal is stabilising
|
||||||
|
self.progress("Pitching vehicle")
|
||||||
self.do_pitch(despitch)
|
self.do_pitch(despitch)
|
||||||
|
self.wait_pitch(despitch, despitch_tolerance)
|
||||||
self.test_mount_pitch(-despitch, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)
|
self.test_mount_pitch(-despitch, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)
|
||||||
|
|
||||||
self.progress("Disable pitch using MAV_CMD_DO_MOUNT_CONFIGURE")
|
# point gimbal at specified angle
|
||||||
self.do_pitch(despitch)
|
self.progress("Point gimbal using GIMBAL_MANAGER_PITCHYAW (ANGLE)")
|
||||||
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_MOUNT_CONFIGURE,
|
self.do_pitch(0) # level vehicle
|
||||||
mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING,
|
self.wait_pitch(0, despitch_tolerance)
|
||||||
0,
|
self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING)
|
||||||
0,
|
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW,
|
||||||
0,
|
-20, # pitch angle in degrees
|
||||||
0,
|
0, # yaw angle in degrees
|
||||||
0,
|
0, # pitch rate in degrees (NaN to ignore)
|
||||||
0,
|
0, # yaw rate in degrees (NaN to ignore)
|
||||||
)
|
0, # flags (0=Body-frame, 16/GIMBAL_MANAGER_FLAGS_YAW_LOCK=Earth Frame)
|
||||||
self.test_mount_pitch(0, 0, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)
|
0, # unused
|
||||||
|
0) # gimbal id
|
||||||
|
self.test_mount_pitch(-20, 1, mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING)
|
||||||
|
|
||||||
self.progress("Point somewhere using MOUNT_CONTROL (ANGLE)")
|
# point gimbal at specified location
|
||||||
|
self.progress("Point gimbal at Location using MOUNT_CONTROL (GPS)")
|
||||||
self.do_pitch(despitch)
|
self.do_pitch(despitch)
|
||||||
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_MOUNT_CONFIGURE,
|
self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT)
|
||||||
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
|
|
||||||
20 * 100, # pitch
|
|
||||||
20 * 100, # roll (centidegrees)
|
|
||||||
0, # yaw
|
|
||||||
0 # save position
|
|
||||||
)
|
|
||||||
self.test_mount_pitch(20, 1, mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING)
|
|
||||||
|
|
||||||
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,
|
|
||||||
)
|
|
||||||
|
|
||||||
# Delay here to allow the attitude to command to timeout and level out the copter a bit
|
# Delay here to allow the attitude to command to timeout and level out the copter a bit
|
||||||
self.delay_sim_time(3)
|
self.delay_sim_time(3)
|
||||||
|
@ -4715,15 +4696,8 @@ class AutoTestCopter(AutoTest):
|
||||||
0, # yaw rate (rad/s)
|
0, # yaw rate (rad/s)
|
||||||
0.5) # thrust, 0 to 1, translated to a climb/descent rate
|
0.5) # thrust, 0 to 1, translated to a climb/descent rate
|
||||||
|
|
||||||
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_MOUNT_CONFIGURE,
|
self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)
|
||||||
mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING,
|
|
||||||
0,
|
|
||||||
0,
|
|
||||||
0,
|
|
||||||
0,
|
|
||||||
0,
|
|
||||||
0,
|
|
||||||
)
|
|
||||||
try:
|
try:
|
||||||
self.context_push()
|
self.context_push()
|
||||||
self.set_parameters({
|
self.set_parameters({
|
||||||
|
@ -4768,11 +4742,11 @@ class AutoTestCopter(AutoTest):
|
||||||
"MNT_ANGMAX_TIL": 1000,
|
"MNT_ANGMAX_TIL": 1000,
|
||||||
})
|
})
|
||||||
self.set_rc(12, 1000)
|
self.set_rc(12, 1000)
|
||||||
self.test_mount_pitch(-90.00, 0.01, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)
|
self.test_mount_pitch(-90.00, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)
|
||||||
self.set_rc(12, 2000)
|
self.set_rc(12, 2000)
|
||||||
self.test_mount_pitch(10.00, 0.01, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)
|
self.test_mount_pitch(10.00, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)
|
||||||
self.set_rc(12, 1500)
|
self.set_rc(12, 1500)
|
||||||
self.test_mount_pitch(-40.00, 0.01, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)
|
self.test_mount_pitch(-40.00, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)
|
||||||
finally:
|
finally:
|
||||||
self.context_pop()
|
self.context_pop()
|
||||||
|
|
||||||
|
@ -4878,15 +4852,7 @@ class AutoTestCopter(AutoTest):
|
||||||
)
|
)
|
||||||
self.test_mount_pitch(-70, 1, mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT, hold=2)
|
self.test_mount_pitch(-70, 1, mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT, hold=2)
|
||||||
|
|
||||||
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_MOUNT_CONFIGURE,
|
self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL)
|
||||||
mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL,
|
|
||||||
0,
|
|
||||||
0,
|
|
||||||
0,
|
|
||||||
0,
|
|
||||||
0,
|
|
||||||
0,
|
|
||||||
)
|
|
||||||
self.test_mount_pitch(0, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL)
|
self.test_mount_pitch(0, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL)
|
||||||
|
|
||||||
self.progress("Testing mount roi-sysid behaviour")
|
self.progress("Testing mount roi-sysid behaviour")
|
||||||
|
@ -4934,15 +4900,7 @@ class AutoTestCopter(AutoTest):
|
||||||
)
|
)
|
||||||
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)
|
||||||
|
|
||||||
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_MOUNT_CONFIGURE,
|
self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL)
|
||||||
mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL,
|
|
||||||
0,
|
|
||||||
0,
|
|
||||||
0,
|
|
||||||
0,
|
|
||||||
0,
|
|
||||||
0,
|
|
||||||
)
|
|
||||||
self.test_mount_pitch(0, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL)
|
self.test_mount_pitch(0, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL)
|
||||||
|
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
|
|
Loading…
Reference in New Issue