zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Tools: autotest: add tests for AP_Mount

mission-4.1.18
Peter Barker 7 years ago committed by Peter Barker
parent
27b444f4e8
commit
432b1fda3a
1 changed files with 359 additions and 33 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 392
      Tools/autotest/arducopter.py

392
Tools/autotest/arducopter.py
Unescape View File

@ -9,9 +9,10 @@ import time
import pexpect import pexpect
from pymavlink import mavutil from pymavlink import mavutil
from pymavlink import mavextra
from pymavlink import quaternion
from pysim import util from pysim import util
from common import AutoTest from common import AutoTest
from common import NotAchievedException, AutoTestTimeoutException, PreconditionFailedException from common import NotAchievedException, AutoTestTimeoutException, PreconditionFailedException
@ -1702,6 +1703,40 @@ class AutoTestCopter(AutoTest):
if ex is not None: if ex is not None:
raise ex 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): def fly_guided_change_submode(self):
""""Ensure we can move around in guided after a takeoff command.""" """"Ensure we can move around in guided after a takeoff command."""
@ -1720,43 +1755,12 @@ class AutoTestCopter(AutoTest):
self.user_takeoff(alt_min=10) 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""" """yaw through absolute angles using MAV_CMD_CONDITION_YAW"""
self.guided_achieve_heading(45) self.guided_achieve_heading(45)
self.guided_achieve_heading(135) self.guided_achieve_heading(135)
"""move the vehicle using set_position_target_global_int""" """move the vehicle using set_position_target_global_int"""
tstart = self.get_sim_time() self.fly_guided_move_relative(5, 5, 10)
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.progress("Landing") self.progress("Landing")
self.mavproxy.send('switch 2\n') # land mode self.mavproxy.send('switch 2\n') # land mode
@ -1796,6 +1800,325 @@ class AutoTestCopter(AutoTest):
if ex is not None: if ex is not None:
raise ex 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): def autotest(self):
"""Autotest ArduCopter in SITL.""" """Autotest ArduCopter in SITL."""
self.check_test_syntax(test_file=os.path.realpath(__file__)) self.check_test_syntax(test_file=os.path.realpath(__file__))
@ -2016,6 +2339,9 @@ class AutoTestCopter(AutoTest):
'''vision position''' # expects vehicle to be disarmed '''vision position''' # expects vehicle to be disarmed
self.run_test("Fly Vision Position", self.fly_vision_position) 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 # Download logs
self.run_test("log download", self.run_test("log download",
lambda: self.log_download( lambda: self.log_download(

Write Preview
Loading…
Cancel
Save