From dc48e13f7a2ad625680865eba685b03014271b69 Mon Sep 17 00:00:00 2001 From: Gergely Imreh Date: Fri, 26 Sep 2008 02:11:09 +0100 Subject: [PATCH] ADD: GSM positioning server for TangoGPS! Added new file: gsmpos_server.py Implemented: simple server using sockets to fake standard NMEA output of gpsd. Gets GSM fix, and outputs it as GPS data. Running: set tangoGPS to localhost/2940, currently hardcoded port. Start up server, if connects to dbus then it will start its socket - tangoGPS will automatically connect to it. The NMEA sentences sent to tangoGPS are shown n the terminal. Stop program with Ctrl-c, should do graceful exit (also if there's an exception) If running just to check output then start server, connect to socket (e.g. nc localhost 2940) and send a single character to make it start the looped reply. Currently only one connection is allowed to the server. --- gsmpos/gsmpos_sever.py | 328 +++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 328 insertions(+) create mode 100755 gsmpos/gsmpos_sever.py diff --git a/gsmpos/gsmpos_sever.py b/gsmpos/gsmpos_sever.py new file mode 100755 index 0000000..b87902a --- /dev/null +++ b/gsmpos/gsmpos_sever.py @@ -0,0 +1,328 @@ +#!/usr/bin/env python +""" +GSM Positioning + +(C) 2008 Gergely Imreh +--- using code from: + cell_locator: Baruch Even + +GPLv3 or later +""" +""" + + Uses input file: cellinfo.dat + Contain : Locations of known cells + Format : comma-separated values + Fields : mmc,mnc,cell_id,lattitude,longitude + +""" + + +import sys, os, serial, time, dbus, csv +from time import strftime +from math import * +### GPS import functions from stripped down version of cell_locator.py +from cell_locator_bare import Terminal +from cell_locator_bare import GPS + + +__version__ = '0.1' + + +############### + +# Debugging output +debug = False + +# Number of digits after decimal point when printing location +posdigits = 6 + +################ + + +def getpos(cells,cellpresent,mcc,mnc): + """ Calculating position based on GSM cell data """ + """ + Signal converted to relative distances with assumptions: + - Equal tower power output + - Free space propagation: signal strength scales with d^(-1/2) + : These are of course not true - but there is a priori knowledge of proper signal strengh/distance scaling + """ + + lon = 0 + lat = 0 + sumweight = 0 + have = 0 + + for k in range(len(cellpresent)): + # Signal strength in dB -> linear scale + sig = 10**(int(cellpresent[k]['rxlev'])/10.0) + # Linear scale -> distance scale (1/2 means assuming free space propagation) + dist = sig**(1/2.) + if debug: print "DEBUG:",cellpresent[k]['cell_id'],":signal:",cellpresent[k]['rxlev'],"Sig-linear:",sig,"Distance scale:",dist + for i in range(len(cells)): + if (mcc == cells[i]['mcc']) and (mnc == cells[i]['mnc']) and (cellpresent[k]['cell_id'] == cells[i]['cell_id']): + sumweight += dist + lon += dist*cells[i]['lon'] + lat += dist*cells[i]['lat'] + if debug: print "DEBUG:",cellpresent[k]['cell_id'],":CellFound: ",cells[i]['lat'], cells[i]['lon'] + have += 1 + break + if debug: print "DEBUG:","Cells-found:",have,"Sum-weight:",sumweight + if (sumweight > 0): + lon = lon/sumweight + lat = lat/sumweight + else: + lon = None + lat = None + if debug: print "DEBUG:","getpos:return:",lat,lon,have,len(cellpresent) + return lat,lon,have,len(cellpresent) + + + +def calcdist(lon1,lat1,lon2,lat2): + """ Calculate (approximate) distance of two coordinates on Earth """ + """ + Returns distance, and North/South difference, Average East/West difference + --- if lat2 / lon2 are more North / East : positive, if more South / East : negative + """ + # Earth mean radius (m) + r = 6372795 + # From Wikipedia :) + # Distance on sphere + 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)) + # North/South difference + ns = r*acos( cos(lat1/180*pi)*cos(lat2/180*pi) + sin(lat1/180*pi)*sin(lat2/180*pi)) + if (lat1 > lat2): + ns = -1*ns + # Usually don't need to, only at big distances: East/West difference, at both lattitudes, take the average + 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)) + 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)) + avew = (ew1+ew2)/2.0 + if (lon1 > lon2): + avew = -1*avew + return dist,ns,avew + + +class GSMpos(): + """ Class to store GSM positioning information """ + def __init__( self): + self.lat = None + self.lon = None + self.numtower = 0 + self.tottower = 0 + +def nmealocform(pos,padd=2): + """ Convert location into NMEA format """ + if pos == '': + nmeapos = '' + else : + a = int(pos) + b = (pos-a)*60.0 + nmeapos = str(a).zfill(padd)+'%02.5f'%b + return nmeapos + +def nmeasentence(lon,lat,numfixtower=0,numtottower=0,hdop=99): + """ Prepare minimal information needed for tangoGPS location display """ + """ Shows: location, date, time, number of seen/known towers (at seen/fixed satellite fields """ + """ for info on sentences: http://gpsd.berlios.de/NMEA.txt and check gpsd output """ + + nmeatime = strftime("%H%M%S.00") + nmeadate = strftime("%d%m%y") + + if (lon == None) or (lat == None): + ## When no GSM position: send 'Navigation device warning' and 'no fix' sentences + sentence = "$GPRMC,"+nmeatime+",V,,,,,0.0,0.0,"+nmeadate+",,\n" + sentence += '$GPGGA,'+strftime("%H%M%S.00")+',,,,,0,,,0,M,0,M,,\n' + sentence += "$GPGSV,1,0,,,,,,,,,,,,,,,,,\n" + else: + if (lon >= 0): + ew = "E" + printlon = nmealocform(lon,3) + else: + ew = "W" + printlon = nmealocform(-1*lon,3) + if (lat >= 0): + ns = "N" + printlat = nmealocform(lat,2) + else: + ns = "S" + printlat = nmealocform(-1*lat,2) + sentence = '' + sentence += "$GPRMC,"+nmeatime+",A,"+printlat+","+ns+","+printlon+","+ew+",0.0,0.0,"+nmeadate+",,\n" + sentence += '$GPGGA,'+strftime("%H%M%S.00")+','+printlat+','+ns+','+printlon+','+ew+',1,'+str(numfixtower)+','+str(hdop)+',0,M,0,M,,\n' + sentence += "$GPGSV,1,1,"+str(numtottower)+",,,,,,,,,,,,,,,,\n" + + return sentence + + + +if __name__ == "__main__": + + ####### + # Load cell information + ####### + f = open('cellinfo.dat','r') + cells = [] + for line in f: + params = ('mcc','mnc','cell_id','lat','lon') + data = line.split(',') + + d = {} + d[params[0]] = int(data[0]) + d[params[1]] = int(data[1]) + d[params[2]] = int(data[2]) + d[params[3]] = float(data[3]) + d[params[4]] = float(data[4]) + cells.append(d) + ######## + + + from dbus.mainloop.glib import DBusGMainLoop + DBusGMainLoop(set_as_default=True) + + import gobject + loop = gobject.MainLoop() + + # Start GPS + gps = GPS() + gps.init() + + t = Terminal() + t.open() + + # Init GSM position storage + gsm = GSMpos() + + + def posloop(pos): + """ Positioning loop, based on cell_locator.py """ + neigh = t.get_neighbour_cell_info() + loc = t.get_location_and_paging_info() + cell = t.get_service_cell_info() + + mcc = loc['mcc'] + mnc = loc['mnc'] + cellpresent = [] + if cell['cell_id'] > 0: + d = {} + d['cell_id'] = cell['cell_id'] + d['rxlev'] = cell['rxlev'] + cellpresent.append(d) + + for k in range(len(neigh)): + if neigh[k]['cell_id'] != 0: + d = {} + d['cell_id'] = neigh[k]['cell_id'] + d['rxlev'] = neigh[k]['rxlev'] + cellpresent.append(d) + pos.lat,pos.lon,pos.numtower,pos.tottower = getpos(cells,cellpresent,mcc,mnc) + + + + def looping(): + """ Displaying results """ + posloop(gsm) + if (gps.lat != None) and (gps.lon != None): + print "GPS: Pos:",round(gps.lat,posdigits),",",round(gps.lon,posdigits),"HDOP:",gps.hdop + else: print "GPS: No fix" + if (gsm.lat != None) and (gsm.lon != None): + print "GSM: Pos:",round(gsm.lat,posdigits),",",round(gsm.lon,posdigits),"T:",gsm.numtower,"/",gsm.tottower + else: print "GSM: No fix" + if (gps.lat != None) and (gps.lon != None) and (gsm.lat != None) and (gsm.lon != None): + dist,ns,ew = calcdist(gps.lon,gps.lat,gsm.lon,gsm.lat) + print "GSM-GPS Diff: ",round(dist),"m" + print "N/S : ",round(ns),"m | E/W: ",round(ew),"m" + gps.clear() + print '' + nmea = nmeasentence(gsm.lon,gsm.lat,gsm.numtower,gsm.tottower) + print nmea + return nmea + sys.stdout.flush() + + + return True + + +################## +# Quick and dirty server section +################# + import socket + +########## + HOST = 'localhost' + PORT = 2940 +########## + + s = None + for res in socket.getaddrinfo(HOST, PORT, socket.AF_UNSPEC, socket.SOCK_STREAM, 0, socket.AI_PASSIVE): + af, socktype, proto, canonname, sa = res + try: + s = socket.socket(af, socktype, proto) + s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) # Important for using again after unexpected crash! + except socket.error, msg: + s = None + continue + try: + s.bind(sa) + s.listen(1) + except socket.error, msg: + s.close() + s = None + continue + break + + if s is None: + print 'could not open socket' + sys.exit(1) + while 1: + print "Awaiting connection..." + run = 1 + try: + conn, addr = s.accept() + except: + break + + print 'Connected by', addr + while 1: + + try : + # If manual connection, have to press a button to start, tangoGPS automatic + data = conn.recv(1024) + except: + conn.close() + run = 0 + + if data: + while 1: + try: + conn.send(looping()) + time.sleep(2.0) + except (KeyboardInterrupt, SystemExit): + conn.close() + print "KeyboardInterrupt - Clean Exit" + run = 0 + break + except : + conn.close() + print "Exception:", sys.exc_type, ":", sys.exc_value + run = 0 + break + else: + break + print "Level 2" + if run ==0 : break + print "Level 1" + if run ==0 : break + print "Level 0" + + + # Closing down + print "Closing GSM" + t.close() + print "Closing GPS - wait for it!!" + gps.uninit() + + + + -- 2.11.4.GIT