mirror of https://github.com/ArduPilot/ardupilot
Tools: autotest: add tests for AP_Mount
This commit is contained in:
Peter Barker
Peter Barker
parent
27b444f4e8
commit
432b1fda3a
|
|
@ -9,9 +9,10 @@ import time
|
|||
|
||||
import pexpect
|
||||
from pymavlink import mavutil
|
||||
from pymavlink import mavextra
|
||||
from pymavlink import quaternion
|
||||
|
||||
from pysim import util
|
||||
|
||||
from common import AutoTest
|
||||
from common import NotAchievedException, AutoTestTimeoutException, PreconditionFailedException
|
||||
|
||||
|
|
@ -1702,6 +1703,40 @@ class AutoTestCopter(AutoTest):
|
|||
if ex is not None:
|
||||
raise ex
|
||||
|
||||
def fly_guided_move_relative(self, lat, lon, alt):
|
||||
startpos = self.mav.recv_match(type='GLOBAL_POSITION_INT',
|
||||
blocking=True)
|
||||
|
||||
tstart = self.get_sim_time()
|
||||
while True:
|
||||
if self.get_sim_time() - tstart > 200:
|
||||
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
|
||||
|
||||
def fly_guided_change_submode(self):
|
||||
""""Ensure we can move around in guided after a takeoff command."""
|
||||
|
||||
|
|
@ -1720,43 +1755,12 @@ class AutoTestCopter(AutoTest):
|
|||
|
||||
self.user_takeoff(alt_min=10)
|
||||
|
||||
startpos = self.mav.recv_match(type='GLOBAL_POSITION_INT',
|
||||
blocking=True)
|
||||
|
||||
"""yaw through absolute angles using MAV_CMD_CONDITION_YAW"""
|
||||
self.guided_achieve_heading(45)
|
||||
self.guided_achieve_heading(135)
|
||||
|
||||
"""move the vehicle using set_position_target_global_int"""
|
||||
tstart = self.get_sim_time()
|
||||
while True:
|
||||
if self.get_sim_time() - tstart > 200:
|
||||
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
|
||||
5, # lat
|
||||
5, # lon
|
||||
10, # 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
|
||||
self.fly_guided_move_relative(5, 5, 10)
|
||||
|
||||
self.progress("Landing")
|
||||
self.mavproxy.send('switch 2\n') # land mode
|
||||
|
|
@ -1796,6 +1800,325 @@ class AutoTestCopter(AutoTest):
|
|||
if ex is not None:
|
||||
raise ex
|
||||
|
||||
def test_mount_pitch(self, despitch, despitch_tolerance, timeout=5):
|
||||
tstart = self.get_sim_time()
|
||||
while True:
|
||||
if self.get_sim_time() - tstart > timeout:
|
||||
raise NotAchievedException()
|
||||
|
||||
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:
|
||||
self.progress("Mount pitch incorrect: %f != %f" %
|
||||
(mount_pitch, despitch))
|
||||
continue
|
||||
self.progress("Mount pitch correct: %f degrees == %f" %
|
||||
(mount_pitch, despitch))
|
||||
return
|
||||
|
||||
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()
|
||||
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:
|
||||
self.progress("Stabilising when not requested")
|
||||
raise NotAchievedException()
|
||||
|
||||
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:
|
||||
self.progress("Stabilising when not requested")
|
||||
raise NotAchievedException()
|
||||
|
||||
self.progress("Enable pitch stabilization using MOUNT_CONFIGURE")
|
||||
self.do_pitch(despitch)
|
||||
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)
|
||||
|
||||
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
|
||||
20 *100, # pitch
|
||||
20 *100, # roll (centidegrees)
|
||||
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
|
||||
t_lat * 1e7, # lat
|
||||
t_lon * 1e7, # lon
|
||||
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")
|
||||
self.set_rc(11, 1500)
|
||||
self.set_rc(12, 1500)
|
||||
self.set_rc(13, 1500)
|
||||
self.test_mount_pitch(0, 1)
|
||||
self.set_rc(12, 1400)
|
||||
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)
|
||||
|
||||
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()
|
||||
|
||||
except Exception:
|
||||
self.context_pop()
|
||||
raise
|
||||
|
||||
self.progress("Testing mount ROI 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_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)
|
||||
|
||||
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")
|
||||
self.wait_seconds(5)
|
||||
start = self.mav.location()
|
||||
self.progress("Moving to guided/position controller")
|
||||
self.fly_guided_move_relative(0, 0, 0)
|
||||
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()
|
||||
|
||||
self.reboot_sitl() # to handle MNT_TYPE changing
|
||||
|
||||
if ex is not None:
|
||||
raise ex
|
||||
|
||||
def autotest(self):
|
||||
"""Autotest ArduCopter in SITL."""
|
||||
self.check_test_syntax(test_file=os.path.realpath(__file__))
|
||||
|
|
@ -2016,6 +2339,9 @@ class AutoTestCopter(AutoTest):
|
|||
'''vision position''' # expects vehicle to be disarmed
|
||||
self.run_test("Fly Vision Position", self.fly_vision_position)
|
||||
|
||||
'''tests for camera/antenna mount'''
|
||||
self.run_test("Test Mount", self.test_mount)
|
||||
|
||||
# Download logs
|
||||
self.run_test("log download",
|
||||
lambda: self.log_download(
|
||||
|
|
|
|||
Loading…
Reference in New Issue