mirror of https://github.com/ArduPilot/ardupilot
Autotest: use MAV_POS_TARGET_TYPE_MASK, remove POS_TYPE_MASK_FORCE
This commit is contained in:
Josh Henderson
Andrew Tridgell
parent
4559274755
commit
162b3b70cf
|
|
@ -24,6 +24,7 @@ from pysim import vehicleinfo
|
|||
from common import AutoTest
|
||||
from common import NotAchievedException, AutoTestTimeoutException, PreconditionFailedException
|
||||
from common import Test
|
||||
from common import MAV_POS_TARGET_TYPE_MASK
|
||||
|
||||
from pymavlink.rotmat import Vector3
|
||||
|
||||
|
|
@ -3515,7 +3516,7 @@ class AutoTestCopter(AutoTest):
|
|||
1, # target system_id
|
||||
1, # target component id
|
||||
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, # y
|
||||
0, # z
|
||||
|
|
@ -3546,7 +3547,7 @@ class AutoTestCopter(AutoTest):
|
|||
1, # target system_id
|
||||
1, # target component id
|
||||
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
|
||||
lon, # lon
|
||||
alt, # alt
|
||||
|
|
@ -3584,7 +3585,7 @@ class AutoTestCopter(AutoTest):
|
|||
1, # target system_id
|
||||
1, # target component id
|
||||
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
|
||||
y, # y
|
||||
-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 rate to 1 Hz")
|
||||
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
|
||||
self.mav.mav.set_position_target_local_ned_send(
|
||||
0, # timestamp
|
||||
1, # target system_id
|
||||
1, # target component id
|
||||
mavutil.mavlink.MAV_FRAME_LOCAL_NED,
|
||||
0b1111111111111000, # mask specifying use only xyz
|
||||
target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE,
|
||||
x, # x
|
||||
y, # y
|
||||
-z_up, # z
|
||||
|
|
@ -3631,8 +3636,9 @@ class AutoTestCopter(AutoTest):
|
|||
m = self.mav.recv_match(type='POSITION_TARGET_LOCAL_NED', blocking=True, timeout=2)
|
||||
self.progress("Received local target: %s" % str(m))
|
||||
|
||||
if not (m.type_mask == 0xFFF8 or m.type_mask == 0x0FF8):
|
||||
raise NotAchievedException("Did not receive proper mask: expected=65528 or 4088, got=%u" % m.type_mask)
|
||||
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=%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:
|
||||
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.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
|
||||
tstart = self.get_sim_time()
|
||||
while self.get_sim_time_cached() - tstart < timeout:
|
||||
|
|
@ -3658,7 +3669,7 @@ class AutoTestCopter(AutoTest):
|
|||
1, # target system_id
|
||||
1, # target component id
|
||||
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, # y
|
||||
0, # z
|
||||
|
|
@ -3679,8 +3690,9 @@ class AutoTestCopter(AutoTest):
|
|||
self.progress("Received local target: %s" % str(m))
|
||||
|
||||
# Check the last received message
|
||||
if not (m.type_mask == 0xFE07 or m.type_mask == 0x0E07):
|
||||
raise NotAchievedException("Did not receive proper mask: expected=65031 or 3591, got=%u" % m.type_mask)
|
||||
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=%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:
|
||||
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 component id
|
||||
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.lng, # lon
|
||||
there.alt, # alt
|
||||
|
|
@ -5016,7 +5028,7 @@ class AutoTestCopter(AutoTest):
|
|||
1, # target system_id
|
||||
1, # target component id
|
||||
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.lng * 1e7), # lon
|
||||
destination.alt, # alt
|
||||
|
|
|
|||
|
|
@ -25,6 +25,7 @@ import socket
|
|||
import struct
|
||||
import random
|
||||
import threading
|
||||
import enum
|
||||
|
||||
from MAVProxy.modules.lib import mp_util
|
||||
|
||||
|
|
@ -41,18 +42,24 @@ try:
|
|||
except ImportError:
|
||||
import Queue
|
||||
|
||||
MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE = (mavutil.mavlink.POSITION_TARGET_TYPEMASK_X_IGNORE |
|
||||
mavutil.mavlink.POSITION_TARGET_TYPEMASK_Y_IGNORE |
|
||||
mavutil.mavlink.POSITION_TARGET_TYPEMASK_Z_IGNORE)
|
||||
MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE = (mavutil.mavlink.POSITION_TARGET_TYPEMASK_VX_IGNORE |
|
||||
mavutil.mavlink.POSITION_TARGET_TYPEMASK_VY_IGNORE |
|
||||
mavutil.mavlink.POSITION_TARGET_TYPEMASK_VZ_IGNORE)
|
||||
MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE = (mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE |
|
||||
mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE |
|
||||
mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE)
|
||||
MAVLINK_SET_POS_TYPE_MASK_FORCE = mavutil.mavlink.POSITION_TARGET_TYPEMASK_FORCE_SET
|
||||
MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE = mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE
|
||||
MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE = mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE
|
||||
|
||||
# Enumeration convenience class for mavlink POSITION_TARGET_TYPEMASK
|
||||
class MAV_POS_TARGET_TYPE_MASK(enum.IntEnum):
|
||||
POS_IGNORE = (mavutil.mavlink.POSITION_TARGET_TYPEMASK_X_IGNORE |
|
||||
mavutil.mavlink.POSITION_TARGET_TYPEMASK_Y_IGNORE |
|
||||
mavutil.mavlink.POSITION_TARGET_TYPEMASK_Z_IGNORE)
|
||||
VEL_IGNORE = (mavutil.mavlink.POSITION_TARGET_TYPEMASK_VX_IGNORE |
|
||||
mavutil.mavlink.POSITION_TARGET_TYPEMASK_VY_IGNORE |
|
||||
mavutil.mavlink.POSITION_TARGET_TYPEMASK_VZ_IGNORE)
|
||||
ACC_IGNORE = (mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE |
|
||||
mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_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 = [
|
||||
mavutil.mavlink.MAV_FRAME_GLOBAL,
|
||||
|
|
@ -8116,11 +8123,10 @@ Also, ignores heartbeats not from our target system'''
|
|||
self.sysid_thismav(), # target system_id
|
||||
1, # target component id
|
||||
mav_frame,
|
||||
MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE |
|
||||
MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE |
|
||||
MAVLINK_SET_POS_TYPE_MASK_FORCE |
|
||||
MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE |
|
||||
MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE,
|
||||
MAV_POS_TARGET_TYPE_MASK.VEL_IGNORE |
|
||||
MAV_POS_TARGET_TYPE_MASK.ACC_IGNORE |
|
||||
MAV_POS_TARGET_TYPE_MASK.YAW_IGNORE |
|
||||
MAV_POS_TARGET_TYPE_MASK.YAW_RATE_IGNORE,
|
||||
int(lat * 1.0e7), # lat
|
||||
int(lng * 1.0e7), # lon
|
||||
alt, # alt
|
||||
|
|
@ -8184,10 +8190,9 @@ Also, ignores heartbeats not from our target system'''
|
|||
self.sysid_thismav(), # target system_id
|
||||
1, # target component id
|
||||
frame,
|
||||
MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE |
|
||||
MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE |
|
||||
MAVLINK_SET_POS_TYPE_MASK_FORCE |
|
||||
MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE,
|
||||
MAV_POS_TARGET_TYPE_MASK.VEL_IGNORE |
|
||||
MAV_POS_TARGET_TYPE_MASK.ACC_IGNORE |
|
||||
MAV_POS_TARGET_TYPE_MASK.YAW_RATE_IGNORE,
|
||||
int(targetpos.lat * 1.0e7), # lat
|
||||
int(targetpos.lng * 1.0e7), # lon
|
||||
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
|
||||
1, # target component id
|
||||
frame,
|
||||
MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE |
|
||||
MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE |
|
||||
MAVLINK_SET_POS_TYPE_MASK_FORCE |
|
||||
MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE,
|
||||
MAV_POS_TARGET_TYPE_MASK.VEL_IGNORE |
|
||||
MAV_POS_TARGET_TYPE_MASK.ACC_IGNORE |
|
||||
MAV_POS_TARGET_TYPE_MASK.YAW_RATE_IGNORE,
|
||||
int(targetpos.lat * 1.0e7), # lat
|
||||
int(targetpos.lng * 1.0e7), # lon
|
||||
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
|
||||
1, # target component id
|
||||
frame,
|
||||
MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE |
|
||||
MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE |
|
||||
MAVLINK_SET_POS_TYPE_MASK_FORCE |
|
||||
MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE,
|
||||
MAV_POS_TARGET_TYPE_MASK.VEL_IGNORE |
|
||||
MAV_POS_TARGET_TYPE_MASK.ACC_IGNORE |
|
||||
MAV_POS_TARGET_TYPE_MASK.YAW_IGNORE,
|
||||
int(targetpos.lat * 1.0e7), # lat
|
||||
int(targetpos.lng * 1.0e7), # lon
|
||||
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
|
||||
1, # target component id
|
||||
mav_frame,
|
||||
MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE |
|
||||
MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE |
|
||||
MAVLINK_SET_POS_TYPE_MASK_FORCE |
|
||||
MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE |
|
||||
MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE,
|
||||
MAV_POS_TARGET_TYPE_MASK.POS_IGNORE |
|
||||
MAV_POS_TARGET_TYPE_MASK.ACC_IGNORE |
|
||||
MAV_POS_TARGET_TYPE_MASK.YAW_IGNORE |
|
||||
MAV_POS_TARGET_TYPE_MASK.YAW_RATE_IGNORE,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
|
|
@ -8436,10 +8438,9 @@ Also, ignores heartbeats not from our target system'''
|
|||
self.sysid_thismav(), # target system_id
|
||||
1, # target component id
|
||||
mav_frame,
|
||||
MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE |
|
||||
MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE |
|
||||
MAVLINK_SET_POS_TYPE_MASK_FORCE |
|
||||
MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE,
|
||||
MAV_POS_TARGET_TYPE_MASK.POS_IGNORE |
|
||||
MAV_POS_TARGET_TYPE_MASK.ACC_IGNORE |
|
||||
MAV_POS_TARGET_TYPE_MASK.YAW_RATE_IGNORE,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
|
|
@ -8466,10 +8467,9 @@ Also, ignores heartbeats not from our target system'''
|
|||
self.sysid_thismav(), # target system_id
|
||||
1, # target component id
|
||||
mav_frame,
|
||||
MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE |
|
||||
MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE |
|
||||
MAVLINK_SET_POS_TYPE_MASK_FORCE |
|
||||
MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE,
|
||||
MAV_POS_TARGET_TYPE_MASK.POS_IGNORE |
|
||||
MAV_POS_TARGET_TYPE_MASK.ACC_IGNORE |
|
||||
MAV_POS_TARGET_TYPE_MASK.YAW_RATE_IGNORE,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
|
|
@ -8535,10 +8535,9 @@ Also, ignores heartbeats not from our target system'''
|
|||
self.sysid_thismav(), # target system_id
|
||||
1, # target component id
|
||||
mav_frame,
|
||||
MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE |
|
||||
MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE |
|
||||
MAVLINK_SET_POS_TYPE_MASK_FORCE |
|
||||
MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE,
|
||||
MAV_POS_TARGET_TYPE_MASK.POS_IGNORE |
|
||||
MAV_POS_TARGET_TYPE_MASK.ACC_IGNORE |
|
||||
MAV_POS_TARGET_TYPE_MASK.YAW_IGNORE,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
|
|
@ -8565,10 +8564,9 @@ Also, ignores heartbeats not from our target system'''
|
|||
self.sysid_thismav(), # target system_id
|
||||
1, # target component id
|
||||
mav_frame,
|
||||
MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE |
|
||||
MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE |
|
||||
MAVLINK_SET_POS_TYPE_MASK_FORCE |
|
||||
MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE,
|
||||
MAV_POS_TARGET_TYPE_MASK.POS_IGNORE |
|
||||
MAV_POS_TARGET_TYPE_MASK.ACC_IGNORE |
|
||||
MAV_POS_TARGET_TYPE_MASK.YAW_IGNORE,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
|
|
|
|||
Loading…
Reference in New Issue