def remoteRegWrite(self, destination, address, data):
"""Performs regWrite on a remote Pozyx device.
Args:
destination: Network ID of destination device. integer ID or NetworkID(ID).
address: Register address to start the writing operation on.
data: Contains the data to be written. ByteStructure-derived object.
Returns:
POZYX_SUCCESS, POZYX_FAILURE, POZYX_TIMEOUT
"""
if len(data) > PozyxConstants.MAX_BUF_SIZE - 1:
return POZYX_FAILURE
send_data = Data([0, address] + data.data, 'BB' + data.data_format)
status = self.regFunction(PozyxRegisters.WRITE_TX_DATA, send_data,
Data([]))
if status != POZYX_SUCCESS:
return status
self.getInterruptStatus(SingleRegister())
status = self.sendTXWrite(destination)
if status != POZYX_SUCCESS:
return status
return self.checkForFlag(PozyxBitmasks.INT_STATUS_FUNC, 0.5)
def remoteRegWrite(self, destination, address, data):
"""
Performs regWrite on a remote Pozyx device.
Args:
destination: Network ID of destination device. integer ID or NetworkID(ID).
address: Register address to start the writing operation on.
data: Contains the data to be written. ByteStructure-derived object.
Returns:
POZYX_SUCCESS, POZYX_FAILURE, POZYX_TIMEOUT
"""
if is_reg_writable(address) == 0:
return POZYX_FAILURE
if len(data) > MAX_BUF_SIZE - 1:
return POZYX_FAILURE
send_data = Data([0, address] + data.data, 'BB' + data.data_format)
status = self.regFunction(POZYX_TX_DATA, send_data, Data([]))
if status == POZYX_FAILURE:
return status
self.regRead(POZYX_INT_STATUS, SingleRegister())
params = Data([destination, 0x04], 'HB')
status = self.regFunction(POZYX_TX_SEND, params, Data([]))
if status == POZYX_FAILURE:
return status
return self.checkForFlag(POZYX_INT_STATUS_FUNC, 0.1)
def __init__(self, comms, anchors, IS_UAV, postimeout=0.1):
super(PozyxPositionProcess, self).__init__()
self.postimeout = postimeout
self.comms = comms
self.my_ip = socket.gethostbyname(socket.gethostname())
self.my_ip_num = int(self.my_ip.split('.')[-1])
self.server = None
self.client = None
self.pos = None
self.pos_error = None
self.rngs = None
self.IS_UAV = IS_UAV
self.anchors = anchors
self.pozyx = PozyxInterface(self.anchors)
self.pozyx.print_configuration_result()
self.pozyx_pos_generator = position_generator(self.pozyx, self.IS_UAV)
self.sensor_data = SensorData()
self.tmp_sensor_data = SensorData()
self.sensor_calibration_status = SingleRegister()
self.stop = False
def checkForFlag(self, interrupt_flag, timeout_s, interrupt=None):
"""Performs waitForFlag_safe and checks against errors or timeouts.
This abstracts the waitForFlag status check routine commonly encountered
in more complex library functions and checks the given flag against
the error flag.
Args:
interrupt_flag: Flag of interrupt type to check the interrupt register against.
timeout_s: duration to wait for the interrupt in seconds
Kwags:
interrupt: Container for the interrupt status register data.
Returns:
POZYX_SUCCESS, POZYX_FAILURE, POZYX_TIMEOUT
"""
if interrupt is None:
interrupt = SingleRegister()
if self.waitForFlag_safe(interrupt_flag | POZYX_INT_STATUS_ERR,
timeout_s, interrupt):
if (interrupt[0] & POZYX_INT_STATUS_ERR) == POZYX_INT_STATUS_ERR:
return POZYX_FAILURE
else:
return POZYX_SUCCESS
else:
return POZYX_TIMEOUT
def validatePozyx(self):
"""Validates whether the connected device is indeed a Pozyx device"""
whoami = SingleRegister()
if self.getWhoAmI(whoami) != POZYX_SUCCESS:
raise PozyxConnectionError("Connected to device, but couldn't read serial data. Is it a Pozyx?")
if whoami.value != 0x43:
raise PozyxConnectionError("POZYX_WHO_AM_I returned 0x%0.2x, something is wrong with Pozyx." % whoami.value)
def set_data_structures(self):
self.containers = []
self.byte_size = 0
self.data_format = ''
for index, sensor in enumerate(self.SENSOR_ORDER):
if self.flags & (1 << index):
self.add_sensor(sensor)
if self.has_ranges():
self.byte_size += 1
self.data_format += 'B'
self.containers.append(SingleRegister())
def waitForFlag(self, interrupt_flag, timeout_s, interrupt=None):
"""
Waits for a certain interrupt flag to be triggered, indicating that
that type of interrupt occured.
Args:
interrupt_flag: Flag indicating interrupt type.
timeout_s: time in seconds that POZYX_INT_STATUS will be checked
for the flag before returning POZYX_TIMEOUT.
Kwargs:
interrupt: Container for the POZYX_INT_STATUS data
Returns:
POZYX_SUCCESS, POZYX_FAILURE, POZYX_TIMEOUT
"""
if interrupt is None:
interrupt = SingleRegister()
return self.waitForFlagSafe(interrupt_flag, timeout_s, interrupt)
def remoteRegRead(self, destination, address, data):
"""
Performs regRead on a remote Pozyx device.
Args:
destination: Network ID of destination device. integer ID or NetworkID(ID).
address: Register address to start the read operation from.
data: Container for the read data. ByteStructure-derived object.
Returns:
POZYX_SUCCESS, POZYX_FAILURE, POZYX_TIMEOUT
"""
if dataCheck(destination):
destination = destination[0]
if is_reg_readable(address) == 0:
return POZYX_FAILURE
if len(data) > MAX_BUF_SIZE:
return POZYX_FAILURE
if destination == 0:
return POZYX_FAILURE
send_data = Data([0, address, data.byte_size])
status = self.regFunction(POZYX_TX_DATA, send_data, Data([]))
if status == POZYX_FAILURE:
return status
self.regRead(POZYX_INT_STATUS, SingleRegister())
params = Data([destination, 0x02], 'HB')
status = self.regFunction(POZYX_TX_SEND, params, Data([]))
if status == POZYX_FAILURE:
return status
status = self.checkForFlag(POZYX_INT_STATUS_FUNC, 1)
if status == POZYX_SUCCESS:
rx_info = Data([0, 0], 'HB')
self.regRead(POZYX_RX_NETWORK_ID, rx_info)
if rx_info[0] == destination and rx_info[1] == data.byte_size:
status = self.readRXBufferData(data)
return status
else:
return POZYX_FAILURE
return status
def remoteRegFunction(self, destination, address, params, data):
"""
Performs regFunction on a remote Pozyx device.
Args:
destination: Network ID of destination device. integer ID or NetworkID(ID).
address: Register address to start the read operation from.
params: Parameters for the register function. ByteStructure-derived object of uint8s.
data: Container for the data returned by the register function. ByteStructure-derived object.
Returns:
POZYX_SUCCESS, POZYX_FAILURE, POZYX_TIMEOUT
"""
if is_functioncall(address) == 0:
return POZYX_FAILURE
send_data = Data([0, address] + params.data, 'BB' + params.data_format)
status = self.regFunction(POZYX_TX_DATA, send_data, Data([]))
if status == POZYX_FAILURE:
return status
self.regRead(POZYX_INT_STATUS, SingleRegister())
tx_params = Data([destination, 0x08], 'HB')
status = self.regFunction(POZYX_TX_SEND, tx_params, Data([]))
if status == POZYX_FAILURE:
return status
status = self.checkForFlag(POZYX_INT_STATUS_FUNC, 1)
if status == POZYX_SUCCESS:
rx_info = Data([0, 0], 'HB')
self.regRead(POZYX_RX_NETWORK_ID, rx_info)
if rx_info[0] == destination and rx_info[1] == data.byte_size + 1:
return_data = Data([0] + data.data, 'B' + data.data_format)
status = self.readRXBufferData(return_data)
if status == POZYX_FAILURE:
return status
if len(return_data) > 1:
data.load(return_data[1:])
return return_data[0]
else:
return POZYX_FAILURE
return status
def waitForFlagSafe(self, interrupt_flag, timeout_s, interrupt=None):
"""Performs waitForFlag in polling mode.
Args:
interrupt_flag: Flag of interrupt type to check the interrupt register against.
timeout_s: duration to wait for the interrupt in seconds.
interrupt (optional): Container for the interrupt status register data.
Returns:
POZYX_SUCCESS, POZYX_FAILURE, POZYX_TIMEOUT
"""
if interrupt is None:
interrupt = SingleRegister()
start = time()
while (time() - start) < timeout_s:
status = self.getInterruptStatus(interrupt)
if (interrupt[0] & interrupt_flag) and status == POZYX_SUCCESS:
return True
sleep(PozyxConstants.DELAY_POLLING)
return False
def print_configuration_result(self):
"""Prints the anchor configuration result in a human-readable way."""
list_size = SingleRegister()
status = self.pozyx.getDeviceListSize(list_size)
log("List size: {0}".format(list_size[0]))
if list_size[0] != len(self.anchors):
self.printPublishErrorCode("configuration")
return
device_list = DeviceList(list_size=list_size[0])
status = self.pozyx.getDeviceIds(device_list)
log("Calibration result:")
log("Anchors found: {0}".format(list_size[0]))
log("Anchor IDs: ", device_list)
for i in range(list_size[0]):
anchor_coordinates = Coordinates()
status = self.pozyx.getDeviceCoordinates(device_list[i],
anchor_coordinates)
log("ANCHOR,0x%0.4x, %s" %
(device_list[i], str(anchor_coordinates)))
def remoteRegRead(self, destination, address, data):
"""Performs regRead on a remote Pozyx device.
Args:
destination: Network ID of destination device. integer ID or NetworkID(ID).
address: Register address to start the read operation from.
data: Container for the read data. ByteStructure-derived object.
Returns:
POZYX_SUCCESS, POZYX_FAILURE, POZYX_TIMEOUT
"""
if dataCheck(destination):
destination = destination[0]
if len(data) > PozyxConstants.MAX_BUF_SIZE:
return POZYX_FAILURE
if destination == 0:
return POZYX_FAILURE
send_data = Data([0, address, data.byte_size])
status = self.regFunction(PozyxRegisters.WRITE_TX_DATA, send_data,
Data([]))
if status != POZYX_SUCCESS:
return status
self.getInterruptStatus(SingleRegister())
status = self.sendTXRead(destination)
if status != POZYX_SUCCESS:
return status
status = self.checkForFlag(PozyxBitmasks.INT_STATUS_FUNC, 1)
if status == POZYX_SUCCESS:
rx_info = RXInfo()
self.getRxInfo(rx_info)
if rx_info.remote_id == destination and rx_info.amount_of_bytes == data.byte_size:
status = self.readRXBufferData(data)
return status
else:
return POZYX_FAILURE
return status
def remoteRegFunction(self, destination, address, params, data):
"""Performs regFunction on a remote Pozyx device.
Args:
destination: Network ID of destination device. integer ID or NetworkID(ID).
address: Register address to start the read operation from.
params: Parameters for the register function. ByteStructure-derived object of uint8s.
data: Container for the data returned by the register function. ByteStructure-derived object.
Returns:
POZYX_SUCCESS, POZYX_FAILURE, POZYX_TIMEOUT
"""
send_data = Data([0, address] + params.data, 'BB' + params.data_format)
status = self.regFunction(PozyxRegisters.WRITE_TX_DATA, send_data,
Data([]))
if status != POZYX_SUCCESS:
return status
self.getInterruptStatus(SingleRegister())
status = self.sendTXFunction(destination)
if status != POZYX_SUCCESS:
return status
status = self.checkForFlag(PozyxBitmasks.INT_STATUS_FUNC, 1)
if status == POZYX_SUCCESS:
rx_info = RXInfo()
self.getRxInfo(rx_info)
if rx_info.remote_id == destination and rx_info.amount_of_bytes == data.byte_size + 1:
return_data = Data([0] + data.data, 'B' + data.data_format)
status = self.readRXBufferData(return_data)
if status != POZYX_SUCCESS:
return status
if len(return_data) > 1:
data.load(return_data[1:])
return return_data[0]
else:
return POZYX_FAILURE
return status
def waitForFlag_safe(self, interrupt_flag, timeout_s, interrupt=None):
"""
Performs waitForFlag in polling mode.
Args:
interrupt_flag: Flag of interrupt type to check the interrupt register against.
timeout_s: duration to wait for the interrupt in seconds.
Kwags:
interrupt: Container for the interrupt status register data.
Returns:
POZYX_SUCCESS, POZYX_FAILURE, POZYX_TIMEOUT
"""
if interrupt is None:
interrupt = SingleRegister()
start = time()
while (time() - start < timeout_s):
sleep(POZYX_DELAY_POLLING)
status = self.regRead(POZYX_INT_STATUS, interrupt)
if (interrupt[0] & interrupt_flag) and status == POZYX_SUCCESS:
return True
return False
from pypozyx import (PozyxSerial, get_first_pozyx_serial_port)
from pypozyx.definitions.registers import POZYX_WHO_AM_I
from pypozyx.structures.generic import Data, SingleRegister
#from pythonosc.udp_client import SimpleU
serial_port = get_first_pozyx_serial_port()
print(serial_port)
pozyx = PozyxSerial(serial_port)
data = Data([0] * 5)
#remote_id = 0x675b
#remote_id = 0x0
#pozyx.getRead(POZYX_WHO_AM_I, data, remote_id=remote_id)
#print('who am i: 0x%0.2x' % data[0])
#print('firmware version: 0x%0.2x' % data[1])
#print('hardware version: 0x%0.2x' % data[2])
#print('self test result: %s' % bin(data[3]))
#print('error: 0x%0.2x' % data[4])
whoami = SingleRegister()
pozyx.regRead(POZYX_WHO_AM_I, whoami) # which is pozyx.getWhoAmI(whoami)
print(whoami)
def print_error_code(self, operation):
"""Prints the Pozyx's error"""
error_code = SingleRegister()
self.pozyx.getErrorCode(error_code)
log("ERROR %s, local error code %s" % (operation, str(error_code)))
def clearInterruptStatus(self):
interrupt = SingleRegister()
return self.getInterruptStatus(interrupt)