def startRecording():
if get_first_pozyx_serial_port() is None:
return json.dumps({'success':False, 'errorType':'NO_POZYX_CONNECTED'}), 200, {'ContentType':'application/json'}
my_var1 = session.get('my_var1', None)
mid1 = my_var1
now = datetime.datetime.now()
sql = '''INSERT INTO recordings (Match_ID, startTime) VALUES(%s,%s)'''
recordingData = ([mid1], now)
cur = conn.cursor()
cur.execute(sql, recordingData)
try:
conn.commit()
global RID
RID = cur.lastrowid
print("currentRecordingID after commit: " + str(RID))
except Exception as e:
print(e)
print("Start recording!avc")
global isRecording
isRecording = True
pozyxThread = PozyxThread()
pozyxThread.start()
return json.dumps({'success':True}), 200, {'ContentType':'application/json'}
def main():
serial_port = get_first_pozyx_serial_port()
if serial_port is None:
print("No Pozyx connected. Check your USB cable or your driver!")
quit()
remote_id = 0x1234 # remote device network ID
remote = False # whether to use a remote device
if not remote:
remote_id = None
use_processing = True # enable to send position data through OSC
ip = "127.0.0.1" # IP for the OSC UDP
network_port = 8888 # network port for the OSC UDP
osc_udp_client = None
if use_processing:
osc_udp_client = SimpleUDPClient(ip, network_port)
anchors = [DeviceCoordinates(0x6E2A, 1, Coordinates(0, 0, 3175)),
DeviceCoordinates(0x6E0E, 1, Coordinates(0, 4114, 3175)),
DeviceCoordinates(0x697F, 1, Coordinates(3429, 0, 3175)),
DeviceCoordinates(0x6E6F, 1, Coordinates(3429, 4114, 3175))]
algorithm = POZYX_POS_ALG_UWB_ONLY # positioning algorithm to use
dimension = POZYX_3D # positioning dimension
height = 1000 # height of device, required in 2.5D positioning
pozyx = PozyxSerial(serial_port)
r = ReadyToLocalize(pozyx, osc_udp_client, anchors, algorithm, dimension, height, remote_id)
r.setup()
while 1:
r.loop()
if not GPIO.input(buttonPin):
in_use()
maintenance = format_time()
data = {"ID": Id, "X": x, "Y": y, "InUse": inUse, "Maintenance": maintenance}
firebase.post('/Ventilator', data)
time.sleep(5)
def pozyx_pose_pub():
pub = rospy.Publisher('/charlie_pose',
PoseWithCovarianceStamped,
vicon_cb,
queue_size=1000)
pub1 = rospy.Publisher('/tag_pose0', Point, queue_size=1000)
pub2 = rospy.Publisher('/tag_pose1', Point, queue_size=1000)
rospy.init_node('pozyx_pose_node')
try:
# MOD @ 12-08-2020
serial_port = pypozyx.get_first_pozyx_serial_port()
pozyx = pypozyx.PozyxSerial(serial_port)
# BKP -> pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device)
except:
rospy.loginfo("Pozyx not connected")
return
while not rospy.is_shutdown():
coords = pypozyx.Coordinates()
tags = pypozyx.Coordinates()
quat = pypozyx.Quaternion()
pozyx.doPositioning(coords, pypozyx.POZYX_3D, remote_id=remote_id)
pozyx.getQuaternion(quat, remote_id=remote_id)
pose.pose.pose.position.x = coords.x / 1000.0
pose.pose.pose.position.y = coords.y / 1000.0
pose.pose.pose.position.z = coords.z / 1000.0
# ho messo come messaggio PoseWithCovarianceStamped ed ho aggiunto la covarianza a mano, quindi per accedere alle variabili di pos e orient bisogna fare pose.pose.pose, se fosse solo PoseStamped basta pose.pose
pose.pose.covariance[0] = 0.01
pose.pose.covariance[7] = 0.01
pose.pose.covariance[14] = 99999
pose.pose.covariance[21] = 99999
pose.pose.covariance[28] = 99999
pose.pose.covariance[35] = 0.01
tags.x = coords.x / 1000.0
tags.y = coords.y / 1000.0
tags.z = coords.z / 1000.0
pose.pose.pose.orientation.x = quat.x
pose.pose.pose.orientation.y = quat.y
pose.pose.pose.orientation.z = quat.z
pose.pose.pose.orientation.w = quat.w
rospy.loginfo("POS: %s, QUAT: %s" % (str(tags), str(quat)))
pub.publish(pose)
pub1.publish(Point(tags.x, tags.y, tags.z))
pub2.publish(Point(tags.x, tags.y, tags.z))
euler_angles = pypozyx.EulerAngles()
euler_angles.heading = numpy.deg2rad(euler_angles.heading)
tf_quat = tf.transformations.quaternion_from_euler(
0, 0, -euler_angles.heading)
br = tf.TransformBroadcaster()
br.sendTransform((coords.x / 1000.0, coords.y / 1000.0, 0), tf_quat,
rospy.Time.now(), tag0_frame_ID, UWB_frame_ID)
def pozyx_pose_pub():
pub = rospy.Publisher('amcl_pose', PoseStamped, vicon_cb, queue_size=100)
rospy.init_node('pozyx_pose_node')
try:
# MOD @ 12-08-2020
serial_port = pypozyx.get_first_pozyx_serial_port()
pozyx = pypozyx.PozyxSerial(serial_port)
# BKP -> pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device)
except:
rospy.loginfo("Pozyx not connected")
return
while not rospy.is_shutdown():
coords = pypozyx.Coordinates()
quat = pypozyx.Quaternion()
pozyx.doPositioning(coords, pypozyx.POZYX_3D, remote_id=remote_id)
pozyx.getQuaternion(quat, remote_id=remote_id)
rospy.loginfo("POS: %s, QUAT: %s" % (str(coords), str(quat)))
pose.pose.position.x = coords.x
pose.pose.position.y = coords.y
pose.pose.position.z = coords.z
pose.pose.orientation.x = quat.x
pose.pose.orientation.y = quat.y
pose.pose.orientation.z = quat.z
pose.pose.orientation.w = quat.w
pub.publish(pose)
def pozyx_position_euler_pub():
#position_pub = rospy.Publisher(
# 'pozyx_positioning', Point32, queue_size=100)
position_pub = rospy.Publisher('/tag_pose0', Point, queue_size=100)
#euler_pub = rospy.Publisher(
# 'pozyx_euler_angles', EulerAngles, queue_size=100)
euler_pub = rospy.Publisher('/orientation', Point, queue_size=100)
vx = 0.0
vy = 0.0
vth = 0.0
rospy.init_node('position_euler_pub')
#pub odom msg for move_base
odom_pub = rospy.Publisher("odom", Odometry, queue_size=50)
odom_broadcaster = tf.TransformBroadcaster()
current_time = rospy.Time.now()
last_time = rospy.Time.now()
try:
# MOD @ 12-08-2020
serial_port = pypozyx.get_first_pozyx_serial_port()
pozyx = pypozyx.PozyxSerial(serial_port)
#BKP -> pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device)
except:
rospy.loginfo("Pozyx not connected")
return
while not rospy.is_shutdown():
coords = pypozyx.Coordinates()
pozyx.doPositioning(coords, remote_id=remote_id)
position_pub.publish(coords.x, coords.y, coords.z)
euler_angles = pypozyx.EulerAngles()
pozyx.getEulerAngles_deg(euler_angles)
rospy.loginfo("POS: %s, ANGLES: %s" % (str(coords), str(euler_angles)))
euler_pub.publish(euler_angles.heading, euler_angles.roll,
euler_angles.pitch)
# combine pos and angle info and publish as odom msg
current_time = rospy.Time.now()
# odometry is 6DOF, created quaternion from yaw
odom_quat = tf.transformations.quaternion_from_euler(
0, 0, euler_angles.heading)
# publish the transform over tf
odom_broadcaster.sendTransform((coords.x, coords.y, 0.), odom_quat,
current_time, "base_footprint",
"odom") #base_link
# publish the odometry message over ROS
odom = Odometry()
odom.header.stamp = current_time
odom.header.frame_id = "odom"
odom.pose.pose = Pose(Point(coords.x, coords.y, 0.),
Quaternion(*odom_quat))
odom.child_frame_id = "base_footprint" #base_link
odom.twist.twist = Twist(Vector3(vx, vy, 0),
Vector3(0, 0, vth)) # set the velocity
odom_pub.publish(odom)
last_time = current_time
def getSerialport(self):
# serialport connection test
serial_port = get_first_pozyx_serial_port()
if serial_port is None:
print("No Pozyx connected. Check your USB cable or your driver!")
return None
else:
return serial_port
def pozyx_position_euler_pub():
global rcvd_msgs
global rec_x
global rec_y
#position_pub = rospy.Publisher(
# 'pozyx_positioning', Point32, queue_size=100)
position_pub = rospy.Publisher('/charlie_pose', Point, queue_size=100)
#euler_pub = rospy.Publisher(
# 'pozyx_euler_angles', EulerAngles, queue_size=100)
euler_pub = rospy.Publisher('/orientation', Point, queue_size=100)
pub1 = rospy.Publisher('/tag_pose0', Point, queue_size=100)
pub2 = rospy.Publisher('/tag_pose1', Point, queue_size=100)
rospy.init_node('position_euler_pub')
rate=rospy.Rate(1)
try:
# MOD @ 12-08-2020
serial_port = pypozyx.get_first_pozyx_serial_port()
pozyx = pypozyx.PozyxSerial(serial_port)
#BKP -> pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device)
except:
rospy.loginfo("Pozyx not connected")
return
while not rospy.is_shutdown():
coords = pypozyx.Coordinates()
tags = pypozyx.Coordinates()
pozyx.doPositioning(coords, remote_id=remote_id)
pozyx.doPositioning(tags, remote_id=remote_id)
euler_angles = pypozyx.EulerAngles()
pozyx.getEulerAngles_deg(euler_angles)
euler_pub.publish(euler_angles.heading,
euler_angles.roll, euler_angles.pitch)
rospy.loginfo("X: %f Y: %f" , tags.x, tags.y)
rospy.loginfo(euler_angles)
pub1.publish(tags.x/1000.0,tags.y/1000.0,tags.z/1000.0)
pub2.publish(tags.x/1000.0, tags.y/1000.0,tags.z/1000.0)
position_pub.publish(coords.x/1000.0, coords.y/1000.0, coords.z/1000.0)
euler_angles.heading = numpy.deg2rad(euler_angles.heading)
tf_quat = tf.transformations.quaternion_from_euler(0, 0, -euler_angles.heading)
br = tf.TransformBroadcaster()
br.sendTransform((coords.x/1000.0, coords.y/1000.0, 0),
tf_quat,
rospy.Time.now(),
tag0_frame_ID,
UWB_frame_ID
)
rate.sleep()
def run(self):
pozyx = PozyxSerial(get_first_pozyx_serial_port())
global RID
r = PozyxControl(pozyx, tag_ids, anchors, RID, algorithm, dimension, height)
r.setup()
while isRecording:
r.loop()
def doSomething():
port = get_first_pozyx_serial_port()
print('Port:', port)
p = PozyxSerial(port)
whoami = SingleRegister()
p.regRead(POZYX_WHO_AM_I, whoami)
print('WhoAmI:', whoami)
return port, whoami.data
def __init__(self, anchors, tags, serial_port=None):
if (serial_port is None):
serial_port = pypozyx.get_first_pozyx_serial_port()
assert (serial_port is not None, "Could not find local pozyx device")
self.local = pypozyx.PozyxSerial(serial_port)
# get local id
local_id = pypozyx.NetworkID()
status = self.local.getNetworkId(local_id)
self._check_status("get network id", status)
self.local_id = local_id[0]
# check for nessary devices and set coordinates
self._find_devices()
device_list = self._get_device_list()
device_list.append(self.local_id)
for device in anchors:
id = device.network_id
cord = device.pos
if id in device_list:
status = self.local.setCoordinates(
cord, None if id == self.local_id else id)
self._check_status("set coordinates", status)
else:
raise Exception(
"failed to find a pozyx device with the id {:02x}".format(
id))
for id in tags:
if id not in device_list:
raise Exception(
"failed to find a pozyx device with the id {:02x}".format(
id))
# remove any unknown devices
anchor_ids = map(lambda device: device.network_id, anchors)
for id in device_list:
if (id not in anchor_ids) and (id not in tags):
# TODO: log removal of unknown device
self.local.removeDevice(id)
# store lists of anchors and tags
self.anchors = anchor_ids
self.tags = tags
def pozyx_range():
pub = rospy.Publisher('pozyx_range', DeviceRange, queue_size=100)
rospy.init_node('range_pub')
try:
serial_port = pypozyx.get_first_pozyx_serial_port()
if serial_port == None:
rospy.loginfo("Pozyx not connected")
return
pozyx = pypozyx.PozyxSerial(serial_port)
except:
rospy.loginfo("Could not connect to pozyx")
return
# get network id of connected pozyx
self_id = pypozyx.NetworkID()
if pozyx.getNetworkId(self_id) != pypozyx.POZYX_SUCCESS:
rospy.loginfo("Could not get network id")
return
# discover all other pozyx
pozyx.doDiscoveryAll()
devices_size = pypozyx.SingleRegister()
pozyx.getDeviceListSize(devices_size)
devices = pypozyx.DeviceList(devices_size)
pozyx.getDeviceIds(devices)
for i in range(devices_size[0]):
rospy.loginfo("Found pozyx device with the id {}".format(
pypozyx.NetworkID(devices[i])))
# publish data
while not rospy.is_shutdown():
for i in range(devices_size[0]):
device_range = pypozyx.DeviceRange()
remote_id = pypozyx.NetworkID(devices[i])
if pozyx.doRanging(self_id[0], device_range, remote_id[0]):
h = Header()
h.stamp = rospy.Time.from_sec(device_range[0] / 1000)
pub.publish(h, remote_id[0], device_range[1], device_range[2])
else:
rospy.loginfo("Error while ranging")
def __init__(self, anchors):
self.serial_port = get_first_pozyx_serial_port()
print(self.serial_port)
if self.serial_port is None:
print("No Pozyx connected. Check your USB cable or your driver!")
self.pozyx = PozyxSerial(self.serial_port)
#print(self.serial_port)
if (anchors.get('count') == 4):
self.anchors = [
DeviceCoordinates(
0x6110, 1,
Coordinates(
anchors.get('0x6110')[0],
anchors.get('0x6110')[1],
anchors.get('0x6110')[2])),
DeviceCoordinates(
0x6115, 1,
Coordinates(
anchors.get('0x6115')[0],
anchors.get('0x6115')[1],
anchors.get('0x6115')[2])),
DeviceCoordinates(
0x6117, 1,
Coordinates(
anchors.get('0x6117')[0],
anchors.get('0x6117')[1],
anchors.get('0x6117')[2])),
DeviceCoordinates(
0x611e, 1,
Coordinates(
anchors.get('0x611e')[0],
anchors.get('0x611e')[1],
anchors.get('0x611e')[2]))
]
self.algorithm = PozyxConstants.POSITIONING_ALGORITHM_UWB_ONLY
self.dimension = PozyxConstants.DIMENSION_3D
self.height = 1000
#self.pub = rospy.Publisher('/mavros/mocap/pose', PoseStamped, queue_size=10)
self.pub = rospy.Publisher('/pose', PoseStamped, queue_size=10)
self.pose = PoseStamped()
self.subZ = rospy.Subscriber('range', Int16, self.rangeCallback)
def __init__(self, anchors):
self.anchors = [DeviceCoordinates()]
self.anchors = anchors
self.port = get_first_pozyx_serial_port()
if self.port is None:
self.error = "No Pozyx connected"
return
self.pozyx = PozyxSerial(self.port)
networkId = NetworkID()
status = self.pozyx.getNetworkId(networkId)
self.id = networkId.id
self.CheckStatus(status)
self.ConfigureAnchor(self.id)
def pozyx_euler_pub():
pub = rospy.Publisher('pozyx_euler_angles', EulerAngles, queue_size=100)
rospy.init_node('pozyx_euler_pub')
try:
# MOD @ 12-08-2020
serial_port = pypozyx.get_first_pozyx_serial_port()
pozyx = pypozyx.PozyxSerial(serial_port)
# BKP -> pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device)
except:
rospy.loginfo("Pozyx not connected")
return
while not rospy.is_shutdown():
euler_angles = pypozyx.EulerAngles()
pozyx.getEulerAngles_deg(euler_angles, remote_id=remote_id)
rospy.loginfo(euler_angles)
pub.publish(euler_angles.heading, euler_angles.roll,
euler_angles.pitch)
def pozyx_ranging_pub():
pub = rospy.Publisher('pozyx_device_range', DeviceRange, queue_size=100)
rospy.init_node('range_info_pub')
try:
# MOD @ 12-08-2020
serial_port = pypozyx.get_first_pozyx_serial_port()
pozyx = pypozyx.PozyxSerial(serial_port)
# BKP -> pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device)
except:
rospy.loginfo("Pozyx not connected")
return
while not rospy.is_shutdown():
device_range = pypozyx.DeviceRange()
if pozyx.doRanging(destination_id, device_range, remote_id=remote_id):
pub.publish(device_range.timestamp, device_range.distance,
device_range.RSS)
rospy.loginfo(device_range)
else:
rospy.loginfo('ERROR: RANGING')
def __init__(self):
super().__init__("range_debugger")
self.range_pub = self.create_publisher(String, "range", 10)
self.position_pub = self.create_publisher(Odometry, "odometry/pozyx",
1000)
self.markers_pub = self.create_publisher(MarkerArray,
"odometry/pozyx/markers", 10)
# serial port setting
serial_port = "/dev/ttyACM0"
seiral_port = get_first_pozyx_serial_port()
if serial_port is None:
print("No Pozyx connected. CHeck your USB cable or your driver!")
quit()
self.pozyx = PozyxSerial(serial_port)
# remote and destination
# But sorry, just 1 tag is useable.
# "None" is setting for use USB-connected tag, "0xXX"(tag id) is to use remote tag.
self.tag_ids = [None] # TODO: To use multiple tags
self.ranging_protocol = PozyxConstants.RANGE_PROTOCOL_PRECISION
self.range_timer_ = self.create_timer(0.02, self.range_callback)
self.anchors = [
# DeviceCoordinates(0x605b, 1, Coordinates( 0, 0, 0)), # test
# DeviceCoordinates(0x603b, 1, Coordinates( 800, 0, 0)), # test
DeviceCoordinates(0x6023, 1, Coordinates(-13563, -8838,
475)), # ROOM
DeviceCoordinates(0x6e23, 1, Coordinates(-3327, -8849,
475)), # ROOM
DeviceCoordinates(0x6e49, 1, Coordinates(-3077, -2959,
475)), # ROOM
# DeviceCoordinates(0x6e58, 1, Coordinates( -7238, -3510, 475)), # ROOM
DeviceCoordinates(0x6050, 1, Coordinates(-9214, -9102,
475)), # ROOM
]
self.algorithm = PozyxConstants.POSITIONING_ALGORITHM_UWB_ONLY
self.dimension = PozyxConstants.DIMENSION_2D
self.height = 475
self.setup()
def pozyx_pose_pub():
pub = rospy.Publisher('pozyx_pose', Pose, queue_size=40)
rospy.init_node('pozyx_pose_node')
try:
# MOD @ 12-08-2020
serial_port = pypozyx.get_first_pozyx_serial_port()
pozyx = pypozyx.PozyxSerial(serial_port)
# BKP -> pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device)
except:
rospy.loginfo("Pozyx not connected")
return
while not rospy.is_shutdown():
coords = pypozyx.Coordinates()
quat = pypozyx.Quaternion()
pozyx.doPositioning(coords, pypozyx.POZYX_3D, remote_id=remote_id)
pozyx.getQuaternion(quat, remote_id=remote_id)
rospy.loginfo("POS: %s, QUAT: %s" % (str(coords), str(quat)))
pub.publish(Point(coords.x, coords.y, coords.z),
Quaternion(quat.x, quat.y, quat.z, quat.w))
def set_same_uwb_settings():
rospy.init_node('uwb_configurator')
devices_met = []
uwb_registers = [0x1C, 0x1D, 0x1E, 0x1F]
s = ''
for device in device_ids:
s += '0x%0.4x ' % device
rospy.loginfo("Setting devices with IDs:%s to UWB settings: %s" %
(s, str(new_uwb_settings)))
try:
# MOD @ 12-08-2020
serial_port = pypozyx.get_first_pozyx_serial_port()
pozyx = pypozyx.PozyxSerial(serial_port)
#BKP-> pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device)
except:
rospy.loginfo("Pozyx not connected")
return
for channel in pypozyx.POZYX_ALL_CHANNELS:
for bitrate in pypozyx.POZYX_ALL_BITRATES:
for prf in pypozyx.POZYX_ALL_PRFS:
for plen in pypozyx.POZYX_ALL_PLENS:
uwb_settings = pypozyx.UWBSettings(
channel, bitrate, prf, plen, gain_db)
pozyx.clearDevices()
pozyx.setUWBSettings(uwb_settings)
pozyx.doDiscovery(pypozyx.POZYX_DISCOVERY_ALL_DEVICES)
device_list_size = pypozyx.SingleRegister()
pozyx.getDeviceListSize(device_list_size)
if device_list_size[0] > 0:
device_list = pypozyx.DeviceList(
list_size=device_list_size[0])
pozyx.getDeviceIds(device_list)
for device in device_list:
if device not in devices_met and device in device_ids:
pozyx.setUWBSettings(new_uwb_settings, device)
pozyx.saveRegisters(uwb_registers, device)
rospy.loginfo(
'Device with ID 0x%0.4x is set' % device)
devices_met.append(device)
pozyx.setUWBSettings(new_uwb_settings)
pozyx.saveRegisters(uwb_registers)
rospy.loginfo("Local device set! Shutting down configurator node now...")
def set_anchor_configuration():
tag_ids = [None]
rospy.init_node('uwb_configurator')
rospy.loginfo("Configuring device list.")
settings_registers = [0x16, 0x17] # POS ALG and NUM ANCHORS
try:
# MOD @ 12-08-2020
serial_port = pypozyx.get_first_pozyx_serial_port()
pozyx = pypozyx.PozyxSerial(serial_port)
# BKP-> pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device)
except:
rospy.loginfo("Pozyx not connected")
return
pozyx.doDiscovery(discovery_type=pypozyx.POZYX_DISCOVERY_TAGS_ONLY)
device_list_size = pypozyx.SingleRegister()
pozyx.getDeviceListSize(device_list_size)
if device_list_size[0] > 0:
device_list = pypozyx.DeviceList(list_size=device_list_size[0])
pozyx.getDeviceIds(device_list)
tag_ids += device_list.data
for tag in tag_ids:
for anchor in anchors:
pozyx.addDevice(anchor, tag)
if len(anchors) > 4:
pozyx.setSelectionOfAnchors(pypozyx.POZYX_ANCHOR_SEL_AUTO,
len(anchors),
remote_id=tag)
pozyx.saveRegisters(settings_registers, remote_id=tag)
pozyx.saveNetwork(remote_id=tag)
if tag is None:
rospy.loginfo("Local device configured")
else:
rospy.loginfo("Device with ID 0x%0.4x configured." % tag)
rospy.loginfo("Configuration completed! Shutting down node now...")
def pozyx_positioning_pub():
# pub = rospy.Publisher('pozyx_positioning', Point32, queue_size=100)
pub = rospy.Publisher('/tag_pose0', Point, queue_size=100)
pub1 = rospy.Publisher('/tag_pose1', Point, queue_size=100)
rospy.init_node('positioning_pub')
try:
# MOD @ 12-08-2020
serial_port = pypozyx.get_first_pozyx_serial_port()
pozyx = pypozyx.PozyxSerial(serial_port)
# BKP-> pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device)
except:
rospy.loginfo("Pozyx not connected")
return
while not rospy.is_shutdown():
coords = pypozyx.Coordinates()
pozyx.doPositioning(coords, remote_id=remote_id)
rospy.loginfo(coords)
#pub.publish(Point32(coords.x, coords.y, coords.z))
#pub.publish(Point(coords.x, coords.y, coords.z))
pub.publish(
Point(coords.x / 1000.0, coords.y / 1000.0, coords.z / 1000.0))
pub1.publish(
Point(coords.x / 1000.0, coords.y / 1000.0, coords.z / 1000.0))
#import pypozyx
import sys, time
from pypozyx import PozyxSerial, get_first_pozyx_serial_port, POZYX_SUCCESS, SingleRegister, EulerAngles, Acceleration, UWBSettings
#pozyx = PozyxLib() # PozyxSerial has PozyxLib's functions, just for generality
CURSOR_UP_ONE = '\x1b[1A'
ERASE_LINE = '\x1b[2K'
is_cursor_up = False
#print(pypozyx.get_first_pozyx_serial_port())
pozyx = PozyxSerial(get_first_pozyx_serial_port())
who_am_i = SingleRegister()
# get the data, passing along the container
status = pozyx.getWhoAmI(who_am_i)
acceleration = Acceleration()
euler_angles = EulerAngles()
uwb_settings = UWBSettings()
# check the status to see if the read was successful. Handling failure is covered later.
if status == POZYX_SUCCESS:
# print the container. Note how a SingleRegister will print as a hex string by default.
print('Who Am I: {}'.format(who_am_i)) # will print '0x43'
while True:
# initalize the Pozyx as above
# initialize the data container
# and repeat
# initialize the data container
# get the data, passing along the container
if is_cursor_up:
sys.stdout.write(CURSOR_UP_ONE)
def __init__(self):
self.pozyx = PozyxSerial(get_first_pozyx_serial_port())
self.direct = EulerAngles()
self.position = Coordinates()
def __init__(self, i2c=False, bus=1, port=None):
super().__init__()
self._sensor = pypozyx.PozyxI2C(bus) if i2c else \
pypozyx.PozyxSerial(port or pypozyx.get_first_pozyx_serial_port())
"/anchor", [device_list[i], int(anchor_coordinates.x), int(anchor_coordinates.y), int(anchor_coordinates.z)])
sleep(0.025)
def printPublishAnchorConfiguration(self):
"""Prints and potentially publishes the anchor configuration"""
for anchor in self.anchors:
print("ANCHOR,0x%0.4x,%s" % (anchor.network_id, str(anchor.coordinates)))
if self.osc_udp_client is not None:
self.osc_udp_client.send_message(
"/anchor", [anchor.network_id, int(anchor.coordinates.x), int(anchor.coordinates.y), int(anchor.coordinates.z)])
sleep(0.025)
if __name__ == "__main__":
# shortcut to not have to find out the port yourself
serial_port = get_first_pozyx_serial_port()
if serial_port is None:
print("No Pozyx connected. Check your USB cable or your driver!")
quit()
remote_id = 0x6069 # remote device network ID
remote = False # whether to use a remote device
if not remote:
remote_id = None
use_processing = False # enable to send position data through OSC
ip = "127.0.0.1" # IP for the OSC UDP
network_port = 8888 # network port for the OSC UDP
osc_udp_client = None
if use_processing:
osc_udp_client = SimpleUDPClient(ip, network_port)
def __init__(self, mpstate):
"""Initialise module"""
super(BeaconToGPS, self).__init__(mpstate, "BeaconToGPS", "")
self.anchor_config = None
self.config_file_parser = ConfigParser.ConfigParser()
self.config_file_parser.read(os.getcwd() + '/config/uwb_config.conf')
self.anchor_config = self.config_file_parser.get(
"Anchor", "anchor_coordinates")
if self.anchor_config is None:
print("Need set the anchor coordinate!")
return
self.yaw_deg = self.config_file_parser.getfloat(
"NED", "yaw_form_ned_to_uwb")
if self.yaw_deg is None:
print("Need set the yaw from ned to uwb!")
return
else:
print("NED to UWB yaw:" + str(self.yaw_deg) + " deg")
self.debug = 0
self.debug = self.config_file_parser.getint("SYS", "debug")
if self.debug is None:
self.debug = 0
serial_port_dev = get_first_pozyx_serial_port()
if serial_port_dev is None:
print("No Pozyx connected. Check your USB cable or your driver!")
return
self.pozyx = PozyxSerial(serial_port_dev)
self.anchors = self.anchor_config[1:len(self.anchor_config) -
1].split(";")
self.anchor_list = []
self.position = Coordinates()
self.velocity = Coordinates()
self.pos_last = Coordinates()
self.pos_last_time = 0
self.setup_pozyx()
self.CONSTANTS_RADIUS_OF_EARTH = 6378100.0
self.DEG_TO_RAD = 0.01745329251994329576
self.RAD_TO_DEG = 57.29577951308232087679
self.reference_lat = 36.26586666666667
self.reference_lon = 120.27563333333333
self.reference_lat_rad = self.reference_lat * self.DEG_TO_RAD
self.reference_lon_rad = self.reference_lon * self.DEG_TO_RAD
self.cos_lat = math.cos(self.reference_lat_rad)
self.target_lon_param = self.CONSTANTS_RADIUS_OF_EARTH * self.cos_lat
self.current_lat = 0
self.current_lon = 0
self.tag_pos_ned = Coordinates()
self.tag_velocity_ned = Coordinates()
self.yaw = math.radians(self.yaw_deg)
self.cos_yaw = math.cos(self.yaw)
self.sin_yaw = math.sin(self.yaw)
self.location_update = False
self.location_update_time = 0
self.pos_update_time = 0
self.location_update_freq = 8
self.data = {
'time_usec':
0, # (uint64_t) Timestamp (micros since boot or Unix epoch)
'gps_id': 0, # (uint8_t) ID of the GPS for multiple GPS inputs
'ignore_flags': self.
IGNORE_FLAG_ALL, # (uint16_t) Flags indicating which fields to ignore (see GPS_INPUT_IGNORE_FLAGS enum). All other fields must be provided.
'time_week_ms':
0, # (uint32_t) GPS time (milliseconds from start of GPS week)
'time_week': 0, # (uint16_t) GPS week number
'fix_type':
0, # (uint8_t) 0-1: no fix, 2: 2D fix, 3: 3D fix. 4: 3D with DGPS. 5: 3D with RTK
'lat': 0, # (int32_t) Latitude (WGS84), in degrees * 1E7
'lon': 0, # (int32_t) Longitude (WGS84), in degrees * 1E7
'alt':
0, # (float) Altitude (AMSL, not WGS84), in m (positive for up)
'hdop': 0, # (float) GPS HDOP horizontal dilution of position in m
'vdop': 0, # (float) GPS VDOP vertical dilution of position in m
'vn':
0, # (float) GPS velocity in m/s in NORTH direction in earth-fixed NED frame
've':
0, # (float) GPS velocity in m/s in EAST direction in earth-fixed NED frame
'vd':
0, # (float) GPS velocity in m/s in DOWN direction in earth-fixed NED frame
'speed_accuracy': 0, # (float) GPS speed accuracy in m/s
'horiz_accuracy': 0, # (float) GPS horizontal accuracy in m
'vert_accuracy': 0, # (float) GPS vertical accuracy in m
'satellites_visible': 0 # (uint8_t) Number of satellites visible.
}
class MultitagPositioning(object):
"""Continuously performs multitag positioning"""
def nicksThing():
def __init__(self,
pozyx,
osc_udp_client,
tag_ids,
anchors,
algorithm=PozyxConstants.POSITIONING_ALGORITHM_UWB_ONLY,
dimension=PozyxConstants.DIMENSION_3D,
height=1000):
self.pozyx = pozyx
self.osc_udp_client = osc_udp_client
self.tag_ids = tag_ids
self.anchors = anchors
self.algorithm = algorithm
self.dimension = dimension
self.height = height
def setup(self):
"""Sets up the Pozyx for positioning by calibrating its anchor list."""
print("------------POZYX MULTITAG POSITIONING V{} -------------".
format(version))
print("")
print(" - System will manually calibrate the tags")
print("")
print(" - System will then auto start positioning")
print("")
if None in self.tag_ids:
for device_id in self.tag_ids:
self.pozyx.printDeviceInfo(device_id)
else:
for device_id in [None] + self.tag_ids:
self.pozyx.printDeviceInfo(device_id)
print("")
print("------------POZYX MULTITAG POSITIONING V{} -------------".
format(version))
print("")
self.setAnchorsManual()
self.printPublishAnchorConfiguration()
def loop(self):
"""Performs positioning and prints the results."""
for tag_id in self.tag_ids:
position = Coordinates()
status = self.pozyx.doPositioning(position,
self.dimension,
self.height,
self.algorithm,
remote_id=tag_id)
if status == POZYX_SUCCESS:
self.printPublishPosition(position, tag_id)
else:
self.printPublishErrorCode("positioning", tag_id)
# WORK IN PROGRESS: First attempt at making a function to collect and store the x, y, and z coordinates globally
"""def giveUsDemCoordyBois(self, position, network_id):
#hopefully gives us the coordinates to be able to access outside of the class
x_position = position.x;
y_position = position.y;
z_position = position.z;
print(x_position);
print(y_position);
print(z_position);"""
def printPublishPosition(self, position, network_id):
"""Prints the Pozyx's position and possibly sends it as a OSC packet"""
if network_id is None:
network_id = 0
s = "POS ID: {}, x(mm): {}, y(mm): {}, z(mm): {}".format(
"0x%0.4x" % network_id, position.x, position.y, position.z)
print(s)
if self.osc_udp_client is not None:
self.osc_udp_client.send_message(
"/position",
[network_id, position.x, position.y, position.z])
#NOTE: front tag is the tag connected to the Pi(0x673c/0x0000) and back tag is connected to external power source
if network_id == 0x0000: #if the tag that is having its coordinates measured is the one connected to the Pi
global x_position_front #indicate to program that the global variable for the front tag's x-position is to be used
global y_position_front #indicate to program that the global variable for the front tag's y-position is to be used
global z_position_front #indicate to program that the global variable for the front tag's z-position is to be used
x_position_front = position.x
#set the x-position of the front tag to the x-value output by the Pozyx
y_position_front = position.y
#set the y-position of the front tag to the y-value output by the Pozyx
z_position_front = position.z
#set the z-position of the front tag to the z-value output by the Pozyx
print(x_position_front)
#output the front tag's x-position
print(y_position_front)
#output the front tag's y-position
print(z_position_front)
#output the front tag's z-position
else: #otherwise, if the tag that is having its coordinates measured is the one connected to the external power source
global x_position_back #indicate to program that the global variable for the back tag's x-position is to be used
global y_position_back #indicate to program that the global variable for the back tag's y-position is to be used
global z_position_back #indicate to program that the global variable for the back tag's z-position is to be used
x_position_back = position.x
#set the x-position of the back tag to the x-value output by the Pozyx
y_position_back = position.y
#set the y-position of the back tag to the y-value output by the Pozyx
z_position_back = position.z
#set the z-position of the back tag to the z-value output by the Pozyx
print(x_position_back)
#output the back tag's x-position
print(y_position_back)
#output the back tag's y-position
print(z_position_back)
#output the back tag's z-position
#print("THE LOOP HAS BEEN EXITED"); FOR TESTING: print a statement that will allow us to see how exactly the loop
# running the main body is working and where it is at in its execution
'''NOTE: The following print statements were used previously to check if the global variables for x, y, and z
were being set to the Pozyx's outputs:
print(x_position);
print(y_position);
print(z_position);'''
def setAnchorsManual(self):
"""Adds the manually measured anchors to the Pozyx's device list one for one."""
for tag_id in self.tag_ids:
status = self.pozyx.clearDevices(tag_id)
for anchor in self.anchors:
status &= self.pozyx.addDevice(anchor, tag_id)
if len(anchors) > 4:
status &= self.pozyx.setSelectionOfAnchors(
PozyxConstants.ANCHOR_SELECT_AUTO, len(anchors))
# enable these if you want to save the configuration to the devices.
def printPublishConfigurationResult(self, status, tag_id):
"""Prints the configuration explicit result, prints and publishes error if one occurs"""
if tag_id is None:
tag_id = 0
if status == POZYX_SUCCESS:
print("Configuration of tag %s: success" % tag_id)
else:
self.printPublishErrorCode("configuration", tag_id)
def printPublishErrorCode(self, operation, network_id):
"""Prints the Pozyx's error and possibly sends it as a OSC packet"""
error_code = SingleRegister()
status = self.pozyx.getErrorCode(error_code, network_id)
if network_id is None:
network_id = 0
if status == POZYX_SUCCESS:
print("Error %s on ID %s, %s" %
(operation, "0x%0.4x" % network_id,
self.pozyx.getErrorMessage(error_code)))
if self.osc_udp_client is not None:
self.osc_udp_client.send_message(
"/error_%s" % operation, [network_id, error_code[0]])
else:
# should only happen when not being able to communicate with a remote Pozyx.
self.pozyx.getErrorCode(error_code)
print("Error % s, local error code %s" %
(operation, str(error_code)))
if self.osc_udp_client is not None:
self.osc_udp_client.send_message("/error_%s" % operation,
[0, error_code[0]])
def printPublishAnchorConfiguration(self):
for anchor in self.anchors:
print("ANCHOR,0x%0.4x,%s" %
(anchor.network_id, str(anchor.pos)))
if self.osc_udp_client is not None:
self.osc_udp_client.send_message("/anchor", [
anchor.network_id, anchor.pos.x, anchor.pos.y,
anchor.pos.z
])
sleep(0.025)
if __name__ == "__main__":
# Check for the latest PyPozyx version. Skip if this takes too long or is not needed by setting to False.
check_pypozyx_version = True
if check_pypozyx_version:
perform_latest_version_check()
# shortcut to not have to find out the port yourself.
serial_port = get_first_pozyx_serial_port()
if serial_port is None:
print("No Pozyx connected. Check your USB cable or your driver!")
quit()
# enable to send position data through OSC
use_processing = True
# configure if you want to route OSC to outside your localhost. Networking knowledge is required.
ip = "127.0.0.1"
network_port = 8888
# IDs of the tags to position, add None to position the local tag as well.
tag_ids = [None, 0x6728]
# necessary data for calibration
anchors = [
DeviceCoordinates(0x6e09, 1, Coordinates(0, 0, 0)),
DeviceCoordinates(0x674c, 1, Coordinates(1650, 0, 0)),
DeviceCoordinates(0x6729, 1, Coordinates(0, 480, 0)),
DeviceCoordinates(0x6765, 1, Coordinates(1650, 480, 0))
]
# positioning algorithm to use, other is PozyxConstants.POSITIONING_ALGORITHM_TRACKING
algorithm = PozyxConstants.POSITIONING_ALGORITHM_UWB_ONLY
# positioning dimension. Others are PozyxConstants.DIMENSION_2D, PozyxConstants.DIMENSION_2_5D
dimension = PozyxConstants.DIMENSION_2D
# height of device, required in 2.5D positioning
height = 1000
osc_udp_client = None
if use_processing:
osc_udp_client = SimpleUDPClient(ip, network_port)
pozyx = PozyxSerial(serial_port)
r = MultitagPositioning(pozyx, osc_udp_client, tag_ids, anchors,
algorithm, dimension, height)
r.setup()
while True:
r.loop()
print("THE FRONT X POSITION IS:" + str(x_position_front))
#used to see if the x-positions of the front tag were correct
print("THE FRONT Y POSITION IS:" + str(y_position_front))
#used to see if the y-positions of the front tag were correct
print("THE FRONT Z POSITION IS:" + str(z_position_front))
#used to see if the z-positions of the front tag were correct
print("THE BACK X POSITION IS:" + str(x_position_front))
#used to see if the x-positions of the back tag were correct
print("THE BACK Y POSITION IS:" + str(y_position_front))
#used to see if the y-positions of the back tag were correct
print("THE BACK Z POSITION IS:" + str(z_position_front))
#used to see if the z-positions of the back tag were correct
y = int(y_position_front)
x = int(x_position_front)
#break; #NOTE: this was previously used to end the execution of the Pozyx measuring to see if the new variables were working
#Previously used to check if the global variables for the x, y, and z coordinates were functioning properly:
#print(x_position);
#print(y_position);
#print(z_position);'''
import RPi.GPIO as GPIO #Pin setup for Entire Pi
import time
import curses #User Interface
import serial
#pin setup
GPIO.setmode(GPIO.BOARD)
GPIO.setup(13, GPIO.OUT)
GPIO.setup(22, GPIO.OUT)
GPIO.setup(15, GPIO.OUT)
GPIO.setup(18, GPIO.OUT)
#motor varibles
FR = GPIO.PWM(13,
50) #Front Right Motor #The value 50 is the Frequency
FL = GPIO.PWM(22, 50) #Front Left Motor #The value 12 is the GPIO pin
RR = GPIO.PWM(15, 50) #Rear Right Motor
RL = GPIO.PWM(18, 50) #Rear Left Motor
FR.start(100)
FL.start(100)
RR.start(100)
RL.start(100)
#curses setup
#screen = curses.initscr()
#curses.noecho()
#curses.cbreak()
#screen.keypad(True)
#User Interface
print('...Loading...')
while True:
#getUpdatedCoordinates()
print(y_position_front)
nicksThing()
if int(y_position_front) > 1400:
FR.ChangeDutyCycle(100)
FL.ChangeDutyCycle(100)
RR.ChangeDutyCycle(100)
RL.ChangeDutyCycle(100)
print('Almost there')
break
else:
print(y_position_front)
FR.ChangeDutyCycle(6.5)
FL.ChangeDutyCycle(8)
RR.ChangeDutyCycle(6.5)
RL.ChangeDutyCycle(8)
while True:
print("turning 90 degrees left")
FR.ChangeDutyCycle(5)
FL.ChangeDutyCycle(5)
RR.ChangeDutyCycle(5)
RL.ChangeDutyCycle(5)
time.sleep(.68)
FR.ChangeDutyCycle(100)
FL.ChangeDutyCycle(100)
RR.ChangeDutyCycle(100)
RL.ChangeDutyCycle(100)
break
while True:
#getUpdatedCoordinates()
print(x_position_front)
nicksThing()
if (x_position_front) > (1400):
FR.ChangeDutyCycle(100)
FL.ChangeDutyCycle(100)
RR.ChangeDutyCycle(100)
RL.ChangeDutyCycle(100)
print('Arrived')
break
else:
print(x_position_front)
FR.ChangeDutyCycle(6.5)
FL.ChangeDutyCycle(8)
RR.ChangeDutyCycle(6.5)
RL.ChangeDutyCycle(8)
print("tada!")
#cleanup
GPIO.cleanup
curses.nobreak()
screen.keypad(0)
curses.echo()
curses.endwin()
def pozyx_position_euler_pub():
#position_pub = rospy.Publisher(
# 'pozyx_positioning', Point32, queue_size=100)
position_pub = rospy.Publisher('/tag_pose0', Point, queue_size=100)
#euler_pub = rospy.Publisher(
# 'pozyx_euler_angles', EulerAngles, queue_size=100)
euler_pub = rospy.Publisher('/orientation', Point, queue_size=100)
vx = 0.0
vy = 0.0
vth = 0.0
rospy.init_node('position_euler_pub') #PROVA
#pub odom msg for move_base
odom_pub = rospy.Publisher("odom", Odometry, queue_size=50)
odom_broadcaster = tf.TransformBroadcaster()
current_time = rospy.Time.now()
last_time = rospy.Time.now()
#rospy.init_node('position_euler_pub')
try:
# MOD @ 12-08-2020
serial_port = pypozyx.get_first_pozyx_serial_port()
pozyx = pypozyx.PozyxSerial(serial_port)
#BKP -> pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device)
except:
rospy.loginfo("Pozyx not connected")
return
while not rospy.is_shutdown():
#################### COORDS ####################
coords = pypozyx.Coordinates()
pozyx.doPositioning(coords, remote_id=remote_id)
# convert from mm to m
#rospy.loginfo("POS_1: %s m" % (str(coords)))
coords.x = coords.x / 1000.0
coords.y = coords.y / 1000.0
coords.z = coords.z / 1000.0
#rospy.loginfo("POS_1: %s m" % (str(coords)))
'''
if records < n_samples:
coords.x += coords.x
coords.y += coords.y
coords.z += coords.z
records = 'done'
continue
elif records == 'done':
coords.x /= n_samples
coords.y /= n_samples
coords.z /= n_samples
records = 'finish'
continue
'''
position_pub.publish(coords.x, coords.y, coords.z)
#################### ANGLES #####################
euler_angles = pypozyx.EulerAngles()
pozyx.getEulerAngles_deg(euler_angles)
# convert from degrees to radians ( *3.14/180 )
euler_angles.heading = numpy.deg2rad(euler_angles.heading)
euler_angles.roll = numpy.deg2rad(euler_angles.roll)
euler_angles.pitch = numpy.deg2rad(euler_angles.pitch)
rospy.loginfo("POS: %s, ANGLES: %s" % (str(coords), str(euler_angles)))
euler_pub.publish(euler_angles.heading, euler_angles.roll,
euler_angles.pitch)
#################### PUBLISHING ODOM TF ################
# combine pos and angle info and publish as odom msg
current_time = rospy.Time.now()
# odometry is 6DOF, created quaternion from yaw
odom_quat = tf.transformations.quaternion_from_euler(
0, 0, euler_angles.heading)
# publish the transform over tf
odom_broadcaster.sendTransform((coords.x, coords.y, 0.), odom_quat,
current_time, tag_frame_ID,
UWB_frame_ID)
# publish the odometry message over ROS
odom = Odometry()
odom.header.stamp = current_time
odom.header.frame_id = UWB_frame_ID
odom.pose.pose = Pose(Point(coords.x, coords.y, 0.),
Quaternion(*odom_quat))
odom.child_frame_id = tag_frame_ID
odom.twist.twist = Twist(Vector3(vx, vy, 0),
Vector3(0, 0, vth)) # set the velocity
odom_pub.publish(odom)
last_time = current_time
def main():
try:
odom_data = com.rxData()
pub.publish(odom_data)
except Exception as e:
rospy.logwarn(e)
pass
com.txData()
if __name__ == "__main__":
rospy.init_node('uwb_node')
serial_port = str(
rospy.get_param('~serial_port', pzx.get_first_pozyx_serial_port()))
frequency = float(rospy.get_param('~frequency', 10))
rate = rospy.Rate(frequency)
pozyx = pzx.PozyxSerial(serial_port)
destination = rospy.get_param('~destination', 0x6e2f)
tx_topic = str(rospy.get_param('~tx_topic', 'uwb_server_tx'))
rx_topic = str(rospy.get_param('~rx_topic', 'uwb_server_rx'))
pub = rospy.Publisher(rx_topic, Odometry, queue_size=10)
com = Communicate(pozyx, destination)
rospy.Subscriber(tx_topic, Odometry, com.odomData)
while not rospy.is_shutdown():
main()
status &= self.pozyx.setLed(2, (distance < 3 * range_step_mm), id)
status &= self.pozyx.setLed(1, (distance < 4 * range_step_mm), id)
return status
if __name__ == "__main__":
# Check for the latest PyPozyx version. Skip if this takes too long or is not needed by setting to False.
check_pypozyx_version = True
if check_pypozyx_version:
perform_latest_version_check()
# hardcoded way to assign a serial port of the Pozyx
serial_port = 'COM12'
# the easier way
serial_port = get_first_pozyx_serial_port()
if serial_port is None:
print("No Pozyx connected. Check your USB cable or your driver!")
quit()
remote_id = 0x605D # the network ID of the remote device
remote = False # whether to use the given remote device for ranging
if not remote:
remote_id = None
destination_id = 0x6e66 # network ID of the ranging destination
# distance that separates the amount of LEDs lighting up.
range_step_mm = 1000
# the ranging protocol, other one is PozyxConstants.RANGE_PROTOCOL_PRECISION
ranging_protocol = PozyxConstants.RANGE_PROTOCOL_PRECISION
status &= self.pozyx.setLed(2, (distance < 3 * range_step_mm), id)
status &= self.pozyx.setLed(1, (distance < 4 * range_step_mm), id)
return status
if __name__ == "__main__":
# Check for the latest PyPozyx version. Skip if this takes too long or is not needed by setting to False.
check_pypozyx_version = True
if check_pypozyx_version:
perform_latest_version_check()
# hardcoded way to assign a serial port of the Pozyx
serial_port = 'COM12'
# the easier way
serial_port = get_first_pozyx_serial_port(
) # should be /dev/ttyACM0 for my computer
if serial_port is None:
print("No Pozyx connected. Check your USB cable or your driver!")
quit()
remote_id = 0x766e # the network ID of the remote device (device 1)
remote = True # whether to use the given remote device for ranging
if not remote:
remote_id = None
destination_id = 0x7607 # network ID of the ranging destination (device 2)
# distance that separates the amount of LEDs lighting up.
range_step_mm = 1000
# the ranging protocol, other one is PozyxConstants.RANGE_PROTOCOL_PRECISION
ranging_protocol = PozyxConstants.RANGE_PROTOCOL_PRECISION
