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
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 380
      Tools/autotest/arducopter.py

380
Tools/autotest/arducopter.py
Unescape View File

@ -9,9 +9,10 @@ import time @@ -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,32 +1703,10 @@ class AutoTestCopter(AutoTest): @@ -1702,32 +1703,10 @@ class AutoTestCopter(AutoTest):
if ex is not None:
raise ex
def fly_guided_change_submode(self):
""""Ensure we can move around in guided after a takeoff command."""
self.context_push()
ex = None
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.mavproxy.send('mode guided\n') # stabilize mode
self.wait_mode('GUIDED')
self.wait_ready_to_arm()
self.arm_vehicle()
self.user_takeoff(alt_min=10)
def fly_guided_move_relative(self, lat, lon, alt):
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:
@ -1739,9 +1718,9 @@ class AutoTestCopter(AutoTest): @@ -1739,9 +1718,9 @@ class AutoTestCopter(AutoTest):
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
lat, # lat
lon, # lon
alt, # alt
0, # vx
0, # vy
0, # vz
@ -1758,6 +1737,31 @@ class AutoTestCopter(AutoTest): @@ -1758,6 +1737,31 @@ class AutoTestCopter(AutoTest):
if delta > 10:
break
def fly_guided_change_submode(self):
""""Ensure we can move around in guided after a takeoff command."""
self.context_push()
ex = None
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.mavproxy.send('mode guided\n') # stabilize mode
self.wait_mode('GUIDED')
self.wait_ready_to_arm()
self.arm_vehicle()
self.user_takeoff(alt_min=10)
"""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"""
self.fly_guided_move_relative(5, 5, 10)
self.progress("Landing")
self.mavproxy.send('switch 2\n') # land mode
self.wait_mode('LAND')
@ -1796,6 +1800,325 @@ class AutoTestCopter(AutoTest): @@ -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): @@ -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(

Write Preview
Loading…
Cancel
Save