Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Autotest: use MAV_POS_TARGET_TYPE_MASK, remove POS_TYPE_MASK_FORCE

Browse Source
This commit is contained in:
Josh Henderson 2021-07-26 04:51:42 -04:00 committed by Andrew Tridgell
parent 4559274755
commit 162b3b70cf
2 changed files with 71 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
Tools/autotest/arducopter.py
View File

@ -24,6 +24,7 @@ from pysim import vehicleinfo
from common import AutoTest from common import AutoTest
from common import NotAchievedException, AutoTestTimeoutException, PreconditionFailedException from common import NotAchievedException, AutoTestTimeoutException, PreconditionFailedException
from common import Test from common import Test
from common import MAV_POS_TARGET_TYPE_MASK
from pymavlink.rotmat import Vector3 from pymavlink.rotmat import Vector3
@ -3515,7 +3516,7 @@ class AutoTestCopter(AutoTest):
1, # target system_id 1, # target system_id
1, # target component id 1, # target component id
mavutil.mavlink.MAV_FRAME_BODY_NED, mavutil.mavlink.MAV_FRAME_BODY_NED,
0b1111111111111000, # mask specifying use-only-x-y-z MAV_POS_TARGET_TYPE_MASK.POS_ONLY | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE, # mask specifying use-only-x-y-z
0, # x 0, # x
0, # y 0, # y
0, # z 0, # z
@ -3546,7 +3547,7 @@ class AutoTestCopter(AutoTest):
1, # target system_id 1, # target system_id
1, # target component id 1, # target component id
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
0b1111111111111000, # mask specifying use-only-lat-lon-alt MAV_POS_TARGET_TYPE_MASK.POS_ONLY, # mask specifying use-only-lat-lon-alt
lat, # lat lat, # lat
lon, # lon lon, # lon
alt, # alt alt, # alt
@ -3584,7 +3585,7 @@ class AutoTestCopter(AutoTest):
1, # target system_id 1, # target system_id
1, # target component id 1, # target component id
mavutil.mavlink.MAV_FRAME_LOCAL_NED, mavutil.mavlink.MAV_FRAME_LOCAL_NED,
0b1111111111111000, # mask specifying use-only-x-y-z MAV_POS_TARGET_TYPE_MASK.POS_ONLY | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE, # mask specifying use-only-x-y-z
x, # x x, # x
y, # y y, # y
-z_up,# z -z_up,# z
@ -3609,13 +3610,17 @@ class AutoTestCopter(AutoTest):
self.progress("Setting local target in NED: (%f, %f, %f)" % (x, y, -z_up)) self.progress("Setting local target in NED: (%f, %f, %f)" % (x, y, -z_up))
self.progress("Setting rate to 1 Hz") self.progress("Setting rate to 1 Hz")
self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, 1) self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, 1)
# mask specifying use only xyz
target_typemask = MAV_POS_TARGET_TYPE_MASK.POS_ONLY
# set position target # set position target
self.mav.mav.set_position_target_local_ned_send( self.mav.mav.set_position_target_local_ned_send(
0, # timestamp 0, # timestamp
1, # target system_id 1, # target system_id
1, # target component id 1, # target component id
mavutil.mavlink.MAV_FRAME_LOCAL_NED, mavutil.mavlink.MAV_FRAME_LOCAL_NED,
0b1111111111111000, # mask specifying use only xyz target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE,
x, # x x, # x
y, # y y, # y
-z_up, # z -z_up, # z
@ -3631,8 +3636,9 @@ class AutoTestCopter(AutoTest):
m = self.mav.recv_match(type='POSITION_TARGET_LOCAL_NED', blocking=True, timeout=2) m = self.mav.recv_match(type='POSITION_TARGET_LOCAL_NED', blocking=True, timeout=2)
self.progress("Received local target: %s" % str(m)) self.progress("Received local target: %s" % str(m))
if not (m.type_mask == 0xFFF8 or m.type_mask == 0x0FF8): if not (m.type_mask == (target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE) or m.type_mask == target_typemask):
raise NotAchievedException("Did not receive proper mask: expected=65528 or 4088, got=%u" % m.type_mask) raise NotAchievedException("Did not receive proper mask: expected=%u or %u, got=%u" %
((target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE), target_typemask, m.type_mask))
if x - m.x > 0.1: if x - m.x > 0.1:
raise NotAchievedException("Did not receive proper target position x: wanted=%f got=%f" % (x, m.x)) raise NotAchievedException("Did not receive proper target position x: wanted=%f got=%f" % (x, m.x))
@ -3649,6 +3655,11 @@ class AutoTestCopter(AutoTest):
self.progress("Setting POSITION_TARGET_LOCAL_NED message rate to 10Hz") self.progress("Setting POSITION_TARGET_LOCAL_NED message rate to 10Hz")
self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, 10) self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, 10)
# mask specifying use only vx,vy,vz & accel. Even though we don't test acceltargets below currently
# a velocity only mask returns a velocity & accel mask
target_typemask = (MAV_POS_TARGET_TYPE_MASK.POS_IGNORE |
MAV_POS_TARGET_TYPE_MASK.YAW_IGNORE | MAV_POS_TARGET_TYPE_MASK.YAW_RATE_IGNORE)
# Drain old messages and ignore the ramp-up to the required target velocity # Drain old messages and ignore the ramp-up to the required target velocity
tstart = self.get_sim_time() tstart = self.get_sim_time()
while self.get_sim_time_cached() - tstart < timeout: while self.get_sim_time_cached() - tstart < timeout:
@ -3658,7 +3669,7 @@ class AutoTestCopter(AutoTest):
1, # target system_id 1, # target system_id
1, # target component id 1, # target component id
mavutil.mavlink.MAV_FRAME_LOCAL_NED, mavutil.mavlink.MAV_FRAME_LOCAL_NED,
0b1111111111000111, # mask specifying use only vx,vy,vz target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE,
0, # x 0, # x
0, # y 0, # y
0, # z 0, # z
@ -3679,8 +3690,9 @@ class AutoTestCopter(AutoTest):
self.progress("Received local target: %s" % str(m)) self.progress("Received local target: %s" % str(m))
# Check the last received message # Check the last received message
if not (m.type_mask == 0xFE07 or m.type_mask == 0x0E07): if not (m.type_mask == (target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE) or m.type_mask == target_typemask):
raise NotAchievedException("Did not receive proper mask: expected=65031 or 3591, got=%u" % m.type_mask) raise NotAchievedException("Did not receive proper mask: expected=%u or %u, got=%u" %
((target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE), target_typemask, m.type_mask))
if vx - m.vx > 0.1: if vx - m.vx > 0.1:
raise NotAchievedException("Did not receive proper target velocity vx: wanted=%f got=%f" % (vx, m.vx)) raise NotAchievedException("Did not receive proper target velocity vx: wanted=%f got=%f" % (vx, m.vx))
@ -4413,7 +4425,7 @@ class AutoTestCopter(AutoTest):
1, # target system_id 1, # target system_id
1, # target component id 1, # target component id
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
0b1111111111111000, # mask specifying use-only-lat-lon-alt MAV_POS_TARGET_TYPE_MASK.POS_ONLY | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE, # mask specifying use-only-lat-lon-alt
there.lat, # lat there.lat, # lat
there.lng, # lon there.lng, # lon
there.alt, # alt there.alt, # alt
@ -5016,7 +5028,7 @@ class AutoTestCopter(AutoTest):
1, # target system_id 1, # target system_id
1, # target component id 1, # target component id
mavutil.mavlink.MAV_FRAME_GLOBAL_INT, mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
0b1111111111111000, # mask specifying use-only-lat-lon-alt MAV_POS_TARGET_TYPE_MASK.POS_ONLY | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE, # mask specifying use-only-lat-lon-alt
int(destination.lat * 1e7), # lat int(destination.lat * 1e7), # lat
int(destination.lng * 1e7), # lon int(destination.lng * 1e7), # lon
destination.alt, # alt destination.alt, # alt

