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)