class Main: """A class for establishing connections""" def start_db_hdlr(self, db): self.db_hdlr = DatabaseHandler(db) self.db_hdlr.flush_rss_readings() self.db_hdlr.check_pos_tbl() self.db_hdlr.check_rsl_tbl() def start_net_srv(self): self.net_srv = NetworkServer() self.net_srv.start() def start_serial_conn(self, port, timeout): self.serial_conn = SerialConnection(port, timeout) self.serial_conn.open() self.serial_conn.flush_input() def start_trilat(self): self.trilat = Trilateration() self.trilat.start() def run(self): try: while True: data = self.serial_conn.read().strip() tag_id = data[:4] rss = data[4:] self.db_hdlr.ins_reading(2, tag_id, rss) except KeyboardInterrupt: self.db_hdlr.close_db() self.net_srv.stop() self.net_srv.join(60) self.trilat.stop() self.trilat.join(20) self.serial_conn.close()
class RcvThread(threading.Thread):
"""A tread class that receives readings over the network"""
def __init__(self, cli_sock, node_name):
threading.Thread.__init__(self)
self.cli_sock = cli_sock
self.node_name = node_name
self.db_hdlr = DatabaseHandler('measurements.db')
self.alive = True
def run(self):
# Keep the receive streaming socket open
fd = self.cli_sock.fileno()
while self.alive:
# Check if there is something to read on the socket
# or if the socket has thrown exceptions for 20s
rlist, wlist, xlist = select.select(
[fd], [], [fd], 5)
if xlist:
break
if rlist:
# Receive data on the socket
data = self.cli_sock.recv(7).strip()
tag_id = data[:4]
rss = data[4:]
self.db_hdlr.ins_reading(self.node_name, tag_id, rss)
#print data, len(data), self.node_name
# Close this client socket
self.cli_sock.close()
self.db_hdlr.close_db();
def stop(self):
self.alive = False
def get_node_name(self):
return self.node_name
class Trilateration(threading.Thread):
"""A class computing a tags position based on reader measurements"""
def __init__(self):
threading.Thread.__init__(self)
self.db_hdlr = DatabaseHandler('measurements.db')
self.alive = True
node0 = [(80, 65), (64, 62), (61, 57), (56, 53), (52, 48), (47, 46), (45, 44), (43, 42), (41, 0)]
node1 = [(77, 63), (62, 58), (57, 55), (54, 53), (52, 49), (48, 47), (46, 44), (43, 42), (41, 0)]
node2 = [(78, 64), (63, 60), (59, 56), (55, 54), (53, 49), (48, 45), (44, 43), (42, 41), (40, 0)]
self.tables = {0 : node0, 1 : node1, 2 : node2}
def run(self):
while self.alive:
rowcount = self.db_hdlr.get_rowcount()
if rowcount < 3:
continue
positions = self.db_hdlr.get_positions()
readings = self.db_hdlr.get_readings()
self.trilateration(positions, readings)
sleep(3)
self.db_hdlr.close_db()
def stop(self):
self.alive = False
def convert_readings(self, readings):
dists = []
for node_name, rss in readings:
#dist = 1 / math.sqrt(rss)
if rss <= 61:
if node_name == 0:
rss += 3
elif node_name == 1:
rss += 6
else:
rss += 12
dist = self.rss_to_dist(node_name, rss)
self.db_hdlr.update_dist(node_name, dist)
dists.append(dist)
print 'Node', node_name, 'with rss', rss, 'has dist', dist
return dists
def rss_to_dist(self, node_name, rss):
rss_tbl = self.tables[node_name]
new_min = 0
for old_min, old_max in rss_tbl:
if rss <= old_min and rss >= old_max:
return self.linear_convertion(rss, old_min, old_max, new_min)
new_min += 1
def linear_convertion(self, rss, old_min, old_max, new_min):
#old_range = float(abs(old_max - old_min))
#return float(abs(rss - old_min)) / old_range + new_min
old_range = float(old_min - old_max)
return ((float(old_min - rss) / old_range) + new_min)
def trilateration(self, positions, readings):
maxzero = 0.0
p1 = numpy.array(positions[0])
p2 = numpy.array(positions[1])
p3 = numpy.array(positions[2])
r1, r2, r3 = self.convert_readings(readings)
ex = p2 - p1
# d = |p2 - p1|
d = numpy.linalg.norm(ex)
if d <= maxzero:
# p1 and p2 are concentric
print 'p1 and p2 are concentric'
return
# ex = (p2 - p1) / |p2 - p1|
ex = ex / d
# t1 = p3 - p1
t1 = p3 - p1
# i = ex . (p3 - p1)
i = numpy.dot(ex, t1)
# t2 = ex(ex . (p3 - p1))
t2 = ex * i
# ey = p3 - p1 - ex(ex . (p3 - p1))
ey = t1 - t2
# t = |p3 - p1 - ex(ex . (p3 - p1))|
t = numpy.linalg.norm(ey)
if t > maxzero:
# p1, p2, p3 are not collinear
# ey = (t1 - t2) / |t1 - t2|
ey = ey / t
# j = ey . (p3 - p1)
j = numpy.dot(ey, t1)
else:
j = 0.0
if math.fabs(j) <= maxzero:
# p1, p2, p3 are collinear
# Is point p1 +/- (r1 along the axis) the intersection?
ts = [p1 + ex*r1, p1 - ex*r1]
for t in ts:
if ((math.fabs(numpy.linalg.norm(p2 / t) - r2) <= maxzero) and
(math.fabs(numpy.linalg.norm(p3 / t) - r3) <= maxzero)):
p = t
print 'p1, p2, p3 are collinear'
print 'p1 +/- ex*r1 is the only intersection point'
print 'p', p
self.db_hdlr.update_pnt(p,p)
return
print 'p1, p2, p3 are collinear but no solution exists'
return
# ez = ex x ey
ez = numpy.cross(ex, ey)
print 'ex', ex
print 'ey', ey
print 'ez', ez
print 'd', d
print 'i', i
print 'j', j
# x = (r1^2 - r2^2) / 2d + d / 2
x = (math.pow(r1, 2) - math.pow(r2, 2)) / (2*d) + d / 2
print 'x', x
# y = (r1^2 - r3^2 + i^2) / (2*j) + j / 2 - x * i / j
y = ((math.pow(r1, 2) - math.pow(r3, 2) + math.pow(i, 2)) / (2*j)
+ j / 2 - x * i / j)
print 'y', y
# z = r1^2 - x^2 - y^2
z = math.pow(r1, 2) - math.pow(x, 2) - math.pow(y, 2)
# Check the sign of the z dimension
if z < -maxzero:
print 'z < 0'
#return
elif z > maxzero:
z = math.sqrt(z)
print 'z > 0'
else:
z = 0
print 'z = 0'
#z = 0
print 'z', z
t = p1 + x*ex + y*ey
p1 = t + z*ez
print 'p1', p1
p2 = t - z*ez
print 'p2', p2
self.db_hdlr.update_pnt(p1,p2)
