gsmpos/gsmpos.py

   1 #!/usr/bin/env python
   2 """
   3 GSM Positioning
   4
   5 (C) 2008 Gergely Imreh <imrehg@gmail.com>
   6 --- using code from:
   7     cell_locator: Baruch Even <baruch@ev-en.org>
   8
   9 GPLv3 or later
  10 """
  11 """
  12
  13   Uses input file: cellinfo.dat
  14   Contain        : Locations of known cells
  15   Format         : comma-separated values
  16   Fields         : mmc,mnc,cell_id,lattitude,longitude
  17
  18 """
  19
  20
  21 import sys, os, serial, time, dbus, csv
  22 from time import strftime
  23 from math import *
  24 from Numeric import *
  25 ### GPS import functions from stripped down version of cell_locator.py
  26 from cell_locator_bare import Terminal
  27 from cell_locator_bare import GPS
  28 import traceback
  29
  30 __version__ = '0.2'
  31
  32
  33 ###############
  34
  35 # Debugging output
  36 debug = False
  37
  38 # Number of digits after decimal point when printing location
  39 posdigits = 6
  40
  41 ################
  42
  43 def triangulate(pos,idist):
  44
  45     def middlep(pos1,idist1,pos2,idist2):
  46         """ Middle point of triangle side """
  47         return (pos1*idist1 + pos2*idist2)/(idist1+idist2)
  48
  49     def perpvec(pos1,pos2):
  50         """ Vector perpendicular to the line connecting pos1 & pos2 """
  51         dir = pos2-pos1
  52         dir = array((-1*dir[1],dir[0]))
  53         return dir
  54
  55     def inters(p1,p2,p3,p4):
  56         """ Calculating intersection point """
  57         try :
  58            ua = ((p4[0]-p3[0])*(p1[1]-p3[1])-(p4[1]-p3[1])*(p1[0]-p3[0]))/((p4[1]-p3[1])*(p2[0]-p1[0]) - (p4[0]-p3[0])*(p2[1]-p1[1]))
  59         except:
  60            ua = 0
  61         return p1+ua*(p2-p1)
  62
  63     def getconsensus(inters):
  64         """ Calculate where to place most likely location,
  65             currently mean of the three intersections """
  66         x = 0
  67         y = 0
  68         for k in range(0,3):
  69            x += inters[k][0]/3.
  70            y += inters[k][1]/3.
  71         return x,y
  72
  73     ## Get triangle side middle points
  74     mp00 =  middlep(pos[:,0],idist[0,0],pos[:,1],idist[0,1])
  75     mp10 =  middlep(pos[:,0],idist[0,0],pos[:,2],idist[0,2])
  76     mp20 =  middlep(pos[:,1],idist[0,1],pos[:,2],idist[0,2])
  77     ## Get another point on the line perpendicular to the sides
  78     mp01 = mp00+perpvec(pos[:,0],pos[:,1])
  79     mp11 = mp10+perpvec(pos[:,0],pos[:,2])
  80     mp21 = mp20+perpvec(pos[:,1],pos[:,2])
  81     ## Get intersections
  82     i1 = inters(mp00,mp01,mp10,mp11)
  83     i2 = inters(mp00,mp01,mp20,mp21)
  84     i3 = inters(mp10,mp11,mp20,mp21)
  85     i = array(((i1),(i2),(i3)))
  86     ## Calculate centre
  87     [x,y] =  getconsensus(i)
  88     return x,y
  89
  90
  91 def getpos(cells,cellpresent,mcc,mnc):
  92         """  Calculating position based on GSM cell data """
  93         """
  94          Signal converted to relative distances with assumptions:
  95          - Equal tower power output
  96          - Free space propagation: signal strength scales with d^(-1/2)
  97          : These are of course not true - but there is a priori knowledge of proper signal strengh/distance scaling
  98         """
  99
 100         lon = 0
 101         lat = 0
 102         have = 0
 103         haveconnected = False
 104         pos = zeros([2,len(cellpresent)], Float)
 105         idist = zeros([1,len(cellpresent)], Float)
 106         sumweight = 0
 107
 108         for k in range(len(cellpresent)):
 109             # Signal strength in dB -> linear scale
 110             sig = 10**(int(cellpresent[k]['rxlev'])/10.0)
 111             # Linear scale -> distance scale (1/2 means assuming free space propagation)
 112             dist = sig**(-1/2.)
 113             if debug: print "DEBUG:",cellpresent[k]['cell_id'],":signal:",cellpresent[k]['rxlev'],"Sig-linear:",sig,"Distance scale:",dist
 114             for i in range(len(cells)):
 115                    if (mcc == cells[i]['mcc']) and (mnc == cells[i]['mnc']) and (cellpresent[k]['cell_id'] == cells[i]['cell_id']):
 116                         if k == 0 : haveconnected = True
 117                         pos[:,have] = [float(cells[i]['lat']),float(cells[i]['lon'])]
 118                         idist[0,have] = 1./dist
 119                         have += 1
 120                         if debug: print "DEBUG:",cellpresent[k]['cell_id'],":CellFound: ",cells[i]['lat'], cells[i]['lon']
 121                         break
 122
 123         if have == 0:
 124             """ No guess if no tower """
 125             lat = None
 126             lon = None
 127         elif have == 1:
 128             """ Wild guess if one tower """
 129             lat = pos[0,0]
 130             lon = pos[1,0]
 131         elif have == 2:
 132             """ On a line if two towers """
 133             lat = (pos[0,0]/idist[0,0] + pos[0,1]/idist[0,1])/(1/idist[0,0]+1/idist[0,1])
 134             lon = (pos[1,0]/idist[0,0] + pos[1,1]/idist[0,1])/(1/idist[0,0]+1/idist[0,1])
 135         elif have == 3:
 136             """ Triangulate if three towers """
 137             [lat,lon] = triangulate(pos[:,0:3],idist[:,0:3])
 138         elif have > 3:
 139                """ If more than three : use cleverness! (hopefully) """
 140                lat = 0
 141                lon = 0
 142                if not haveconnected:
 143                    """ If the connected tower is not on the known list, move forward the tower
 144                        with the highest signal level to be first in the list """
 145                    signals = sort(idist)
 146                    for k in range(have):
 147                       if float(idist[0,k]) == float(signals[0][-1]):
 148                          maxsignal = k
 149                          break
 150                    templat = pos[0,0]
 151                    templon = pos[1,0]
 152                    tempidist = idist[0,0]
 153                    pos[0,0] = pos[0,k]
 154                    pos[1,0] = pos[1,k]
 155                    idist[0,0] = idist[0,k]
 156                    pos[0,k] = templat
 157                    pos[1,k] = templon
 158                    idist[0,k] = tempidist
 159
 160                """ Triangulate possible triads (but keep the connected tower (or the highest signal one)
 161                    for all of them), and select for those that are closest to the known tower position,
 162                    finally average position """
 163                subpos = zeros([2,3], Float)
 164                subidist = zeros([1,3], Float)
 165                subpos[:,0] = pos[:,0]
 166                subidist[0,0] = idist[0,0]
 167                numcalc = ((have-1)*(have-2)) / 2
 168                icalc = 0
 169                cpos = zeros([2,numcalc], Float)
 170                cdist = []
 171                for k in range(1,have):
 172                   for m in range(k+1,have):
 173                      subpos[:,1] = pos[:,k]
 174                      subpos[:,2] = pos[:,m]
 175                      subidist[0,1] = idist[0,k]
 176                      subidist[0,2] = idist[0,m]
 177                      [sublat,sublon] = triangulate(subpos,subidist)
 178                      cpos[0,icalc] = sublat
 179                      cpos[1,icalc] = sublon
 180                      [dist, hdist, vdist] =  calcdist(pos[1,0],pos[0,0],sublon,sublat)
 181                      cdist.append(dist)
 182                      icalc += 1
 183                numuse = math.ceil(numcalc/2)
 184                scdist = cdist[:]
 185                scdist.sort()
 186                uplimit = scdist[int(numuse)-1]
 187                for k in range(numcalc):
 188                    if  float(cdist[k]) <= (uplimit):
 189                        lat += float(cpos[0,k])
 190                        lon += float(cpos[1,k])
 191                lat = lat / numuse
 192                lon = lon / numuse
 193
 194         else:
 195             lat = None
 196             lon = None
 197         if debug: print "DEBUG:","Cells-found:",have
 198         if debug: print "DEBUG:","getpos:return:",lat,lon,have,len(cellpresent)
 199         return lat,lon,have,len(cellpresent)
 200
 201
 202
 203 def calcdist(lon1,lat1,lon2,lat2):
 204     """ Calculate (approximate) distance of two coordinates on Earth """
 205     """
 206       Returns distance, and North/South difference, Average East/West difference
 207       --- if lat2 / lon2 are more North / East : positive, if more South / East : negative
 208     """
 209     # Earth mean radius (m)
 210     r = 6372795
 211     # From Wikipedia :)
 212     # Distance on sphere
 213     dist = r*acos( cos(lat1/180*pi)*cos(lat2/180*pi)*cos(lon1/180*pi - lon2/180*pi) + sin(lat1/180*pi)*sin(lat2/180*pi))
 214     # North/South difference
 215     ns = r*acos( cos(lat1/180*pi)*cos(lat2/180*pi) + sin(lat1/180*pi)*sin(lat2/180*pi))
 216     if (lat1 > lat2):
 217         ns = -1*ns
 218     # Usually don't need to, only at big distances: East/West difference, at both lattitudes, take the average
 219     ew1 = r*acos( cos(lat1/180*pi)*cos(lat1/180*pi)*cos(lon1/180*pi - lon2/180*pi) + sin(lat1/180*pi)*sin(lat1/180*pi))
 220     ew2 = r*acos( cos(lat2/180*pi)*cos(lat2/180*pi)*cos(lon1/180*pi - lon2/180*pi) + sin(lat2/180*pi)*sin(lat2/180*pi))
 221     avew = (ew1+ew2)/2.0
 222     if (lon1 > lon2):
 223         avew = -1*avew
 224     return dist,ns,avew
 225
 226
 227 class GSMpos():
 228     """ Class to store GSM positioning information """
 229     def __init__( self):
 230         self.lat = None
 231         self.lon = None
 232         self.numtower = 0
 233         self.tottower = 0
 234
 235 def getgsmpos(gsmpos,gsmterm,cells):
 236         """ Positioning loop, based on cell_locator.py """
 237         neigh = gsmterm.get_neighbour_cell_info()
 238         loc = gsmterm.get_location_and_paging_info()
 239         cell = gsmterm.get_service_cell_info()
 240
 241         mcc = loc['mcc']
 242         mnc = loc['mnc']
 243         cellpresent = []
 244         if cell['cell_id'] > 0:
 245                 d = {}
 246                 d['cell_id'] = cell['cell_id']
 247                 d['rxlev'] = cell['rxlev']
 248                 cellpresent.append(d)
 249
 250         for k in range(len(neigh)):
 251                 if neigh[k]['cell_id'] != 0:
 252                         d = {}
 253                         d['cell_id'] = neigh[k]['cell_id']
 254                         d['rxlev'] = neigh[k]['rxlev']
 255                         cellpresent.append(d)
 256         gsmpos.lat,gsmpos.lon,gsmpos.numtower,gsmpos.tottower = getpos(cells,cellpresent,mcc,mnc)
 257
 258
 259 def looping(gsmterm,gsm,cells,gps=None):
 260       """ Displaying results """
 261       try:
 262         getgsmpos(gsm,gsmterm,cells)
 263         if (gsm.lat != None) and (gsm.lon != None):
 264                 print "GSM: Pos:",round(gsm.lat,posdigits),",",round(gsm.lon,posdigits),"T:",gsm.numtower,"/",gsm.tottower
 265         else:   print "GSM: No fix"
 266         if gps != None:
 267            if (gps.lat != None) and (gps.lon != None):
 268               print "GPS: Pos:",round(gps.lat,posdigits),",",round(gps.lon,posdigits),"HDOP:",gps.hdop
 269            else:   print "GPS: No fix"
 270            if (gps.lat != None) and (gps.lon != None) and (gsm.lat != None) and (gsm.lon != None):
 271               dist,ns,ew = calcdist(gps.lon,gps.lat,gsm.lon,gsm.lat)
 272               print "GSM-GPS Diff: ",round(dist),"m"
 273               print "N/S : ",round(ns),"m | E/W: ",round(ew),"m"
 274            gps.clear()
 275         print ''
 276         sys.stdout.flush()
 277         return True
 278       except (KeyboardInterrupt, SystemExit):
 279         print "Keyboard Quit - please press Ctrl-C once more"
 280         return False
 281       except:
 282         etype = sys.exc_info()[0]
 283         evalue = sys.exc_info()[1]
 284         etb = traceback.extract_tb(sys.exc_info()[2])
 285         print "Exception in main loop: ", str(etype), " : ", str(evalue) , ' : ', etb
 286         print "Looping stopped - press Ctrl-c to quit"
 287         self_exit = 1
 288         return False
 289
 290
 291
 292
 293 def loadcellinfo(celldatafile):
 294     """ Load cell information """
 295     cells = []
 296     try:
 297       f = open(celldatafile,'r')
 298       for line in f:
 299         params = ('mcc','mnc','cell_id','lat','lon')
 300         data = line.split(',')
 301
 302         d = {}
 303         d[params[0]] = int(data[0])
 304         d[params[1]] = int(data[1])
 305         d[params[2]] = int(data[2])
 306         d[params[3]] = float(data[3])
 307         d[params[4]] = float(data[4])
 308         cells.append(d)
 309     except:
 310       etype = sys.exc_info()[0]
 311       evalue = sys.exc_info()[1]
 312       etb = traceback.extract_tb(sys.exc_info()[2])
 313       print "Error while loading GSM tower database: ", str(etype), " : ", str(evalue) , ' : ', str(etb)
 314     finally:
 315       return cells
 316
 317
 318 if __name__ == "__main__":
 319
 320     cells = loadcellinfo('cellinfo.dat')
 321
 322     from dbus.mainloop.glib import DBusGMainLoop
 323     DBusGMainLoop(set_as_default=True)
 324
 325     import gobject
 326     loop = gobject.MainLoop()
 327     context = loop.get_context()
 328
 329     # Start GPS
 330     gps = GPS()
 331     gps.init()
 332
 333     t = Terminal()
 334     t.open()
 335
 336     # Init GSM position storage
 337     gsm = GSMpos()
 338
 339     # Do positioning ever 3s
 340     self_exit = 0
 341     gobject.timeout_add(3000, looping, t, gsm, cells, gps)
 342     try:
 343         loop.run()
 344     except KeyboardInterrupt:
 345         print "Keyboard Quit"
 346         pass
 347     except:
 348         pass
 349
 350     # Closing down
 351     print "Closing GSM"
 352     t.close()
 353     print "Closing GPS - wait for it!!"
 354     gps.uninit()