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 signal
  24 #import optparse
  25 import dbus
  26 #import gobject
  27 import zavai
  28
  29 class GPSMonitor():
  30     def __init__(self, gps):
  31         self.gps = gps
  32         self.gps_ubx = gps.gps_ubx
  33
  34         # This piece of machinery is taken from Zhone
  35         self.debug_busy = None
  36         self.debug_want = set( ["NAV-STATUS", "NAV-SVINFO"] )
  37         self.debug_have = set()
  38         self.debug_error = set()
  39
  40         self.callbacks = set()
  41
  42     def debug_update(self):
  43         if self.debug_busy is None:
  44             pending = self.debug_want - self.debug_have - self.debug_error
  45             if pending:
  46                 self.debug_busy = pending.pop()
  47                 self.gps_ubx.SetDebugFilter(
  48                     self.debug_busy,
  49                     True,
  50                     reply_handler=self.on_debug_reply,
  51                     error_handler=self.on_debug_error,
  52                 )
  53
  54     def debug_request(self):
  55         self.debug_have = set()
  56         self.debug_update()
  57
  58     def on_debug_reply(self):
  59         self.debug_have.add(self.debug_busy)
  60         self.debug_busy = None
  61         self.debug_update()
  62
  63     def on_debug_error(self, e):
  64         zavai.info(e, "error while requesting debug packet %s" % self.debug_busy)
  65         self.debug_error.add(self.debug_busy)
  66         self.debug_busy = None
  67         self.debug_update()
  68
  69     def on_satellites_changed(self, satellites):
  70         self.debug_request()
  71
  72     def on_ubxdebug_packet(self, clid, length, data):
  73         for c in self.callbacks:
  74             c(clid, length, data)
  75
  76     def _start_listening(self):
  77         self.gps.request(self)
  78         # TODO: find out how come sometimes these events are not sent
  79         self.gps.bus.add_signal_receiver(
  80             self.on_satellites_changed, 'SatellitesChanged', 'org.freedesktop.Gypsy.Satellite',
  81             'org.freesmartphone.ogpsd', '/org/freedesktop/Gypsy')
  82         self.gps.bus.add_signal_receiver(
  83             self.on_ubxdebug_packet, 'DebugPacket', 'org.freesmartphone.GPS.UBX',
  84             'org.freesmartphone.ogpsd', '/org/freedesktop/Gypsy')
  85         self.debug_request()
  86
  87     def _stop_listening(self):
  88         self.gps.bus.remove_signal_receiver(
  89             self.on_satellites_changed, 'SatellitesChanged', 'org.freedesktop.Gypsy.Satellite',
  90             'org.freesmartphone.ogpsd', '/org/freedesktop/Gypsy')
  91         self.gps.bus.remove_signal_receiver(
  92             self.on_ubxdebug_packet, 'DebugPacket', 'org.freesmartphone.GPS.UBX',
  93             'org.freesmartphone.ogpsd', '/org/freedesktop/Gypsy')
  94         self.gps.release(self)
  95
  96     def connect(self, callback):
  97         "Send UBX debug packets to the given callback"
  98         do_start = not self.callbacks
  99         self.callbacks.add(callback)
 100         if do_start:
 101             self._start_listening()
 102
 103     def disconnect(self, callback):
 104         "Stop sending UBX debug packets to the given callback"
 105         if not self.callbacks: return
 106         self.callbacks.discard(callback)
 107         if not self.callbacks:
 108             self._stop_listening()
 109
 110
 111 # For a list of dbus services, look in /etc/dbus-1/system.d/
 112 class GPS(zavai.Resource):
 113     def __init__(self, registry, name):
 114         self.bus = registry.resource("dbus.system_bus")
 115
 116         # see mdbus -s org.freesmartphone.ousaged /org/freesmartphone/Usage
 117         self.usage = self.bus.get_object('org.freesmartphone.ousaged', '/org/freesmartphone/Usage')
 118         self.usage = dbus.Interface(self.usage, "org.freesmartphone.Usage")
 119
 120         # see mdbus -s org.freesmartphone.ogpsd /org/freedesktop/Gypsy
 121         gps = self.bus.get_object('org.freesmartphone.ogpsd', '/org/freedesktop/Gypsy')
 122         self.gps = dbus.Interface(gps, "org.freedesktop.Gypsy.Device")
 123         self.gps_time = dbus.Interface(gps, "org.freedesktop.Gypsy.Time")
 124         self.gps_position = dbus.Interface(gps, 'org.freedesktop.Gypsy.Position')
 125         self.gps_ubx = dbus.Interface(gps, 'org.freesmartphone.GPS.UBX')
 126
 127         self.monitor = GPSMonitor(self)
 128
 129         self.requestors = set()
 130
 131     def request(self, tag):
 132         "Request usage of GPS by the subsystem with the given tag"
 133         do_start = not self.requestors
 134         self.requestors.add(tag)
 135         if do_start:
 136             # Request GPS resource
 137             self.usage.RequestResource('GPS')
 138             zavai.info("Acquired GPS")
 139
 140     def release(self, tag):
 141         "Release usage of GPS by the subsystem with the given tag"
 142         if not self.requestors: return
 143         self.requestors.discard(tag)
 144         if not self.requestors:
 145             self.usage.ReleaseResource('GPS')
 146             zavai.info("Released GPS")
 147
 148     def shutdown(self):
 149         if self.requestors:
 150             self.requestors.clear()
 151             self.usage.ReleaseResource('GPS')
 152             zavai.info("Released GPS")
 153
 154 #    def wait_for_fix(self, callback):
 155 #        status = self.gps.GetFixStatus()
 156 #        if status in [2, 3]:
 157 #            zavai.info("We already have a fix, good.")
 158 #            callback()
 159 #            return True
 160 #        else:
 161 #            zavai.info("Waiting for a fix...")
 162 #            self.waiting_for_fix = callback
 163 #            self.bus.add_signal_receiver(
 164 #                self.on_fix_status_changed, 'FixStatusChanged', 'org.freedesktop.Gypsy.Device',
 165 #                'org.freesmartphone.ogpsd', '/org/freedesktop/Gypsy')
 166 #            return False
 167 #
 168 #    def on_fix_status_changed(self, status):
 169 #        if status not in [2, 3]: return
 170 #
 171 #        zavai.info("Got GPS fix")
 172 #        self.bus.remove_signal_receiver(
 173 #            self.on_fix_status_changed, 'FixStatusChanged', 'org.freedesktop.Gypsy.Device',
 174 #            'org.freesmartphone.ogpsd', '/org/freedesktop/Gypsy')
 175 #
 176 #        if self.waiting_for_fix:
 177 #            self.waiting_for_fix()
 178 #            self.waiting_for_fix = None
 179 #
 180 #    def track_position(self, callback):
 181 #        self.bus.add_signal_receiver(
 182 #            callback, 'PositionChanged', 'org.freedesktop.Gypsy.Position',
 183 #            'org.freesmartphone.ogpsd', '/org/freedesktop/Gypsy')
 184
 185
 186 class GPX(zavai.Resource):
 187     "Write GPX track and waypoint files"
 188     def __init__(self, resource, name):
 189         self.resource = resource
 190         self.trk = None
 191         self.wpt = None
 192
 193     def start(basename = None, tstamp = None):
 194         if basename is None:
 195             # compute it from GPS
 196             pass
 197
 198         self.trk = open(basename + "-trk.gpx", "wt")
 199         print >>self.trk, """<?xml version="1.0" encoding="UTF-8"?>
 200 <gpx
 201     version="1.0"
 202     creator="audiomap %s"
 203     xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
 204     xmlns="http://www.topografix.com/GPX/1/0"
 205     xsi:schemaLocation="http://www.topografix.com/GPX/1/0 http://www.topografix.com/GPX/1/0/gpx.xsd">
 206   <trk>
 207     <trkseg>""" % VERSION
 208
 209         self.wpt = open(basename + "-wpt.gpx", "wt")
 210         print >>self.wpt, """<?xml version="1.0" encoding="UTF-8"?>
 211 <gpx
 212     version="1.0"
 213     creator="audiomap %s"
 214     xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
 215     xmlns="http://www.topografix.com/GPX/1/0"
 216     xsi:schemaLocation="http://www.topografix.com/GPX/1/0 http://www.topografix.com/GPX/1/0/gpx.xsd">""" % VERSION
 217
 218         self.wpt_seq = 1;
 219
 220     def stop(self):
 221         if self.trk is not None:
 222             print >>self.trk, "</trkseg></trk></gpx>"
 223             self.trk.close()
 224             self.trk = None
 225         if self.wpt is not None:
 226             print >>self.wpt, "</gpx>"
 227             self.wpt.close()
 228             self.wpt = None
 229
 230     def shutdown(self):
 231         self.stop()
 232
 233     def waypoint(self, tstamp, lat, lon, ele = None, name = None):
 234         "Mark a waypoint"
 235         if name is None:
 236             name = "wpt_%d" % self.wpt_seq
 237             self.wpt_seq += 1
 238
 239         print >>self.wpt, """<wpt lat="%f" lon="%f">
 240     <name>%s</name>
 241     <time>%s</time>""" % (
 242             lat, lon, name, time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime(tstamp)))
 243         if ele is not None:
 244             print >>self.wpt, "    <ele>%f</ele>""" % ele
 245         print >>self.wpt, "</wpt>"
 246
 247     def trackpoint(self, tstamp, lat, lon, ele = None, course = None, speed = None, fix = None, hdop = None):
 248         print >>self.trk, """<trkpt lat="%f" lon="%f">
 249   <time>%s</time>""" % (lat, lon, time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime(tstamp)))
 250         if ele is not None: print >>self.trk, "    <ele>%f</ele>" % ele
 251         if course is not None: print >>self.trk, "    <course>%f</course>" % course
 252         if speed is not None: print >>self.trk, "    <speed>%f</speed>" % speed
 253         if fix is not None: print >>self.trk, "    <fix>%f</fix>" % fix
 254         if hdop is not None: print >>self.trk, "    <hdop>%f</hdop>" % hdop
 255         print >>self.trk, "</trkpt>"
 256
 257
 258
 259
 260 #    def record(self):
 261 #        self.audio = Audio(self.make_waypoint)
 262 #        self.gps = GPS()
 263 #        # Get a fix and start recording
 264 #        if not self.gps.wait_for_fix(self.start_recording):
 265 #            self.gps_monitor = GPSMonitor(self.gps)
 266 #            self.gps_monitor.start()
 267 #
 268 #    def monitor(self):
 269 #        self.audio = None
 270 #        self.gps = GPS()
 271 #        self.gps_monitor = GPSMonitor(self.gps)
 272 #        self.gps_monitor.start()
 273 #
 274 #    def start_recording(self):
 275 #        if self.gps_monitor:
 276 #            self.gps_monitor.stop()
 277 #            self.gps_monitor = None
 278 #
 279 #        if not self.audio:
 280 #            return
 281 #
 282 #        # Sync system time
 283 #        gpstime = self.gps.gps_time.GetTime()
 284 #        subprocess.call(["date", "-s", "@%d" % gpstime])
 285 #        subprocess.call(["hwclock", "--systohc"])
 286 #
 287 #        # Compute basename for output files
 288 #        self.basename = time.strftime("%Y-%m-%d-%H-%M-%S", time.localtime(gpstime))
 289 #        self.basename = os.path.join(AUDIODIR, self.basename)
 290 #
 291 #        # Start recording the GPX track
 292 #        self.gpx = GPX(self.basename)
 293 #        self.gps.track_position(self.on_position_changed)
 294 #
 295 #        # Start recording in background forking arecord
 296 #        self.audio.set_basename(self.basename)
 297 #        self.audio.start_recording()
 298 #
 299 #    def on_position_changed(self, fields, tstamp, lat, lon, alt):
 300 #        self.last_pos = (fields, tstamp, lat, lon, alt)
 301 #        if self.gpx:
 302 #            self.gpx.trackpoint(tstamp, lat, lon, alt)
 303 #
 304 #    def make_waypoint(self):
 305 #        if self.gpx is None:
 306 #            return
 307 #        if self.last_pos is None:
 308 #            self.last_pos = self.gps.gps_position.GetPosition()
 309 #        (fields, tstamp, lat, lon, alt) = self.last_pos
 310 #        self.gpx.waypoint(tstamp, lat, lon, alt)
 311 #        zavai.info("Making waypoint at %s: %f, %f, %f" % (
 312 #            time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(tstamp)), lat, lon, alt))