zavai/gps.py

   1 # gps - gps resource for zavai
   2 #
   3 # Copyright (C) 2009  Enrico Zini <enrico@enricozini.org>
   4 #
   5 # This program is free software; you can redistribute it and/or modify
   6 # it under the terms of the GNU General Public License as published by
   7 # the Free Software Foundation; either version 2 of the License, or
   8 # (at your option) any later version.
   9 #
  10 # This program is distributed in the hope that it will be useful,
  11 # but WITHOUT ANY WARRANTY; without even the implied warranty of
  12 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13 # GNU General Public License for more details.
  14 #
  15 # You should have received a copy of the GNU General Public License
  16 # along with this program; if not, write to the Free Software
  17 # Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  18
  19 #import sys
  20 import os
  21 import os.path
  22 import time
  23 import dbus
  24 import zavai
  25
  26 class GPSMonitor():
  27     def __init__(self, gps):
  28         self.gps = gps
  29         self.gps_ubx = gps.gps_ubx
  30
  31         # This piece of machinery is taken from Zhone
  32         self.debug_busy = None
  33         self.debug_want = set( ["NAV-STATUS", "NAV-SVINFO"] )
  34         self.debug_have = set()
  35         self.debug_error = set()
  36
  37         self.callbacks = set()
  38
  39     def debug_update(self):
  40         if self.debug_busy is None:
  41             pending = self.debug_want - self.debug_have - self.debug_error
  42             if pending:
  43                 self.debug_busy = pending.pop()
  44                 self.gps_ubx.SetDebugFilter(
  45                     self.debug_busy,
  46                     True,
  47                     reply_handler=self.on_debug_reply,
  48                     error_handler=self.on_debug_error,
  49                 )
  50
  51     def debug_request(self):
  52         self.debug_have = set()
  53         self.debug_update()
  54
  55     def on_debug_reply(self):
  56         self.debug_have.add(self.debug_busy)
  57         self.debug_busy = None
  58         self.debug_update()
  59
  60     def on_debug_error(self, e):
  61         zavai.info(e, "error while requesting debug packet %s" % self.debug_busy)
  62         self.debug_error.add(self.debug_busy)
  63         self.debug_busy = None
  64         self.debug_update()
  65
  66     def on_satellites_changed(self, satellites):
  67         zavai.info("gps monitor: satellites changed")
  68         self.debug_request()
  69
  70     def on_ubxdebug_packet(self, clid, length, data):
  71         zavai.info("gps monitor: UBX debug packet")
  72         for c in self.callbacks:
  73             c(clid, length, data)
  74
  75     def _start_listening(self):
  76         self.gps.request(self)
  77         # TODO: find out how come sometimes these events are not sent
  78         self.gps.bus.add_signal_receiver(
  79             self.on_satellites_changed, 'SatellitesChanged', 'org.freedesktop.Gypsy.Satellite',
  80             'org.freesmartphone.ogpsd', '/org/freedesktop/Gypsy')
  81         self.gps.bus.add_signal_receiver(
  82             self.on_ubxdebug_packet, 'DebugPacket', 'org.freesmartphone.GPS.UBX',
  83             'org.freesmartphone.ogpsd', '/org/freedesktop/Gypsy')
  84         self.debug_request()
  85
  86     def _stop_listening(self):
  87         self.gps.bus.remove_signal_receiver(
  88             self.on_satellites_changed, 'SatellitesChanged', 'org.freedesktop.Gypsy.Satellite',
  89             'org.freesmartphone.ogpsd', '/org/freedesktop/Gypsy')
  90         self.gps.bus.remove_signal_receiver(
  91             self.on_ubxdebug_packet, 'DebugPacket', 'org.freesmartphone.GPS.UBX',
  92             'org.freesmartphone.ogpsd', '/org/freedesktop/Gypsy')
  93         self.gps.release(self)
  94
  95     def connect(self, callback):
  96         "Send UBX debug packets to the given callback"
  97         do_start = not self.callbacks
  98         self.callbacks.add(callback)
  99         if do_start:
 100             self._start_listening()
 101
 102     def disconnect(self, callback):
 103         "Stop sending UBX debug packets to the given callback"
 104         if not self.callbacks: return
 105         self.callbacks.discard(callback)
 106         if not self.callbacks:
 107             self._stop_listening()
 108
 109 class GPSPosition():
 110     def __init__(self, gps):
 111         self.gps = gps
 112         self.callbacks = set()
 113
 114     def on_position_changed(self, fields, tstamp, lat, lon, alt):
 115         zavai.info("gps position: position changed")
 116         for c in self.callbacks:
 117             c(fields, tstamp, lat, lon, alt)
 118
 119     def _start_listening(self):
 120         self.gps.request(self)
 121
 122         self.bus.add_signal_receiver(
 123             self.on_position_changed, 'PositionChanged', 'org.freedesktop.Gypsy.Position',
 124             'org.freesmartphone.ogpsd', '/org/freedesktop/Gypsy')
 125
 126         # TODO: find out how come sometimes these events are not sent
 127         self.gps.bus.add_signal_receiver(
 128             self.on_satellites_changed, 'SatellitesChanged', 'org.freedesktop.Gypsy.Satellite',
 129             'org.freesmartphone.ogpsd', '/org/freedesktop/Gypsy')
 130         self.gps.bus.add_signal_receiver(
 131             self.on_ubxdebug_packet, 'DebugPacket', 'org.freesmartphone.GPS.UBX',
 132             'org.freesmartphone.ogpsd', '/org/freedesktop/Gypsy')
 133         self.debug_request()
 134
 135     def _stop_listening(self):
 136         self.gps.bus.remove_signal_receiver(
 137             self.on_satellites_changed, 'SatellitesChanged', 'org.freedesktop.Gypsy.Satellite',
 138             'org.freesmartphone.ogpsd', '/org/freedesktop/Gypsy')
 139         self.gps.bus.remove_signal_receiver(
 140             self.on_ubxdebug_packet, 'DebugPacket', 'org.freesmartphone.GPS.UBX',
 141             'org.freesmartphone.ogpsd', '/org/freedesktop/Gypsy')
 142         self.gps.release(self)
 143
 144     def connect(self, callback):
 145         "Send position changed messages to the given callback"
 146         do_start = not self.callbacks
 147         self.callbacks.add(callback)
 148         if do_start:
 149             self._start_listening()
 150
 151     def disconnect(self, callback):
 152         "Stop sending position changed messages to the given callback"
 153         if not self.callbacks: return
 154         self.callbacks.discard(callback)
 155         if not self.callbacks:
 156             self._stop_listening()
 157
 158
 159 # For a list of dbus services, look in /etc/dbus-1/system.d/
 160 class GPS(zavai.Resource):
 161     def __init__(self, registry, name):
 162         self.bus = registry.resource("dbus.system_bus")
 163
 164         # see mdbus -s org.freesmartphone.ousaged /org/freesmartphone/Usage
 165         self.usage = self.bus.get_object('org.freesmartphone.ousaged', '/org/freesmartphone/Usage')
 166         self.usage = dbus.Interface(self.usage, "org.freesmartphone.Usage")
 167
 168         # see mdbus -s org.freesmartphone.ogpsd /org/freedesktop/Gypsy
 169         gps = self.bus.get_object('org.freesmartphone.ogpsd', '/org/freedesktop/Gypsy')
 170         self.gps = dbus.Interface(gps, "org.freedesktop.Gypsy.Device")
 171         self.gps_time = dbus.Interface(gps, "org.freedesktop.Gypsy.Time")
 172         self.gps_position = dbus.Interface(gps, 'org.freedesktop.Gypsy.Position')
 173         self.gps_ubx = dbus.Interface(gps, 'org.freesmartphone.GPS.UBX')
 174
 175         self.monitor = GPSMonitor(self)
 176         self.position = GPSPosition(self)
 177
 178         self.requestors = set()
 179
 180     def request(self, tag):
 181         "Request usage of GPS by the subsystem with the given tag"
 182         do_start = not self.requestors
 183         self.requestors.add(tag)
 184         if do_start:
 185             # Request GPS resource
 186             self.usage.RequestResource('GPS')
 187             zavai.info("Acquired GPS")
 188
 189     def release(self, tag):
 190         "Release usage of GPS by the subsystem with the given tag"
 191         if not self.requestors: return
 192         self.requestors.discard(tag)
 193         if not self.requestors:
 194             self.usage.ReleaseResource('GPS')
 195             zavai.info("Released GPS")
 196
 197     def shutdown(self):
 198         if self.requestors:
 199             self.requestors.clear()
 200             self.usage.ReleaseResource('GPS')
 201             zavai.info("Released GPS")
 202
 203 #    def wait_for_fix(self, callback):
 204 #        status = self.gps.GetFixStatus()
 205 #        if status in [2, 3]:
 206 #            zavai.info("We already have a fix, good.")
 207 #            callback()
 208 #            return True
 209 #        else:
 210 #            zavai.info("Waiting for a fix...")
 211 #            self.waiting_for_fix = callback
 212 #            self.bus.add_signal_receiver(
 213 #                self.on_fix_status_changed, 'FixStatusChanged', 'org.freedesktop.Gypsy.Device',
 214 #                'org.freesmartphone.ogpsd', '/org/freedesktop/Gypsy')
 215 #            return False
 216 #
 217 #    def on_fix_status_changed(self, status):
 218 #        if status not in [2, 3]: return
 219 #
 220 #        zavai.info("Got GPS fix")
 221 #        self.bus.remove_signal_receiver(
 222 #            self.on_fix_status_changed, 'FixStatusChanged', 'org.freedesktop.Gypsy.Device',
 223 #            'org.freesmartphone.ogpsd', '/org/freedesktop/Gypsy')
 224 #
 225 #        if self.waiting_for_fix:
 226 #            self.waiting_for_fix()
 227 #            self.waiting_for_fix = None
 228 #
 229
 230 #    def start_recording(self):
 231 #        if self.gps_monitor:
 232 #            self.gps_monitor.stop()
 233 #            self.gps_monitor = None
 234 #
 235 #        if not self.audio:
 236 #            return
 237 #
 238 #        # Sync system time
 239 #        gpstime = self.gps.gps_time.GetTime()
 240 #        subprocess.call(["date", "-s", "@%d" % gpstime])
 241 #        subprocess.call(["hwclock", "--systohc"])
 242 #
 243 #        # Compute basename for output files
 244 #        self.basename = time.strftime("%Y-%m-%d-%H-%M-%S", time.localtime(gpstime))
 245 #        self.basename = os.path.join(AUDIODIR, self.basename)
 246 #
 247 #        # Start recording the GPX track
 248 #        self.gpx = GPX(self.basename)
 249 #        self.gps.track_position(self.on_position_changed)
 250 #
 251 #        # Start recording in background forking arecord
 252 #        self.audio.set_basename(self.basename)
 253 #        self.audio.start_recording()
 254 #
 255
 256 class GPX(zavai.Resource):
 257     "Write GPX track and waypoint files"
 258     def __init__(self, registry, name):
 259         self.registry = registry
 260         self.trk = None
 261         self.wpt = None
 262         self.last_pos = None
 263         self.requestors = set()
 264         conf = registry.resource("conf")
 265         self.trackdir = os.path.expanduser("~/.zavai")
 266         if conf.has_section("gps"):
 267             if conf.has_option("gps", "trackdir"):
 268                 self.trackdir = conf.get("gps", "trackdir")
 269         if not os.path.isdir(self.trackdir):
 270             os.makedirs(self.trackdir)
 271
 272     def request(self, tag):
 273         "Request the GPX trace to be taken"
 274         do_start = not self.requestors
 275         self.requestors.add(tag)
 276         if do_start:
 277             zavai.info("Starting GPX trace subsystem")
 278             gps = self.registry.resource("gps")
 279             gps.position.connect(self.on_position_changed)
 280
 281     def release(self, tag):
 282         "Release a GPX trace request"
 283         if not self.requestors: return
 284         self.requestors.discard(tag)
 285         if not self.requestors:
 286             zavai.info("Stopping GPX trace subsystem")
 287             gps = self.registry.resource("gps")
 288             gps.position.disconnect(self.on_position_changed)
 289             self.stop()
 290
 291     def on_position_changed(self, fields, tstamp, lat, lon, alt):
 292         self.last_pos = (fields, tstamp, lat, lon, alt)
 293         self.trackpoint()
 294
 295     def start(basename = None, tstamp = None):
 296         if basename is None:
 297             # Compute basename for output files
 298             basename = time.strftime("%Y-%m-%d-%H-%M-%S", time.localtime(tstamp))
 299             basename = os.path.join(self.trackdir, self.basename)
 300
 301         self.trk = open(basename + "-trk.gpx", "wt")
 302         print >>self.trk, """<?xml version="1.0" encoding="UTF-8"?>
 303 <gpx
 304     version="1.0"
 305     creator="audiomap %s"
 306     xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
 307     xmlns="http://www.topografix.com/GPX/1/0"
 308     xsi:schemaLocation="http://www.topografix.com/GPX/1/0 http://www.topografix.com/GPX/1/0/gpx.xsd">
 309   <trk>
 310     <trkseg>""" % VERSION
 311
 312         self.wpt = open(basename + "-wpt.gpx", "wt")
 313         print >>self.wpt, """<?xml version="1.0" encoding="UTF-8"?>
 314 <gpx
 315     version="1.0"
 316     creator="audiomap %s"
 317     xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
 318     xmlns="http://www.topografix.com/GPX/1/0"
 319     xsi:schemaLocation="http://www.topografix.com/GPX/1/0 http://www.topografix.com/GPX/1/0/gpx.xsd">""" % VERSION
 320
 321         self.wpt_seq = 1;
 322
 323     def stop(self):
 324         if self.trk is not None:
 325             print >>self.trk, "</trkseg></trk></gpx>"
 326             self.trk.close()
 327             self.trk = None
 328         if self.wpt is not None:
 329             print >>self.wpt, "</gpx>"
 330             self.wpt.close()
 331             self.wpt = None
 332         self.last_pos = None
 333
 334     def shutdown(self):
 335         self.stop()
 336
 337     def trackpoint(self):
 338         "Mark a track point"
 339         if self.last_pos is None:
 340             return
 341
 342         fields, tstamp, lat, lon, ele = self.last_pos
 343
 344         if not self.trk:
 345             self.start(tstamp)
 346
 347         print >>self.trk, """<trkpt lat="%f" lon="%f">
 348   <time>%s</time>
 349   <ele>%f</ele>""" % (lat, lon, time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime(tstamp)), ele)
 350         #if course is not None: print >>self.trk, "    <course>%f</course>" % course
 351         #if speed is not None: print >>self.trk, "    <speed>%f</speed>" % speed
 352         #if fix is not None: print >>self.trk, "    <fix>%f</fix>" % fix
 353         #if hdop is not None: print >>self.trk, "    <hdop>%f</hdop>" % hdop
 354         print >>self.trk, "</trkpt>"
 355
 356     def waypoint(self, name = None):
 357         "Mark a waypoint"
 358         if self.last_pos is None:
 359             return
 360
 361         fields, tstamp, lat, lon, ele = self.last_pos
 362
 363         if not self.wpt:
 364             self.start(tstamp)
 365
 366         if name is None:
 367             name = "wpt_%d" % self.wpt_seq
 368             self.wpt_seq += 1
 369
 370         print >>self.wpt, """<wpt lat="%f" lon="%f">
 371     <name>%s</name>
 372     <time>%s</time>
 373     <ele>%f</ele>
 374 </wpt>""" % (
 375             lat, lon, name, time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime(tstamp)), ele)
 376
 377
 378 #    def record(self):
 379 #        self.audio = Audio(self.make_waypoint)
 380 #        self.gps = GPS()
 381 #        # Get a fix and start recording
 382 #        if not self.gps.wait_for_fix(self.start_recording):
 383 #            self.gps_monitor = GPSMonitor(self.gps)
 384 #            self.gps_monitor.start()
 385 #
 386 #    def monitor(self):
 387 #        self.audio = None
 388 #        self.gps = GPS()
 389 #        self.gps_monitor = GPSMonitor(self.gps)
 390 #        self.gps_monitor.start()
 391 #
 392 #
 393 #    def make_waypoint(self):
 394 #        if self.gpx is None:
 395 #            return
 396 #        if self.last_pos is None:
 397 #            self.last_pos = self.gps.gps_position.GetPosition()
 398 #        (fields, tstamp, lat, lon, alt) = self.last_pos
 399 #        self.gpx.waypoint(tstamp, lat, lon, alt)
 400 #        zavai.info("Making waypoint at %s: %f, %f, %f" % (
 401 #            time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(tstamp)), lat, lon, alt))