98
Tools/autotest/common.py
View File

@ -25,6 +25,7 @@ import socket
import struct import struct
import random import random
import threading import threading
import enum
from MAVProxy.modules.lib import mp_util from MAVProxy.modules.lib import mp_util
@ -41,18 +42,24 @@ try:
except ImportError: except ImportError:
import Queue import Queue
MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE = (mavutil.mavlink.POSITION_TARGET_TYPEMASK_X_IGNORE |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_Y_IGNORE | # Enumeration convenience class for mavlink POSITION_TARGET_TYPEMASK
mavutil.mavlink.POSITION_TARGET_TYPEMASK_Z_IGNORE) class MAV_POS_TARGET_TYPE_MASK(enum.IntEnum):
MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE = (mavutil.mavlink.POSITION_TARGET_TYPEMASK_VX_IGNORE | POS_IGNORE = (mavutil.mavlink.POSITION_TARGET_TYPEMASK_X_IGNORE |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_VY_IGNORE | mavutil.mavlink.POSITION_TARGET_TYPEMASK_Y_IGNORE |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_VZ_IGNORE) mavutil.mavlink.POSITION_TARGET_TYPEMASK_Z_IGNORE)
MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE = (mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE | VEL_IGNORE = (mavutil.mavlink.POSITION_TARGET_TYPEMASK_VX_IGNORE |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE | mavutil.mavlink.POSITION_TARGET_TYPEMASK_VY_IGNORE |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE) mavutil.mavlink.POSITION_TARGET_TYPEMASK_VZ_IGNORE)
MAVLINK_SET_POS_TYPE_MASK_FORCE = mavutil.mavlink.POSITION_TARGET_TYPEMASK_FORCE_SET ACC_IGNORE = (mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE |
MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE = mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE |
MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE = mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE)
FORCE_SET = mavutil.mavlink.POSITION_TARGET_TYPEMASK_FORCE_SET
YAW_IGNORE = mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE
YAW_RATE_IGNORE = mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE
POS_ONLY = VEL_IGNORE | ACC_IGNORE | YAW_IGNORE | YAW_RATE_IGNORE
LAST_BYTE = 0xF000
MAV_FRAMES_TO_TEST = [ MAV_FRAMES_TO_TEST = [
mavutil.mavlink.MAV_FRAME_GLOBAL, mavutil.mavlink.MAV_FRAME_GLOBAL,
@ -8116,11 +8123,10 @@ Also, ignores heartbeats not from our target system'''
self.sysid_thismav(), # target system_id self.sysid_thismav(), # target system_id
1, # target component id 1, # target component id
mav_frame, mav_frame,
MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE | MAV_POS_TARGET_TYPE_MASK.VEL_IGNORE |
MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE | MAV_POS_TARGET_TYPE_MASK.ACC_IGNORE |
MAVLINK_SET_POS_TYPE_MASK_FORCE | MAV_POS_TARGET_TYPE_MASK.YAW_IGNORE |
MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE | MAV_POS_TARGET_TYPE_MASK.YAW_RATE_IGNORE,
MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE,
int(lat * 1.0e7), # lat int(lat * 1.0e7), # lat
int(lng * 1.0e7), # lon int(lng * 1.0e7), # lon
alt, # alt alt, # alt
@ -8184,10 +8190,9 @@ Also, ignores heartbeats not from our target system'''
self.sysid_thismav(), # target system_id self.sysid_thismav(), # target system_id
1, # target component id 1, # target component id
frame, frame,
MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE | MAV_POS_TARGET_TYPE_MASK.VEL_IGNORE |
MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE | MAV_POS_TARGET_TYPE_MASK.ACC_IGNORE |
MAVLINK_SET_POS_TYPE_MASK_FORCE | MAV_POS_TARGET_TYPE_MASK.YAW_RATE_IGNORE,
MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE,
int(targetpos.lat * 1.0e7), # lat int(targetpos.lat * 1.0e7), # lat
int(targetpos.lng * 1.0e7), # lon int(targetpos.lng * 1.0e7), # lon
to_alt_frame(targetpos.alt, frame_name), # alt to_alt_frame(targetpos.alt, frame_name), # alt
@ -8214,10 +8219,9 @@ Also, ignores heartbeats not from our target system'''
self.sysid_thismav(), # target system_id self.sysid_thismav(), # target system_id
1, # target component id 1, # target component id
frame, frame,
MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE | MAV_POS_TARGET_TYPE_MASK.VEL_IGNORE |
MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE | MAV_POS_TARGET_TYPE_MASK.ACC_IGNORE |
MAVLINK_SET_POS_TYPE_MASK_FORCE | MAV_POS_TARGET_TYPE_MASK.YAW_RATE_IGNORE,
MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE,
int(targetpos.lat * 1.0e7), # lat int(targetpos.lat * 1.0e7), # lat
int(targetpos.lng * 1.0e7), # lon int(targetpos.lng * 1.0e7), # lon
to_alt_frame(targetpos.alt, frame_name), # alt to_alt_frame(targetpos.alt, frame_name), # alt
@ -8244,10 +8248,9 @@ Also, ignores heartbeats not from our target system'''
self.sysid_thismav(), # target system_id self.sysid_thismav(), # target system_id
1, # target component id 1, # target component id
frame, frame,
MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE | MAV_POS_TARGET_TYPE_MASK.VEL_IGNORE |
MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE | MAV_POS_TARGET_TYPE_MASK.ACC_IGNORE |
MAVLINK_SET_POS_TYPE_MASK_FORCE | MAV_POS_TARGET_TYPE_MASK.YAW_IGNORE,
MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE,
int(targetpos.lat * 1.0e7), # lat int(targetpos.lat * 1.0e7), # lat
int(targetpos.lng * 1.0e7), # lon int(targetpos.lng * 1.0e7), # lon
to_alt_frame(targetpos.alt, frame_name), # alt to_alt_frame(targetpos.alt, frame_name), # alt
@ -8338,11 +8341,10 @@ Also, ignores heartbeats not from our target system'''
self.sysid_thismav(), # target system_id self.sysid_thismav(), # target system_id
1, # target component id 1, # target component id
mav_frame, mav_frame,
MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE | MAV_POS_TARGET_TYPE_MASK.POS_IGNORE |
MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE | MAV_POS_TARGET_TYPE_MASK.ACC_IGNORE |
MAVLINK_SET_POS_TYPE_MASK_FORCE | MAV_POS_TARGET_TYPE_MASK.YAW_IGNORE |
MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE | MAV_POS_TARGET_TYPE_MASK.YAW_RATE_IGNORE,
MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE,
0, 0,
0, 0,
0, 0,
@ -8436,10 +8438,9 @@ Also, ignores heartbeats not from our target system'''
self.sysid_thismav(), # target system_id self.sysid_thismav(), # target system_id
1, # target component id 1, # target component id
mav_frame, mav_frame,
MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE | MAV_POS_TARGET_TYPE_MASK.POS_IGNORE |
MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE | MAV_POS_TARGET_TYPE_MASK.ACC_IGNORE |
MAVLINK_SET_POS_TYPE_MASK_FORCE | MAV_POS_TARGET_TYPE_MASK.YAW_RATE_IGNORE,
MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE,
0, 0,
0, 0,
0, 0,
@ -8466,10 +8467,9 @@ Also, ignores heartbeats not from our target system'''
self.sysid_thismav(), # target system_id self.sysid_thismav(), # target system_id
1, # target component id 1, # target component id
mav_frame, mav_frame,
MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE | MAV_POS_TARGET_TYPE_MASK.POS_IGNORE |
MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE | MAV_POS_TARGET_TYPE_MASK.ACC_IGNORE |
MAVLINK_SET_POS_TYPE_MASK_FORCE | MAV_POS_TARGET_TYPE_MASK.YAW_RATE_IGNORE,
MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE,
0, 0,
0, 0,
0, 0,
@ -8535,10 +8535,9 @@ Also, ignores heartbeats not from our target system'''
self.sysid_thismav(), # target system_id self.sysid_thismav(), # target system_id
1, # target component id 1, # target component id
mav_frame, mav_frame,
MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE | MAV_POS_TARGET_TYPE_MASK.POS_IGNORE |
MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE | MAV_POS_TARGET_TYPE_MASK.ACC_IGNORE |
MAVLINK_SET_POS_TYPE_MASK_FORCE | MAV_POS_TARGET_TYPE_MASK.YAW_IGNORE,
MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE,
0, 0,
0, 0,
0, 0,
@ -8565,10 +8564,9 @@ Also, ignores heartbeats not from our target system'''
self.sysid_thismav(), # target system_id self.sysid_thismav(), # target system_id
1, # target component id 1, # target component id
mav_frame, mav_frame,
MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE | MAV_POS_TARGET_TYPE_MASK.POS_IGNORE |
MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE | MAV_POS_TARGET_TYPE_MASK.ACC_IGNORE |
MAVLINK_SET_POS_TYPE_MASK_FORCE | MAV_POS_TARGET_TYPE_MASK.YAW_IGNORE,
MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE,
0, 0,
0, 0,
0, 0,