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