-
Notifications
You must be signed in to change notification settings - Fork 2
/
Copy pathpozyx.py
157 lines (131 loc) · 6.71 KB
/
pozyx.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
#!/usr/bin/env python
"""
The Pozyx ready to localize tutorial (c) Pozyx Labs
Please read the tutorial that accompanies this sketch:
https://www.pozyx.io/Documentation/Tutorials/ready_to_localize/Python
This tutorial requires at least the contents of the Pozyx Ready to Localize kit. It demonstrates the positioning capabilities
of the Pozyx device both locally and remotely. Follow the steps to correctly set up your environment in the link, change the
parameters and upload this sketch. Watch the coordinates change as you move your device around!
"""
from __future__ import print_function
from pypozyx import (POZYX_POS_ALG_UWB_ONLY, POZYX_2D, Coordinates, POZYX_SUCCESS, POZYX_ANCHOR_SEL_AUTO,
DeviceCoordinates, PozyxSerial, get_first_pozyx_serial_port, SingleRegister, DeviceList)
import csv
import time
import datetime
class ReadyToLocalize(object):
"""Continuously calls the Pozyx positioning function and prints its position."""
def __init__(self, pozyx, anchors, algorithm=POZYX_POS_ALG_UWB_ONLY, dimension=POZYX_2D, height=277, remote_id=None, csvWriter=None):
self.pozyx = pozyx
self.anchors = anchors
self.algorithm = algorithm
self.dimension = dimension
self.height = height
self.remote_id = remote_id
def setup(self):
"""Sets up the Pozyx for positioning by calibrating its anchor list."""
print("------------POZYX POSITIONING V1.1 -------------")
print("NOTES: ")
print("- No parameters required.")
print()
print("- System will auto start configuration")
print()
print("- System will auto start positioning")
print()
self.pozyx.printDeviceInfo(self.remote_id)
print()
print("------------POZYX POSITIONING V1.1 --------------")
print()
self.pozyx.clearDevices(self.remote_id)
self.setAnchorsManual()
self.printPublishConfigurationResult()
def loop(self):
"""Performs positioning and displays/exports the results."""
position = Coordinates()
status = self.pozyx.doPositioning(
position, self.dimension, self.height, self.algorithm, remote_id=self.remote_id)
if status == POZYX_SUCCESS:
self.printPublishPosition(position)
if csvWriter is not None:
self.writePositionToCsv(position, csvWriter)
else:
self.printPublishErrorCode("positioning")
def printPublishPosition(self, position):
"""Prints the Pozyx's position and possibly sends it as a OSC packet"""
network_id = self.remote_id
if network_id is None:
network_id = 0
print("\rPOS ID {}, x(mm): {pos.x} y(mm): {pos.y} z(mm): {pos.z}".format(
"0x%0.4x" % network_id, pos=position), end='')
def printPublishErrorCode(self, operation):
"""Prints the Pozyx's error and possibly sends it as a OSC packet"""
error_code = SingleRegister()
network_id = self.remote_id
if network_id is None:
self.pozyx.getErrorCode(error_code)
print("LOCAL ERROR %s, %s" % (operation, self.pozyx.getErrorMessage(error_code)), end='\r')
return
status = self.pozyx.getErrorCode(error_code, self.remote_id)
if status == POZYX_SUCCESS:
print("ERROR %s on ID %s, %s" %
(operation, "0x%0.4x" % network_id, self.pozyx.getErrorMessage(error_code)))
else:
self.pozyx.getErrorCode(error_code)
print("ERROR %s, couldn't retrieve remote error code, LOCAL ERROR %s" %
(operation, self.pozyx.getErrorMessage(error_code)))
# should only happen when not being able to communicate with a remote Pozyx.
def setAnchorsManual(self):
"""Adds the manually measured anchors to the Pozyx's device list one for one."""
status = self.pozyx.clearDevices(self.remote_id)
for anchor in self.anchors:
status &= self.pozyx.addDevice(anchor, self.remote_id)
if len(self.anchors) > 4:
status &= self.pozyx.setSelectionOfAnchors(POZYX_ANCHOR_SEL_AUTO, len(self.anchors))
return status
def printPublishConfigurationResult(self):
"""Prints and potentially publishes the anchor configuration result in a human-readable way."""
list_size = SingleRegister()
self.pozyx.getDeviceListSize(list_size, self.remote_id)
print("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])
self.pozyx.getDeviceIds(device_list, self.remote_id)
print("Calibration result:")
print("Anchors found: {0}".format(list_size[0]))
print("Anchor IDs: ", device_list)
for i in range(list_size[0]):
anchor_coordinates = Coordinates()
self.pozyx.getDeviceCoordinates(device_list[i], anchor_coordinates, self.remote_id)
print("ANCHOR, 0x%0.4x, %s" % (device_list[i], str(anchor_coordinates)))
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)))
def writePositionToCsv(self, position, csvWriter):
csvWriter.writerow([str(time.time()), '%.4f' % position.x, '%.4f' % position.y, '%.4f' % position.z])
if __name__ == "__main__":
outputfile = 'output/'+ str(datetime.datetime.now()).replace(' ', '_') + '_pozywx.csv'
# 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 = None
# necessary data for calibration, change the IDs and coordinates yourself
anchors = [DeviceCoordinates(0x6733, 1, Coordinates(4577, 0, 277)),
DeviceCoordinates(0x6765, 1, Coordinates(0, 0, 277)),
DeviceCoordinates(0x6744, 1, Coordinates(-114, 6105, 277)),
DeviceCoordinates(0x6745, 1, Coordinates(4681, 6264, 277))]
algorithm = POZYX_POS_ALG_UWB_ONLY # positioning algorithm to use
dimension = POZYX_2D # positioning dimension
height = 277 # height of device, required in 2.5D positioning
pozyx = PozyxSerial(serial_port)
r = ReadyToLocalize(pozyx, anchors, algorithm, dimension, height, remote_id)
r.setup()
with open(outputfile, mode='w') as csvFile:
csvWriter = csv.writer(csvFile)
csvWriter.writerow(['timestamp', 'X', 'Y', 'Z'])
while True:
r.loop()