diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index cdaaf96166..5fd5cc02cd 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -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 diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 47737a3854..a3bda804f7 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -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,