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(zavai.Service):
  27     def __init__(self, gps):
  28         super(GPSMonitor, self).__init__()
  29
  30         self.gps = gps
  31         self.gps_ubx = gps.gps_ubx
  32
  33         # This piece of machinery is taken from Zhone
  34         self.debug_busy = None
  35         self.debug_want = set( ["NAV-STATUS", "NAV-SVINFO"] )
  36         self.debug_have = set()
  37         self.debug_error = 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         self.notify(clid, length, data)
  73
  74     def start(self):
  75         self.gps.request(self)
  76         # TODO: find out how come sometimes these events are not sent
  77         self.gps.bus.add_signal_receiver(
  78             self.on_satellites_changed, 'SatellitesChanged', 'org.freedesktop.Gypsy.Satellite',
  79             'org.freesmartphone.ogpsd', '/org/freedesktop/Gypsy')
  80         self.gps.bus.add_signal_receiver(
  81             self.on_ubxdebug_packet, 'DebugPacket', 'org.freesmartphone.GPS.UBX',
  82             'org.freesmartphone.ogpsd', '/org/freedesktop/Gypsy')
  83         self.debug_request()
  84
  85     def stop(self):
  86         self.gps.bus.remove_signal_receiver(
  87             self.on_satellites_changed, 'SatellitesChanged', 'org.freedesktop.Gypsy.Satellite',
  88             'org.freesmartphone.ogpsd', '/org/freedesktop/Gypsy')
  89         self.gps.bus.remove_signal_receiver(
  90             self.on_ubxdebug_packet, 'DebugPacket', 'org.freesmartphone.GPS.UBX',
  91             'org.freesmartphone.ogpsd', '/org/freedesktop/Gypsy')
  92         self.gps.release(self)
  93
  94
  95 class GPSPosition(zavai.Service):
  96     def __init__(self, gps):
  97         self.gps = gps
  98
  99     def on_position_changed(self, fields, tstamp, lat, lon, alt):
 100         zavai.info("gps position: position changed")
 101         self.notify(fields, tstamp, lat, lon, alt)
 102
 103     def start(self):
 104         self.gps.request(self)
 105         self.gps.bus.add_signal_receiver(
 106             self.on_position_changed, 'PositionChanged', 'org.freedesktop.Gypsy.Position',
 107             'org.freesmartphone.ogpsd', '/org/freedesktop/Gypsy')
 108
 109     def stop(self):
 110         self.gps.bus.remove_signal_receiver(
 111             self.on_position_changed, 'PositionChanged', 'org.freedesktop.Gypsy.Position',
 112             'org.freesmartphone.ogpsd', '/org/freedesktop/Gypsy')
 113         self.gps.release(self)
 114
 115 # For a list of dbus services, look in /etc/dbus-1/system.d/
 116 class GPS(zavai.Service):
 117     def __init__(self, registry, name):
 118         super(GPS, self).__init__()
 119
 120         self.bus = registry.resource("dbus.system_bus")
 121
 122         # see mdbus -s org.freesmartphone.ousaged /org/freesmartphone/Usage
 123         self.usage = self.bus.get_object('org.freesmartphone.ousaged', '/org/freesmartphone/Usage')
 124         self.usage = dbus.Interface(self.usage, "org.freesmartphone.Usage")
 125
 126         # see mdbus -s org.freesmartphone.ogpsd /org/freedesktop/Gypsy
 127         gps = self.bus.get_object('org.freesmartphone.ogpsd', '/org/freedesktop/Gypsy')
 128         self.gps = dbus.Interface(gps, "org.freedesktop.Gypsy.Device")
 129         self.gps_time = dbus.Interface(gps, "org.freedesktop.Gypsy.Time")
 130         self.gps_position = dbus.Interface(gps, 'org.freedesktop.Gypsy.Position')
 131         self.gps_ubx = dbus.Interface(gps, 'org.freesmartphone.GPS.UBX')
 132
 133         self.monitor = GPSMonitor(self)
 134         self.position = GPSPosition(self)
 135
 136     def start(self):
 137         """Request GPS resource"""
 138         self.usage.RequestResource('GPS')
 139         zavai.info("Acquired GPS")
 140
 141     def stop(self):
 142         """Release usage of GPS"""
 143         self.usage.ReleaseResource('GPS')
 144         zavai.info("Released GPS")
 145
 146 #    def wait_for_fix(self, callback):
 147 #        status = self.gps.GetFixStatus()
 148 #        if status in [2, 3]:
 149 #            zavai.info("We already have a fix, good.")
 150 #            callback()
 151 #            return True
 152 #        else:
 153 #            zavai.info("Waiting for a fix...")
 154 #            self.waiting_for_fix = callback
 155 #            self.bus.add_signal_receiver(
 156 #                self.on_fix_status_changed, 'FixStatusChanged', 'org.freedesktop.Gypsy.Device',
 157 #                'org.freesmartphone.ogpsd', '/org/freedesktop/Gypsy')
 158 #            return False
 159 #
 160 #    def on_fix_status_changed(self, status):
 161 #        if status not in [2, 3]: return
 162 #
 163 #        zavai.info("Got GPS fix")
 164 #        self.bus.remove_signal_receiver(
 165 #            self.on_fix_status_changed, 'FixStatusChanged', 'org.freedesktop.Gypsy.Device',
 166 #            'org.freesmartphone.ogpsd', '/org/freedesktop/Gypsy')
 167 #
 168 #        if self.waiting_for_fix:
 169 #            self.waiting_for_fix()
 170 #            self.waiting_for_fix = None
 171 #
 172
 173 #    def start_recording(self):
 174 #        if self.gps_monitor:
 175 #            self.gps_monitor.stop()
 176 #            self.gps_monitor = None
 177 #
 178 #        if not self.audio:
 179 #            return
 180 #
 181 #        # Sync system time
 182 #        gpstime = self.gps.gps_time.GetTime()
 183 #        subprocess.call(["date", "-s", "@%d" % gpstime])
 184 #        subprocess.call(["hwclock", "--systohc"])
 185 #
 186 #        # Compute basename for output files
 187 #        self.basename = time.strftime("%Y-%m-%d-%H-%M-%S", time.localtime(gpstime))
 188 #        self.basename = os.path.join(AUDIODIR, self.basename)
 189 #
 190 #        # Start recording the GPX track
 191 #        self.gpx = GPX(self.basename)
 192 #        self.gps.track_position(self.on_position_changed)
 193 #
 194 #        # Start recording in background forking arecord
 195 #        self.audio.set_basename(self.basename)
 196 #        self.audio.start_recording()
 197 #
 198
 199 class GPX(zavai.Service):
 200     "Write GPX track and waypoint files"
 201     def __init__(self, registry, name):
 202         self.registry = registry
 203         self.trk = None
 204         self.wpt = None
 205         self.last_pos = None
 206         conf = registry.resource("conf")
 207         self.trackdir = conf.homedir
 208         self.activity_monitors = set()
 209         self.started = False
 210
 211     def add_activity_monitor(self, cb):
 212         self.activity_monitors.add(cb)
 213         cb(self, self.last_pos is not None)
 214
 215     def del_activity_monitor(self, cb):
 216         self.activity_monitors.discard(cb)
 217
 218     def notify_activity_monitors(self):
 219         for mon in self.activity_monitors:
 220             mon(self, self.last_pos is not None)
 221
 222     def start(self):
 223         zavai.info("Starting GPX trace subsystem")
 224         self.started = True
 225         gps = self.registry.resource("gps")
 226         gps.position.connect(self.on_position_changed)
 227
 228     def stop(self):
 229         if not self.started: return
 230         zavai.info("Stopping GPX trace subsystem")
 231         gps = self.registry.resource("gps")
 232         gps.position.disconnect(self.on_position_changed)
 233         self.stop_track()
 234         self.started = False
 235
 236     def on_position_changed(self, fields, tstamp, lat, lon, alt):
 237         self.last_pos = (fields, tstamp, lat, lon, alt)
 238         self.trackpoint()
 239
 240     def start_track(self, tstamp = None, basename = None):
 241         if basename is None:
 242             self.basename = basename
 243         elif tstamp is not None:
 244             # Compute basename for output files
 245             self.basename = time.strftime("%Y-%m-%d-%H-%M-%S", time.localtime(tstamp))
 246             self.basename = os.path.join(self.trackdir, self.basename)
 247
 248         self.trk = open(self.basename + "-trk.gpx", "wt")
 249         print >>self.trk, """<?xml version="1.0" encoding="UTF-8"?>
 250 <gpx
 251     version="1.0"
 252     creator="audiomap %s"
 253     xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
 254     xmlns="http://www.topografix.com/GPX/1/0"
 255     xsi:schemaLocation="http://www.topografix.com/GPX/1/0 http://www.topografix.com/GPX/1/0/gpx.xsd">
 256   <trk>
 257     <trkseg>""" % VERSION
 258
 259         self.wpt = open(self.basename + "-wpt.gpx", "wt")
 260         print >>self.wpt, """<?xml version="1.0" encoding="UTF-8"?>
 261 <gpx
 262     version="1.0"
 263     creator="audiomap %s"
 264     xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
 265     xmlns="http://www.topografix.com/GPX/1/0"
 266     xsi:schemaLocation="http://www.topografix.com/GPX/1/0 http://www.topografix.com/GPX/1/0/gpx.xsd">""" % VERSION
 267
 268         self.wpt_seq = 1;
 269         self.notify_activity_monitors()
 270
 271     def stop_track(self):
 272         if self.trk is not None:
 273             print >>self.trk, "</trkseg></trk></gpx>"
 274             self.trk.close()
 275             self.trk = None
 276         if self.wpt is not None:
 277             print >>self.wpt, "</gpx>"
 278             self.wpt.close()
 279             self.wpt = None
 280         self.last_pos = None
 281         self.notify_activity_monitors()
 282
 283     def trackpoint(self):
 284         "Mark a track point"
 285         if self.last_pos is None:
 286             return
 287
 288         fields, tstamp, lat, lon, ele = self.last_pos
 289
 290         if not self.trk:
 291             self.start_track(tstamp)
 292
 293         print >>self.trk, """<trkpt lat="%f" lon="%f">
 294   <time>%s</time>
 295   <ele>%f</ele>""" % (lat, lon, time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime(tstamp)), ele)
 296         #if course is not None: print >>self.trk, "    <course>%f</course>" % course
 297         #if speed is not None: print >>self.trk, "    <speed>%f</speed>" % speed
 298         #if fix is not None: print >>self.trk, "    <fix>%f</fix>" % fix
 299         #if hdop is not None: print >>self.trk, "    <hdop>%f</hdop>" % hdop
 300         print >>self.trk, "</trkpt>"
 301
 302     def waypoint(self, name = None):
 303         "Mark a waypoint"
 304         if self.last_pos is None:
 305             return
 306
 307         fields, tstamp, lat, lon, ele = self.last_pos
 308
 309         if not self.wpt:
 310             self.start_track(tstamp)
 311
 312         if name is None:
 313             name = "wpt_%d" % self.wpt_seq
 314             self.wpt_seq += 1
 315
 316         print >>self.wpt, """<wpt lat="%f" lon="%f">
 317     <name>%s</name>
 318     <time>%s</time>
 319     <ele>%f</ele>
 320 </wpt>""" % (
 321             lat, lon, name, time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime(tstamp)), ele)
 322
 323
 324 #    def record(self):
 325 #        self.audio = Audio(self.make_waypoint)
 326 #        self.gps = GPS()
 327 #        # Get a fix and start recording
 328 #        if not self.gps.wait_for_fix(self.start_recording):
 329 #            self.gps_monitor = GPSMonitor(self.gps)
 330 #            self.gps_monitor.start()
 331 #
 332 #    def monitor(self):
 333 #        self.audio = None
 334 #        self.gps = GPS()
 335 #        self.gps_monitor = GPSMonitor(self.gps)
 336 #        self.gps_monitor.start()
 337 #
 338 #
 339 #    def make_waypoint(self):
 340 #        if self.gpx is None:
 341 #            return
 342 #        if self.last_pos is None:
 343 #            self.last_pos = self.gps.gps_position.GetPosition()
 344 #        (fields, tstamp, lat, lon, alt) = self.last_pos
 345 #        self.gpx.waypoint(tstamp, lat, lon, alt)
 346 #        zavai.info("Making waypoint at %s: %f, %f, %f" % (
 347 #            time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(tstamp)), lat, lon, alt))