Newer
Older
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
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
__author__ = 'moritz'
import serial
import sys
import time
import itertools
import math
import functools
import numpy as np
import scipy.optimize
ADVERTISERS = {"00:07:80:52:64:e6": [2.5, 5, 1],
"00:07:80:7e:c3:68": [1, 0.5, 0.5],
"00:07:80:7e:c3:7b": [5, 2, 1],
"00:07:80:68:1c:9c": [0.5, .5, 2.2],
"00:07:80:68:28:29": [6, 5, 1],
"00:07:80:68:28:67": [3, 3, 0.5],
"00:07:80:79:1f:f1": [6, .5, 1.8],
"00:07:80:c0:ff:ee": [0.5, 5, 1 ]
}
X=0
Y=1
Z=2
C = - math.log(10) / 20
def read_packet(s):
s.timeout = None
header = s.read(4)
s.timeout = 0.1
# print(repr(header[0]))
packet_length = ((header[0] & 0x03) << 8) + header[1]
class_id = header[2]
cmd_id = header[3]
payload = s.read(packet_length)
return class_id, cmd_id, payload
def vec_sum (a, b):
return [x + y for x, y in zip(a, b)]
def vec_sub (a, b):
return [x - y for x, y in zip(a, b)]
def vec_abs (a):
return sum([x ** 2 for x in a]) ** 0.5
def vec_vprod(a,b):
return a[Y] * b[Z] - a[Z] * b[Y], a[Z] * b[X] - a[X] * b[Z], a[X] * b[Y] - a[Y] * b[X]
def vec_sprod(a, b):
return sum([x * y for x, y in zip(a, b)])
def vec_scale(a, b):
return [b * x for x in a]
def get_position(advertisers, debug=False, callback=None):
if len(advertisers) < 4:
return None
# get distances s according to the field strengths
s = np.array([x.get_distance() for x in advertisers])
# get starting position sx as offset next to the strongest advertiser
sx = vec_sub(min(map(lambda a: (a.get_distance(), a.pos), advertisers))[1], (0,0,0.2))
beacon_positions = np.array([x.pos for x in advertisers])
beacon_variance = np.array([x.get_distance_error() for x in advertisers])
a = np.array([1] * len(advertisers))
def error (x):
# squared error = sum of [(setpoint - measurement)**2]
# get distances to the beacons
r=(((x-beacon_positions)**2).sum(axis=1))**0.5
# get all [(meas distance - calc distance)**2]
errors = (s-r)**2
# cut off high errors to decrease the influence of values that are far away
errors = np.fmin(errors, 0*r+1**2)
# get scaling factors (beacon_variance, replace None by some low value)
scaling = []
for bv in beacon_variance:
if bv is None: scaling.append(1e-20) # there is no variance
elif bv == 0: scaling.append(1e-20) # 0 cannot be inverted
else: scaling.append(1/bv) #
scaling = np.array(scaling)
# not working because python doesn't lazy evaluate arrays
# scaling = map(lambda s: [1e-6,1/s][s in [float, int, long]], beacon_variance)
# return sum of all [(meas distance - calc distance)**2]
return (errors).sum()
return (errors * scaling).sum()
# fmin works since it doesn't need the brocken error gradient funktion
return scipy.optimize.fmin(error, sx, ftol=1e-2, disp=debug, callback=callback)
class Advertiser ():
def __init__(self, mac, pos, tx_power):
self.mac = mac
self.pos = pos
self.tx_power = tx_power
self.rssi = {}
def clean_measurements(self, old=2):
now = time.time()
tmp = {}
for ts in self.rssi:
if now - ts < old:
tmp[ts] = self.rssi[ts]
self.rssi = tmp
def add_measurement(self, rssi):
self.clean_measurements()
self.rssi[time.time()] = rssi
def get_medium_rssi(self):
self.clean_measurements()
if len(self.rssi) != 0:
return sorted(self.rssi.values())[len(self.rssi) // 2]
else:
return None
def get_middled_rssi(self):
self.clean_measurements()
if len(self.rssi) != 0:
return functools.reduce(lambda x, y: x + y, self.rssi.values()) / float(len(self.rssi))
else:
return None
def get_rssi_error(self):
if len(self.rssi) >= 2:
m = self.get_middled_rssi()
return functools.reduce(lambda x, y: x + y, [(v - m) ** 2 for v in self.rssi.values()]) / (len(self.rssi) - 1)
else:
return None
def get_distance(self):
if self.get_middled_rssi() != None:
ratio_power = self.tx_power - self.get_middled_rssi()
return (10 ** (ratio_power / 10.)) ** 0.5
else:
return None
def get_distance_error(self):
try: return C * self.get_distance() * self.get_rssi_error()
except: return None
def __str__(self):
try:
return "%s: (%d/%0.2f|%0.2f|%d) %0.5f+-%0.3f" % (self.mac, self.get_medium_rssi(),
self.get_middled_rssi(),
self.get_rssi_error(),
self.tx_power,
self.get_distance(),
self.get_distance_error())
except TypeError:
return self.mac + ": - "
def __gt__(self, other):
self.get_middled_rssi() > other.get_middled_rssi()
def __lt__(self, other):
self.get_middled_rssi() < other.get_middled_rssi()
def __eq__(self, other):
self.get_middled_rssi() == other.get_middled_rssi()
if __name__ == "__main__":
s = serial.Serial(sys.argv[1], baudrate=25600, rtscts=True)
s.timeout = 0.1
s.readall()
s.write(b"\x00\x01\x06\x02\x01")
devices = {}
with open("%d.log" % (time.time()), "w") as fd:
try:
while True:
class_id, cmd_id, payload = read_packet(s)
if class_id == 0x06 and cmd_id == 0x00 and len(payload) == 0x29:
rssi = payload[0] - 0xFF
mac = ":".join(map(lambda c: "%02x" % (c,), payload[7:1:-1]))
tx_power = payload[-1] - 0xFF
# print ("a:", mac, repr(devices))
if mac not in devices.keys():
if mac in ADVERTISERS:
devices[mac] = Advertiser(mac, ADVERTISERS[mac], tx_power)
else:
devices[mac] = Advertiser(mac, None, tx_power)
devices[mac].add_measurement(rssi)
pos = get_position(list(filter(lambda x: x.pos != None and x.get_distance() != None, devices.values())))
print("\x1b[2J")
for i in sorted(devices):
print(devices[i])
print (repr(pos))
except KeyboardInterrupt:
s.write(b"\x00\x00\x06\x04")