class Controller(object):
"""description of class"""
def CheckStatus(self, status):
if status == PozyxConstants.STATUS_FAILURE:
raise ConnectionError('Pozyx function failed')
if status == PozyxConstants.STATUS_TIMEOUT:
raise ConnectionError('Pozyx function timed out')
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 ConfigureAnchor(self, anchorId):
for devCoords in self.anchors:
if devCoords.network_id == anchorId:
position = devCoords.pos
break
if anchorId == self.id:
anchorId = None
status = self.pozyx.setCoordinates(position, anchorId)
self.CheckStatus(status)
def ConfigureTag(self, tagId):
for anchor in self.anchors:
status = self.pozyx.addDevice(anchor, tagId)
self.CheckStatus(status)
status = self.pozyx.configureAnchors(self.anchors,
PozyxConstants.ANCHOR_SELECT_AUTO,
tagId)
self.CheckStatus(status)
mode = SingleRegister(PozyxConstants.GPIO_DIGITAL_INPUT)
pullup = SingleRegister(PozyxConstants.GPIO_PULL_UP)
status = self.pozyx.setConfigGPIO(1, mode, pullup, tagId)
self.CheckStatus(status)
status = self.pozyx.setConfigGPIO(2, mode, pullup, tagId)
self.CheckStatus(status)
status = self.pozyx.setConfigGPIO(3, mode, pullup, tagId)
self.CheckStatus(status)
status = self.pozyx.setConfigGPIO(4, mode, pullup, tagId)
self.CheckStatus(status)
def ConfigureUWB(self,
id,
txPower=33.0,
channel=1,
bitrate=0,
gain=67,
prf=2,
preamble=40):
if txPower < 0.0 or txPower > 33.0:
raise ValueError("Invalid txPower")
if id == self.id:
id = None
settings = UWBSettings()
settings.bitrate = bitrate
settings.channel = channel
settings.gain_db = gain
settings.plen = preamble
settings.prf = prf
status = self.pozyx.setUWBSettings(settings, id)
self.CheckStatus(status)
status = self.pozyx.setTxPower(txPower, id)
self.CheckStatus(status)
def GetError(self):
code = SingleRegister()
status = self.pozyx.getErrorCode(code)
self.CheckStatus(status)
return self.pozyx.getErrorMessage(code)
def GetCalibrationStatus(self, tagId):
result = SingleRegister()
status = self.pozyx.getCalibrationStatus(result, tagId)
self.CheckStatus(status)
value = dict()
value['magnetic'] = result.value & 3
value['accelerometer'] = (result.value & 12) >> 2
value['gyroscope'] = (result.value & 48) >> 4
value['system'] = (result.value & 192) >> 6
return value
def GetGPIO(self, tagId, pinId):
value = SingleRegister()
status = self.pozyx.getGPIO(pinId, value, tagId)
self.CheckStatus(status)
return value.value == 1
def GetPosition(self, tagId):
target = Coordinates()
status = self.pozyx.doPositioning(
target,
PozyxConstants.DIMENSION_3D,
algorithm=PozyxConstants.POSITIONING_ALGORITHM_UWB_ONLY,
remote_id=tagId)
self.CheckStatus(status)
result = dict()
result['x'] = target.x
result['y'] = target.y
result['z'] = target.z
return target
def GetEulerAngles(self, tagId):
result = EulerAngles()
status = self.pozyx.getEulerAngles_deg(result, tagId)
self.CheckStatus(status)
res = dict()
res['heading'] = result.heading
res['roll'] = result.roll
res['pitch'] = result.pitch
return res
def GetPressure(self, tagId):
data = Pressure()
status = self.pozyx.getPressure_Pa(data, tagId)
self.CheckStatus(status)
return {'pressure': data.value}
def GetTemperature(self, tagId):
data = Temperature()
status = self.pozyx.getTemperature_c(data, tagId)
self.CheckStatus(status)
return {'temperature': data.value}
def DiscoverTags(self):
status = self.pozyx.doDiscovery(PozyxConstants.DISCOVERY_TAGS_ONLY)
self.CheckStatus(status)
count = SingleRegister()
status = self.pozyx.getDeviceListSize(count)
self.CheckStatus(status)
allIds = DeviceList(list_size=count.value)
status = self.pozyx.getDeviceIds(allIds)
self.CheckStatus(status)
tagIds = list()
for id in allIds:
any = False
for tagId in self.anchors:
if tagId.network_id == id:
any = True
if not any:
tagIds.append(id)
return tagIds
from configparser import ConfigParser
#read setup file
cParser = ConfigParser()
cParser.read('config.ini')
#Get the connected device
connectedDevice = int(cParser.get('default', 'connected_id'), 16)
#Check if connected
serial_port = get_first_pozyx_serial_port()
if serial_port is not None:
pozyx = PozyxSerial(serial_port)
print("Connection success!")
else:
print("No Pozyx port was found")
exit()
try:
#Does discovery on the device and print the found devices
if pozyx.doDiscovery(discovery_type=PozyxConstants.DISCOVERY_ALL_DEVICES
) == POZYX_SUCCESS:
print("Connected Device: " + hex(connectedDevice))
print("Found devices:")
pozyx.printDeviceList(include_coordinates=False)
#Exception exit
except:
print("")
print("Exit program")
