def update_xy(): # Init variables global x, y, xMobile, yMobile, xStatic, yStatic, tdf3 x, y = 0, 0 xStatic, yStatic = [], [] xMobile, yMobile = [], [] lxP = lx.Project(mode="2D", solver="LSE") lxObj = None # staticAP = [] # rssiVal = [] try: # Update Statics: # print(tdf3.mobile) for mobileAddr, mobile in tdf3.mobile.items(): lxObj, mobile.name = lxP.add_target() for staticName, static in tdf3.static.items(): xStatic.append(static.xPos) yStatic.append(static.yPos) lxP.add_anchor(static.name, (static.xPos, static.yPos)) lxObj.add_measure(static.name, (static.kDist + 0.00000000001)) # Update Mobile print("Heading:", tdf3.heading / 100) print("Steps:", tdf3.steps) try: lxP.solve() except ZeroDivisionError: pass xMobile.append(lxObj.loc.x) yMobile.append(lxObj.loc.y) except AttributeError: pass
def print_all(): while True: os.system("clear") # print(networks) pd.concat([networks, networks2, networks3], axis=1, sort=False) result = pd.concat([networks, networks2, networks3], axis=1, sort=False) print(result) val1 = result['meter'].values[0] val2 = result['meter2'].values[0] val3 = result['meter3'].values[0] P=lx.Project(mode='2D',solver='LSE') P.add_anchor('anchore_A',(1,1)) P.add_anchor('anchore_B',(10,2)) P.add_anchor('anchore_C',(1,2)) t,label=P.add_target() t.add_measure('anchore_A',val1) t.add_measure('anchore_B',val2) t.add_measure('anchore_C',val3) P.solve() print(t.loc) data = {'X': t.loc.x, 'Y' : t.loc.y} koor = json.dumps(data) with open('/var/www/html/beta/posisi.json', 'w', encoding='utf-8') as f: json.dump(data, f, ensure_ascii=False, indent=4) print (koor) time.sleep(0.3)
def print_all(): while True: os.system("clear") df_row = pandas.concat([networks, networks2, networks3], axis=1) print(df_row) # print(networks) # print(networks2) # print (df_row) val1 = df_row['meter'].values[0] val2 = df_row['meter2'].values[0] val3 = df_row['meter3'].values[0] P = lx.Project(mode='2D', solver='LSE') P.add_anchor('anchore_A', (1, 1)) P.add_anchor('anchore_B', (5, 9)) P.add_anchor('anchore_C', (10, 2)) t, label = P.add_target() t.add_measure('anchore_A', val1) t.add_measure('anchore_B', val2) t.add_measure('anchore_C', val3) P.solve() print(t.loc) # data = {'X': t.loc.x, 'Y' : t.loc.y} # koor = json.dumps(data) # with open('koor_user.json', 'w', encoding='utf-8') as f: # json.dump(data, f, ensure_ascii=False, indent=4) # print (koor) time.sleep(1)
def get_tag_location(anchors, ranges, transforms): P = lx.Project(mode="3D", solver="LSE") #define anchor locations for i in range(REQ_ANCHOR): P.add_anchor(anchors[i], transforms[i]) t, label = P.add_target() #define anchor ranges for i in range(REQ_ANCHOR): t.add_measure(anchors[i], ranges[i]) P.solve() B = t.loc return {'x': B.x, 'y': B.y, 'z': B.z}
def get_target_location(transforms, ranges): P = lx.Project(mode="3D", solver="LSE") #define anchor locations for anchor in transforms: P.add_anchor(anchor, transforms[anchor]) t, label = P.add_target() #define anchor ranges for anchor in ranges: t.add_measure(anchor, ranges[anchor]) P.solve() B = t.loc return {'x': B.x, 'y': B.y, 'z': B.z}
def calc_location(details, device_id): blockPrint() P=lx.Project(mode="2D",solver="LSE") for i in details: P.add_anchor(i[1], i[2]) t,label=P.add_target() for i in details: t.add_measure(i[1], i[0]) P.solve() enablePrint() device_queue[device_id]["location"] = t.loc print(t.loc.x, t.loc.y) # os.system("echo " + str(t.loc.x) + "," + str(t.loc.y) + " >> Log_files/stat_only_from_rssi.csv") os.system("echo " + str(t.loc.x) + "," + str(t.loc.y) + " >> Log_files/real_data_thameera_stat_only_from_rssi.csv")
def calc_location(details, device_id): blockPrint() # Setting up the mode and solver. P=lx.Project(mode="2D",solver="LSE") # Adding anchor tags of ESP32 devices for i in details: P.add_anchor(i[1], i[2]) t,label=P.add_target() for i in details: t.add_measure(i[1], i[0]) P.solve() enablePrint() # Adding calculated location in to the corresponding location of the device queue device_queue[device_id]["location"] = [t.loc.x, t.loc.y] # print (t.loc) return [t.loc.x, t.loc.y]
def three_anchor_multilat(): print("... Starting multilateration.") dist_dict = {} for anchor in rssi_dict: dist_dict[anchor] = dist_from_rssi(median(rssi_dict[anchor])) # https://github.com/kamalshadi/Localization import localization as lx P=lx.Project(mode='2D',solver='LSE') print("... adding anchors") for anchor in anchor_positions: P.add_anchor(anchor, anchor_positions[anchor]) t,label = P.add_target() print("... adding measurements") for dist in dist_dict: P.add_measure(dist, dist_dict[dist]) print("... calculating...") P.solve() print("> Done! Multilat result:", t.loc)
import localization as lx P = lx.Project(mode='Earth1', solver='CCA') # print str(P) P.add_anchor('1', (18.530143, 73.854764)) P.add_anchor('2', (18.530835, 73.856097)) P.add_anchor('3', (18.530637, 73.857547)) t, label = P.add_target() print str(t), '-----', str(label) t.add_measure('1', 216) t.add_measure('2', 160) t.add_measure('3', 173) P.solve() # print str(B) B = t.loc print str(B) # print 'location: ', str(ecef2llh((B.x, B.y, B.z)))
async def echo(websocket, path): async for message in websocket: #print(f"[From client]: {message}") protocol = message.split("-")[0] if protocol == "$id": query = message.split("-")[1] data = None print("[CLIENT][" + tempo() + "]: " + query) if (executeQuery(query, data)): await websocket.send("$id-" + str(indexDevice[0][0])) else: await websocket.send("$400:Bad Request") # inserimento dati nel database ######################################################### if protocol == "$insert": query = message.split("-")[1] lat = message.split("-")[2] lng = message.split("-")[3] nome = message.split("-")[4] indoor = message.split("-")[5] data = (nome, lat, lng, indoor) print("[CLIENT][" + tempo() + "]: INSERT INTO DB nome:" + nome + " latitudine: " + lat + " longitudine: " + lng + "indoor: " + indoor) if (executeQuery(query, data)): await websocket.send("$insert:200:OK") else: await websocket.send("$400:Bad Request") ######################################################### # eliminazione dati dal database ######################################################### if protocol == "$delete": query = message.split("-")[1] data = None print("[CLIENT][" + tempo() + "]: " + query) if (executeQuery(query, data)): await websocket.send("$200:OK") else: await websocket.send("$400:Bad Request") #####################################################localStorage.setItem('myCat', 'Tom');#### # inizio scansione e inserimento dei dati nel database ######################################################### if protocol == "$scan": global dst global coords global listaDati global ID ID = message.split("-")[1] timer = int(message.split("-")[2]) indoor = message.split("-")[3] data = None msg = "[CLIENT][{}]: Scan del dispositivo: {} per {} secondi" print(msg.format(tempo(), str(ID), timer)) t = AsyncSniffer(iface=interface, prn=PacketHandler) t.start() time.sleep(timer) t.stop() print("[SERVER][" + tempo() + "]: scansione terminata") await websocket.send("$end") query = "select macadress FROM(SELECT * FROM ProbeRequest JOIN Dispositivi on Dispositivi.ID == ProbeRequest.nodeID " + \ "WHERE Dispositivi.indoor = " + indoor + " " + \ "GROUP by macadress, nodeid ) GROUP by macadress HAVING COUNT(macadress) >= 3" data = None if (executeQuery(query, data)): for x in indexDevice: for y in x: query = "select macadress, nodeid, distance, vendor, latitudine, longitudine from ProbeRequest join Dispositivi on ProbeRequest.nodeID = Dispositivi.id where ProbeRequest.macadress = '" + \ y+"' and Dispositivi.indoor == " + indoor #query = "select macadress,nodeid, avg(distance), vendor, latitudine, longitudine from ProbeRequest " +\ # "join Dispositivi on ProbeRequest.nodeID = Dispositivi.id " + \ # "where ProbeRequest.macadress = '" + y +"' and Dispositivi.indoor == " + indoor + " " + \ # "GROUP BY nodeid" try: if (executeQuery(query, data)): vendor = None x = False anchor = [] measure = [] for z in indexDevice: print(z) x = True vendor = z[3] macAdr = z[0] #P.add_anchor(str(z[2]),(float(z[5]),float(z[4]))) anchor.append( [str(z[1]), float(z[5]), float(z[4])]) print(indoor) if indoor == str(1): #t.add_measure(str(z[2]),float(z[2])) #10 per la scala indoor measure.append( [str(z[1]), float(z[2])]) else: #t.add_measure(str(z[2]),float(z[2]*(10**-5))) measure.append([ str(z[1]), float(z[2]) * (10**-5) ]) #10**-5 per la mappa if (vendor == None): vendor = "Vendor non riconosciuto" if x: P = lx.Project(mode='2D', solver='LSE') for h in anchor: P.add_anchor(str( h[0]), (float(h[1]), float(h[2]))) t, label = P.add_target() for s in measure: t.add_measure(str(s[0]), float(s[1])) P.solve() p = t.loc print(p.x, p.y) await websocket.send("$pos&" + str(p.x) + "&" + str(p.y) + "&" + macAdr + "&" + vendor + "&" + indoor) else: await websocket.send( "$pos&nessun riscontro trovato") except IndexError as e: print( Fore.RED + "[SERVER][" + tempo() + "]: " + Fore.RESET, e) '''except: print( Fore.RED+"[SERVER][" + tempo() + "]: Errore sconosciuto" + Fore.RESET)''' else: print(Fore.RED + "[SERVER][" + tempo() + "]: Nessun Riscontro trovato nella trilaterazione" + Fore.RESET) ######################################################### # invio dei dati presenti nel database ######################################################### if protocol == "$db": query = message.split("-")[1] indoor = message.split("=")[1] data = None print("[CLIENT][" + tempo() + "]: " + query) if (executeQuery(query, data)): data = "" for x in indexDevice: for y in x: data = data + "-" + str(y) await websocket.send("$db" + data) data = "" else: await websocket.send("$400:BadRequest") query = "SELECT MAX(ts) FROM ProbeRequest JOIN Dispositivi ON ProbeRequest.nodeID = Dispositivi.id " + \ "where Dispositivi.indoor = " + indoor data = None if (executeQuery(query, data)): await websocket.send("$ts-" + str(indexDevice[0][0])) else: await websocket.send("$400:BadRequest")
2-3D 3-Earth1 Also three solvers can be utilized: 1-LSE for least square error 2-LSE_GC for least square error with geometric constraints. Geometric constraints force the solutions to be in the intersection areas of all multilateration circles. 3- CCA for centroid method, i.e., the solution will be the centroid of the intersection area. If no common intersection area exist, the area with maximum overlap is used. ''' MODE = '2D' SOLVER = 'LSE' P = lx.Project(mode=MODE, solver=SOLVER) ''' To add anchors to the project use: P.add_anchor(<name>,<loc>) where name denote user provided label of the anchor and <loc> is the location of the anchor provided in tuple, e.g., (120,60). ''' anchors = [('first', 0, 0), ('second', 9, 0), ('third', 0, 9), ('fourth', 9, 9)] for a in range(len(anchors)): P.add_anchor(anchors[a][0], (anchors[a][1], anchors[a][2])) '''
from parse import * from hiddenprints import * import localization as lx import numpy as np modes = ["2/3indep", "4/9indep", "bestline", "routers"] #Use client-independent #mode = modes[2] #Use client-dependent mode = modes[2] #localization object for performing multilateration #LSE_GC refers to "least square error with geometric constraints" #geometric constraints force the solutions to be in the intersection areas of all multilateration circles P = lx.Project(mode='Earth1', solver='LSE_GC') #reverse switches tuple order of a pair #useful because localization module uses (long, lat) format def reverse(pair): return (pair[1], pair[0]) #anchor data: num_anchors = 11 anchor_names = [ "prin", "cali", "cana", "ohio", "oreg", "virg", "gcali", "gcaro", "giowa", "goreg", "gvirg" ] anchor_locs = [ reverse(here_prin),
# -*- coding: utf-8 -*- import localization as lx ##################### Codigo de multilateração de dados obtidos durante o voo que substituirá o código atual############################# # Só é possivel usar métodos diferentes do LSE se os circulos das dist6ancias se conectarem. # Otimizar os valores toleráveis e só importar esses # Lib utilizada: https://pypi.python.org/pypi/Localization/0.1.4 P = lx.Project(mode="Earth1", solver="LSE") # Currently three modes are supported: # 1-2D # 2-3D # 3-Earth1 # # Also three solvers can be utilized: # 1-LSE for least square error # 2-LSE_GC for least square error with geometric constraints. Geometric constraints force the solutions to be in the intersection areas of all multilateration circles. # 3-CCA for centroid method, i.e., the solution will be the centroid of the intersection area. If no common intersection area exist, the area with maximum overlap is used. # Fabio # anchor = [(-22.86985120,-43.1051227),(-22.86983310,-43.1051285),(-22.86984080,-43.1051355),(-22.86983130,-43.1051311),(-22.86984440,-43.1051408),(-22.86987940,-43.1051325),(-22.86986800,-43.1051353),(-22.86986740,-43.1051099),(-22.86982530,-43.1051313),(-22.86986990,-43.1051268),(-22.86987800,-43.1051076),(-22.86984740,-43.1051061),(-22.86984480,-43.1051351),(-22.86984460,-43.1051185),(-22.86986010,-43.1051341),(-22.86987300,-43.1051315),(-22.86985720,-43.1051022),(-22.86986560,-43.1051251),(-22.86986950,-43.1051279),(-22.86987230,-43.1051344),(-22.86984940,-43.1051007),(-22.86984500,-43.1051201),(-22.86992110,-43.1050613),(-22.86984650,-43.1050980),(-22.86997450,-43.1050409),(-22.86984980,-43.1050972),(-22.86989680,-43.1050272),(-22.86984260,-43.1050673),(-22.86982940,-43.1050112),(-22.86996080,-43.1050254)] # dist_wf = (0.000251189,0.000271227,0.000292864,0.000316228,0.000316228,0.000398107,0.000398107,0.000398107,0.000398107,0.000429866,0.000429866,0.000464159,0.000464159,0.000501187,0.000681292,0.000735642,0.000926119,0.001000000,0.001079775,0.001165914,0.001165914,0.001847850,0.001847850,0.002712273,0.002712273,0.002928645,0.002928645,0.003162278,0.003981072,0.005843414) # Voo de 17 Nov 2017 anchor = [(-20.7463786, -41.2290764), (-20.7465702, -41.2290774), (-20.7465712, -41.2293447), (-20.7463795, -41.2293439)] dist_wf = (0.0130352714943, 0.0163173436884, 0.00627227441873, 0.0041974967925) t, label = P.add_target()
import localization as lx import requests import minimalFlask filename = 'walking_long_floor_serial.txt' x = []#calculated positions y = [] z = [] T0 = {'ID':0,'x':0,'y':0,'enabled':True} T1 = {'ID':1,'x':0,'y':0,'enabled':False} T2 = {'ID':2,'x':0,'y':0,'enabled':False} T3 = {'ID':3,'x':0,'y':0,'enabled':False} P=lx.Project(mode='3D',solver='LSE_GC') #Setting up location solver #P.add_anchor('a00',(12.355,54.791,2.87)) #for now, these are hardcoded because the anchors are #P.add_anchor('a01',(18.146,65.261,2.87)) #fixed on the floor. A plaintext configuration file #P.add_anchor('a02',(12.355,78.680,2.87)) #would work well to store an arbitrary # of anchors #P.add_anchor('a03',(18.146,83.548,2.87)) #temporary for testing, matches what DW ranginer app sees. P.add_anchor('a00',(0.+12.355, 0.+54.79, 2.87)) #for now, these are hardcoded because the anchors are P.add_anchor('a01',(5.791+12.355,10.47+54.79, 2.87)) #fixed on the floor. A plaintext configuration file P.add_anchor('a02',(0.+12.355, 23.889+54.79,2.87)) #would work well to store an arbitrary # of anchors P.add_anchor('a03',(5.791+12.355,28.757+54.79,2.87)) target,label=P.add_target() #initialize the target to solve the position of
print line2, print line3, print line4 try: line1.split(' ') and line2.split(' ') and line3.split(' ') and line4.split(' ') id1, r1 = line1.split(' ') id2, r2 = line2.split(' ') id3, r3 = line3.split(' ') id4, r4 = line4.split(' ') if((len(id1) == 8) and (len(id2) == 8) and (len(id3) == 8) and (len(id4) == 8)): try: float(r1) and float(r2) and float(r3) #and float(r4) P=lx.Project(mode='3D', solver='LSE_GC') P.add_anchor('FFFFFAFF', (0, 3.62, 2.34)) P.add_anchor('FFFFFBFF', (0, 0, 0)) P.add_anchor('FFFFFCFF', (0, 0, 2.34)) P.add_anchor('FFFFFDFF', (4.86, 0, 2.34)) #P.add_anchor('FFFFFAFF', (1.46, 4.88, 2.34)) #P.add_anchor('FFFFFBFF', (7.10, 4.32 , 0.92)) #P.add_anchor('FFFFFCFF', (0, 0, 0)) #P.add_anchor('FFFFFDFF', (6.86, -1.22, 2.5)) # P.add_anchor('FFFFFAFF', (0, 0, 0)) # P.add_anchor('FFFFFBFF', (12, 0 , 0)) # P.add_anchor('FFFFFCFF', (10, 5, 3)) # P.add_anchor('FFFFFDFF', (8, 5, -2))
def __init__(self): """ Set mode for localization module. """ self.P=lz.Project(mode='2D',solver='LSE') self.target,self.label_target=self.P.add_target()
def locate(): #location of access points AP1 = ['AP1', (16.6, 16.6)] AP2 = ['AP2', (16.6, 33.3)] AP3 = ['AP3', (33.3, 16.6)] AP4 = ['AP4', (33.3, 33.3)] #do the file thing res = open('estLoc.txt', 'w') i = 1 f = open('simulation.txt', 'r') z = f.readlines() err = [] fe = [[0], [0], [0], [0]] for i in range(0, 3000): p = lx.Project('2D', 'LSE') #set access points as anchors to use for localization p.add_anchor(AP1[0], AP1[1]) p.add_anchor(AP2[0], AP2[1]) p.add_anchor(AP3[0], AP3[1]) p.add_anchor(AP4[0], AP4[1]) #define target to locate rogue, label = p.add_target() x = z[i].strip('\n').split(' ') #fe=[(float(x[2])+fe[0]*(i+1))/(i+1), (float(x[3])+fe[1]*(i+1))/(i+1), (float(x[4])+fe[2]*(i+1))/(i+1), (float(x[5])+fe[3]*(i+1))/(i+1)] #print(x) if i == 0: fe = [[float(x[2])], [float(x[3])], [float(x[4])], [float(x[5])]] rogue.add_measure(AP1[0], calcDistance(fe[0][0])) rogue.add_measure(AP2[0], calcDistance(fe[1][0])) rogue.add_measure(AP3[0], calcDistance(fe[2][0])) rogue.add_measure(AP4[0], calcDistance(fe[3][0])) elif i < 5: fe[0].append(float(x[2])) fe[1].append(float(x[3])) fe[2].append(float(x[4])) fe[3].append(float(x[5])) rogue.add_measure(AP1[0], calcDistance(statistics.mean(fe[0]))) rogue.add_measure(AP2[0], calcDistance(statistics.mean(fe[1]))) rogue.add_measure(AP3[0], calcDistance(statistics.mean(fe[2]))) rogue.add_measure(AP4[0], calcDistance(statistics.mean(fe[3]))) else: fe[0].append(float(x[2])) fe[1].append(float(x[3])) fe[2].append(float(x[4])) fe[3].append(float(x[5])) fe[0].pop(0) fe[1].pop(0) fe[2].pop(0) fe[3].pop(0) rogue.add_measure(AP1[0], calcDistance(statistics.mean(fe[0]))) rogue.add_measure(AP2[0], calcDistance(statistics.mean(fe[1]))) rogue.add_measure(AP3[0], calcDistance(statistics.mean(fe[2]))) rogue.add_measure(AP4[0], calcDistance(statistics.mean(fe[3]))) #perform a multilateration to locate AP sys.stdout = open(os.devnull, "w") p.solve() sys.stdout = sys.__stdout__ #just data types manipulation ltemp = tuple( map(lambda x: isinstance(x, float) and round(x, 1) or x, eval(str(rogue.loc).strip('p')))) #ltemp = tuple(str(rogue.loc).strip('p').strip()) loc = (ltemp[0], ltemp[1]) '''lt = (str(rogue.loc).strip('p').strip('(').strip(')').split(',')) ltemp = (round(float(lt[0]),2),round(float(lt[1]),2))''' res.write( str(loc[0]) + ' ' + str(loc[1]) + ' ' + str(x[0]) + ' ' + str(x[1]) + '\n') est = (float(loc[0]), float(loc[1])) real = (float(x[0]), float(x[1])) err.append(round(distance(real, est), 2)) #print(i,'-','real location is ', real, 'init. estimated location is', loc, 'error is', round(distance(real,est),2), len(fe[0])) i = i + 1 print('avarge error', round(statistics.mean(err), 2)) f.close() res.close()
def __init__(self): self.beacons = {} self.p = lx.Project(mode='3D')
if __name__ == "__main__": print(get_radii(-80.0, -80.0, -80.0)) mean_1, mean_2, mean_3 = get_radii(-80.0, -80.0, -80.0) #x,y = trilateration(2,0,mean_1,1.5,1,mean_2,0,0,mean_3) x, y = trilateration(0, 0, mean_1, 5, 0, mean_2, 5, 5, mean_3) print("distance between calculated and setup coordinates=", calculateDistance(1.5, 0, x, y)) print("calculated cordinates of tag (x,y)=", x, y) # -*- coding: utf-8 -*- import localization as lx P = lx.Project(mode='2D', solver='LSE') P.add_anchor('anchore_A', (0, 0)) #P.add_anchor('anchore_B',(100,100)) P.add_anchor('anchore_C', (0, 1000)) t, label = P.add_target(ID="M90") print(t, label) t.add_measure('anchore_A', 400) #t.add_measure('anchore_B',10) t.add_measure('anchore_C', 601) P.solve() # Then the target location is:
def update_all_devices(): ''' All devices and their corresponding M9Bs are updated. DB sample record: [{'account': '000413B50038', 'bt_mac': '000413B50038', 'rssi': '-53', 'uuid': 'FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF90FF', 'beacon_type': 'a', 'proximity': '3', 'beacon_gateway_IPEI': '0328D3C918', 'beacon_gateway_name': 'Schreibtisch', 'time_stamp': '2020-10-29 10:44:22.489801', 'server_time_stamp': '2020-10-29 09:44:22', 'timeout': 96945}] real location is defined in list m9bpositions, e.g. {'m9b_IPEI': '000413666660', 'x': 0.0, 'y':0.0}, ''' # kill all previous M9B animations global rssi_circle_list global CLEANUP_DEVICES rssi_circle_list.empty() if CLEANUP_DEVICES: player_list.empty() # floorplan alpha mask to check if device is on the floor global floorplan_alpha_mask if msgDb: result = msgDb.read_m9b_device_status_db() #print(result) btmacs_non_unique = [sub['bt_mac'] for sub in result] btmacs_set = set(btmacs_non_unique) btmacs = list(btmacs_set) #print('btmacs:', btmacs) for idx, btmac in enumerate(btmacs): # get m9b data for btmac selected_items = [{k: d[k] for k in d if k != "a"} for d in result if d.get("bt_mac") == btmac] #print('selected_items:' , selected_items) # search devices list for known devices. old_player = False for dev in player_list: if dev.name == btmac: print('already there:', dev.name) old_player = True found_device = dev # BTLE devices #print(colors[idx % len(colors)], idx % len(colors)) print(len(player_list)) if not old_player: btle_device = Device(colors[len(player_list) % len(colors)], 32, 32, name=btmac) player_list.add(btle_device) else: btle_device = found_device # create list of visible m9bs gateway_list = [] for elem in selected_items: if is_valid_proximity(elem['proximity']) and get_m9bposition( elem['beacon_gateway_IPEI']): gateway_list.append({ 'ipei': elem['beacon_gateway_IPEI'], 'proximity': elem['proximity'], 'rssi': elem['rssi'] }) for m9b in gateway_list: #print(m9b, colors[idx % len(colors)]) # get physical location match = get_m9bposition(m9b['ipei']) if match: (xcenter, ycenter, zcenter) = toViewport(float(match['x']), float(match['y']), float(match['z'])) (radius, _notused, _notused) = toViewport( calc_dist(float(m9b['rssi']), 2) * 100.0, 0.0) create_rssi_circle(xcenter, ycenter, zcenter, radius, colors[idx % len(colors)]) # how many M9Bs detected the current BTLE device num_m9bs_see_device = len(gateway_list) if num_m9bs_see_device == 0: continue # sort M9Bs, no matter how many we got m9bs_sorted = sort_m9b_rssi(gateway_list) print( f'sorted m9bs [{num_m9bs_see_device}] for {btmac}:{m9bs_sorted}' ) radius1 = radius2 = radius3 = 0 match_best = get_m9bposition(m9bs_sorted[0]['ipei']) if not match_best: continue if num_m9bs_see_device >= 3: # trilaterate with best, 2nd best, 3rd best # best match_2ndbest = get_m9bposition(m9bs_sorted[1]['ipei']) match_3rdbest = get_m9bposition(m9bs_sorted[2]['ipei']) # check to be sure, all positions are defined if match_best and match_2ndbest and match_3rdbest: # all units in cm (x1, y1, z1) = (float(match_best['x']), float(match_best['y']), float(match_best['z'])) (x1, y1, z1) = adjust_floor(x1, y1, z1) (x2, y2, z2) = (float(match_2ndbest['x']), float(match_2ndbest['y']), float(match_2ndbest['z'])) (x2, y2, z2) = adjust_floor(x2, y2, z2) (x3, y3, z3) = (float(match_3rdbest['x']), float(match_3rdbest['y']), float(match_3rdbest['z'])) (x3, y3, z3) = adjust_floor(x3, y3, z3) radius1 = calc_dist(float(m9bs_sorted[0]['rssi']), 2) * 100.0 radius2 = calc_dist(float(m9bs_sorted[1]['rssi']), 2) * 100.0 radius3 = calc_dist(float(m9bs_sorted[2]['rssi']), 2) * 100.0 #radius3 = calc_dist(float(m9bs_sorted[2]['rssi']), 2) * 100.0 P = lx.Project(mode='3D', solver='LSE') P.add_anchor('match_best', (x1, y1, z1)) P.add_anchor('match_2ndbest', (x2, y2, z2)) P.add_anchor('match_3rdbest', (x3, y3, z3)) t, _label = P.add_target(ID=btmac) t.add_measure('match_best', radius1) t.add_measure('match_2ndbest', radius2) t.add_measure('match_3rdbest', radius3) if radius1 <= 100.0: # 1m for extra in range(5): P.add_anchor(f'match_best{extra}', (x1, y1, z1)) t.add_measure(f'match_best{extra}', 1.0) P.solve() # split floors back if t.loc.z > 2.8 / 2.0: # we are in the 2nd floor t.loc.x += offset6OG[0] t.loc.y += offset6OG[1] print("ajusted_:", t.loc) # go screen coordinates (xcenter, ycenter, zcenter) = toViewport(t.loc.x, t.loc.y, t.loc.z) elif num_m9bs_see_device == 2: # all units in cm radius1 = calc_dist(float(m9bs_sorted[0]['rssi']), 2) * 100.0 radius2 = calc_dist(float(m9bs_sorted[1]['rssi']), 2) * 100.0 match_2ndbest = get_m9bposition(m9bs_sorted[1]['ipei']) (x1, y1, z1) = (float(match_best['x']), float(match_best['y']), float(match_best['z'])) (x1, y1, z1) = adjust_floor(x1, y1, z1) (x2, y2, z2) = (float(match_2ndbest['x']), float(match_2ndbest['y']), float(match_2ndbest['z'])) (x2, y2, z2) = adjust_floor(x2, y2, z2) P = lx.Project(mode='3D', solver='LSE') t, _label = P.add_target(ID=btmac) P.add_anchor('match_best', (x1, y1, z1)) # we are very very near, increase the influence by adding more points. if radius1 <= 100.0: # 1m for extra in range(5): P.add_anchor(f'match_best{extra}', (x1, y1, z1)) t.add_measure(f'match_best{extra}', 1.0) P.add_anchor('match_2ndbest', (x2, y2, z2)) t.add_measure('match_best', radius1) t.add_measure('match_2ndbest', radius2) P.solve() # split floors back print(t.loc) if t.loc.z > 2.8 / 2.0: # we are in the 2nd floor t.loc.x += offset6OG[0] t.loc.y += offset6OG[1] # go screen coordinates (xcenter, ycenter, zcenter) = toViewport(t.loc.x, t.loc.y, t.loc.z) else: # only one M9B, assume direction along X+ achses # screen coordinates # all units in m radius1 = calc_dist(float(m9bs_sorted[0]['rssi']), 2) * 100.0 (xcenter, ycenter, zcenter) = toViewport( float(match_best['x']) + radius1 * 0.0, float(match_best['y']), float(match_best['z'])) # screen coordinates print(f'pos:({xcenter}, {ycenter}, {zcenter})') print(f'radi:({radius1} cm, {radius2} cm, {radius3} cm)') if floorplan_alpha_mask.mask.get_at( (int(max(0, xcenter)), int(max(0, ycenter)))): btle_device.rect.center = (xcenter, ycenter)