source: modules/mod_location.py @ 30bff6f1

Last change on this file since 30bff6f1 was 30bff6f1, checked in by xkolman2 <xkolman2@…>, 3 years ago

add unicode text encoding declaration
# -*- coding: utf-8 -*-

git-svn-id: https://nlp.fi.muni.cz/svn/gps_navigace/trunk@652 0858a4d0-ffff-46e5-938e-62b5ecb34222

  • Property mode set to 100644
File size: 9.9 KB
Line 
1# -*- coding: utf-8 -*-
2# supplies position info from the GPS daemon
3#---------------------------------------------------------------------------
4# Copyright 2007-2008, Oliver White
5#
6# This program is free software: you can redistribute it and/or modify
7# it under the terms of the GNU General Public License as published by
8# the Free Software Foundation, either version 3 of the License, or
9# (at your option) any later version.
10#
11# This program is distributed in the hope that it will be useful,
12# but WITHOUT ANY WARRANTY; without even the implied warranty of
13# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14# GNU General Public License for more details.
15#
16# You should have received a copy of the GNU General Public License
17# along with this program.  If not, see <http://www.gnu.org/licenses/>.
18#---------------------------------------------------------------------------
19from __future__ import with_statement # for python 2.5
20from base_module import ranaModule
21import threading
22from time import *
23
24def getModule(m,d,i):
25  return(gpsd2(m,d,i))
26
27class gpsd2(ranaModule):
28  """Supplies position info from GPSD"""
29  def __init__(self, m, d, i):
30    ranaModule.__init__(self, m, d, i)
31    self.tt = 0
32    self.connected = False
33    self.set('speed', None)
34    self.set('metersPerSecSpeed', None)
35    self.set('bearing', None)
36    self.set('elevation', None)
37    self.status = "Unknown"
38    self.locationUpdate = self.nop
39
40    # GPSD support
41    self.GPSDConsumer = None
42
43  def nop(self):
44    """navigation update function placeholder"""
45    pass
46
47  def firstTime(self):
48    # start screen update 1 per second screen update
49    # TODO: event based redrawing
50    cron = self.m.get('cron', None)
51    if cron:
52      cron.addTimeout(self.screenUpdateCB, 1000, self, "screen and GPSD update")
53
54    # start location if persistantly enabled
55    if self.get('GPSEnabled', True): # is GPS enabled ?
56      self.startLocation()
57
58  def screenUpdateCB(self):
59    """update the screen and also GPSD location if enabled
60    TODO: more efficient screen updates"""
61#    print "updating screen"
62    self.locationUpdate()
63
64    gui = self.modrana.gui
65    if gui and gui.getIDString() == "GTK":
66      """
67      the location update method which might run asynchronously in the
68      device module also sends redraw requests
69      -> no need to send a new one if there is already one pending
70      --> there would not be a change position anyway until next the fix
71      -> surplus redraw requests are actually harmful with map rotation enabled
72      NOTE: this currently applies only to the GTK GUI
73      """
74      sFromLastRequest = time() - gui.getLastFullRedrawRequest()
75      if sFromLastRequest > 0.85:
76        self.set('needRedraw', True)
77    else:
78      print("location: GUI module not available")
79
80  def startGPSD(self):
81    """start the GPSD based location update method"""
82    try:
83      self.GPSDConsumer = GPSDConsumer()
84      self._checkVerbose() # check if verbose debugging is enabled
85      self.GPSDConsumer.daemon = True
86      self.GPSDConsumer.start()
87      self.connected = True
88      self.locationUpdate = self.updateGPSD
89    except Exception, e:
90      print("location: connecting to GPSD failed", e)
91      self.status = "No GPSD running"
92
93  def stopGPSD(self):
94    """stop the GPSD based location update method"""
95    self.GPSDConsumer.shutdown()
96    self.locationUpdate = self.nop
97    self.status = "No GPSD running"
98
99  def handleMessage(self, message, type, args):
100    if message == "setPosLatLon" and type == "ml":
101      if args and len(args) == 2:
102        lat = float(args[0])
103        lon = float(args[1])
104        print "gps:setting current position to: %f,%f" % (lat,lon)
105        self.set('pos',(lat,lon))
106    elif message == "checkGPSEnabled":
107        state = self.get('GPSEnabled', True)
108        if state == True:
109          self.startLocation()
110        elif state == False:
111          self.stopLocation()
112    elif message == "gpsdCheckVerboseDebugEnabled":
113      self._checkVerbose()
114
115  def startLocation(self):
116    """start location - device based or gpsd"""
117    print "location: enabling location"
118    if self.dmod.handlesLocation():
119      self.dmod.startLocation()
120    else:
121      self.startGPSD()
122
123  def stopLocation(self):
124    """stop location - device based or gpsd"""
125    print "location: disabling location"
126    if self.dmod.handlesLocation():
127      self.dmod.stopLocation()
128    else:
129      self.stopGPSD()
130 
131  def socket_cmd(self, cmd):
132    try:
133      self.s.send("%s\r\n" % cmd)
134    except:
135      print "something is wrong with the gps daemon"
136    result = self.s.recv(8192)
137    #print "Reply: %s" % result
138    expect = 'GPSD,' + cmd.upper() + '='
139    if(result[0:len(expect)] != expect):
140      print "Fail: received %s after sending %s" % (result, cmd)
141      return(None)
142    remainder = result[len(expect):]
143    if(remainder[0:1] == '?'):
144      print "Fail: Unknown data in " + cmd
145      return(None)
146    return(remainder)
147   
148  def test_socket(self):
149    for i in ('i','p','p','p','p'):
150      print "%s = %s" % (i, self.socket_cmd(i))
151      sleep(1)
152     
153  def gpsStatus(self):
154    return(self.socket_cmd("M"))
155
156  def bearing(self):
157    """return bearing as reported by gpsd"""
158    return self.socket_cmd("t")
159   
160  def elevation(self):
161    """return elevation as reported by gpsd
162    (meters above mean sea level)"""
163    return self.socket_cmd("a")
164
165  def speed(self):
166    """return speed in knots/sec as reported by gpsd"""
167    return self.socket_cmd("v")
168
169  def GPSTime(self):
170    """return a string representing gps time
171    in this format: D=yyyy-mm-ddThh:nmm:ss.ssZ (fractional seccond are not guarantied)
172    (for tagging trackpoints with acurate timestamp ?)"""
173    timeFromGPS = self.socket_cmd("d")
174    return timeFromGPS
175
176  def satellites(self):
177    list = self.socket_cmd('y')
178    if(not list):
179      return
180    parts = list.split(':')
181    (spare1,spare2,count) = parts[0].split(' ')
182    count = int(count)
183    self.set("gps_num_sats", count)
184    for i in range(count):
185      (prn,el,az,db,used) = [int(a) for a in parts[i+1].split(' ')]
186      self.set("gps_sat_%d"%i, (db,used,prn))
187      #print "%d: %d, %d, %d, %d, %d" % (i,prn,el,az,db,used)
188
189  def quality(self):
190    result = self.socket_cmd('q')
191    if(result):
192      (count,dd,dx,dy) = result.split(' ')
193      count = int(count)
194      (dx,dy,dd) = [float(a) for a in (dx,dy,dd)]
195      print "%d sats, quality %f, %f, %f" % (count,dd,dx,dy)
196
197  def updateGPSD(self):   
198    fix = self.GPSDConsumer.getFix()
199    if fix:
200      (lat,lon,elevation,bearing,speed,timestamp) = fix
201
202      # position
203      self.set('pos', (lat,lon))
204      self.set('pos_source', 'GPSD')
205      self.status = "OK"
206      # bearing
207      self.set('bearing', float(bearing))
208      # speed
209      if speed != None:
210        # normal gpsd reports speed in knots per second
211        gpsdSpeed = self.get('gpsdSpeedUnit', 'knotsPerSecond')
212        if gpsdSpeed == 'knotsPerSecond':
213          # convert to meters per second
214          speed = float(speed) * 0.514444444444444 # knots/sec to m/sec
215        self.set('metersPerSecSpeed', speed)
216        self.set('speed', float(speed) * 3.6)
217      else:
218        self.set('metersPerSecSpeed', None)
219        self.set('speed', None)
220      # elevation
221      if elevation:
222        self.set('elevation', elevation)
223      else:
224        self.set('elevation', None)
225
226      """always set this key to current epoch once the location is updated
227      so that modules can watch it and react"""
228      self.set('locationUpdated', time())
229
230      self.set('needRedraw', True)
231
232    # make the screen refresh after the update
233    # even when centering is turned off
234    # TODO: make this more efficinet !
235    # * only redraw when the position actually changes
236    # * do we need to dedraw when we momentarily dont know the position ?
237    # * redraw only the needed part of the screen
238    # -> make scrolling more efficinet
239    #  * reuse the alredy drawn area ?
240    #  * dont overdraw the whole screen for a simple nudge ?
241    #  * draw the new area with a delay/after the drag ended ?
242
243  def shutdown(self):
244    try:
245      self.stopLocation()
246    except Exception, e:
247      print "location: stopping location failed", e
248
249  def _checkVerbose(self):
250    verbose = self.get('gpsdDebugVerbose', False)
251    if self.GPSDConsumer:
252      if verbose:
253        self.GPSDConsumer.setVerbose(True)
254        print "location: gpsd debugging output turned ON"
255      else:
256        self.GPSDConsumer.setVerbose(False)
257        print "location: gpsd debugging output turned OFF"
258    else:
259      print("location: gpsd not used, so there is no debug output to enable")
260
261class GPSDConsumer(threading.Thread):
262  """consume data as they come in from the GPSD and store last known fix"""
263  def __init__(self):
264    threading.Thread.__init__(self)
265    self.lock = threading.RLock()
266    self.stop = False
267    import gps_module as gps
268    self.session = gps.gps(host="localhost", port="2947")
269    self.session.stream(flags=gps.client.WATCH_JSON)
270    self.verbose = False
271    # vars
272    self.fix = None
273
274  def run(self):
275    import gps_module as gps
276    print("GPSDConsumer: starting")
277    while True:
278      if self.stop == True:
279        print "GPSDConsumer: breaking"
280        break
281      self.session.next() # this function blocks until a new fix is available
282      sf = self.session.fix
283      if sf.mode != gps.MODE_NO_FIX:
284        with self.lock:
285          self.fix = (sf.latitude,sf.longitude,sf.altitude,sf.track,sf.speed, time())
286          if self.verbose:
287            print self.fix
288      else:
289        if self.verbose:
290          print "NO FIX, will retry in 1 s"
291        sleep(1)
292    print("GPSDConsumer: stopped")
293
294#      if r["class"] == "TPV":
295#        with self.lock:
296#          try:
297#            self.fix = (r['lat'],r['lon'],r['alt'],r['track'],r['speed'], time())
298#          except Exception, e:
299#            print("GPSDConsumer: eror reading data", e)
300
301  def shutdown(self):
302    print("GPSDConsumer: stopping")
303    self.stop = True
304
305  def getFix(self):
306    with self.lock:
307      return self.fix
308
309  def setVerbose(self, value):
310    with self.lock:
311      self.verbose = value
Note: See TracBrowser for help on using the repository browser.