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

Autotest: use MAV_POS_TARGET_TYPE_MASK, remove POS_TYPE_MASK_FORCE

gps-1.3.1
Josh Henderson 4 years ago committed by Andrew Tridgell
parent
4559274755
commit
162b3b70cf
2 changed files with 71 additions and 61 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. 34
      Tools/autotest/arducopter.py
  2. 98
      Tools/autotest/common.py

34
Tools/autotest/arducopter.py
Unescape View File

@ -24,6 +24,7 @@ from pysim import vehicleinfo @@ -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): @@ -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): @@ -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): @@ -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): @@ -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): @@ -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): @@ -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): @@ -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): @@ -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): @@ -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): @@ -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

98
Tools/autotest/common.py
Unescape View File

@ -25,6 +25,7 @@ import socket @@ -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: @@ -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''' @@ -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''' @@ -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''' @@ -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''' @@ -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''' @@ -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''' @@ -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''' @@ -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''' @@ -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''' @@ -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,

Write Preview
Loading…
Cancel
Save