Notify trackdir creation
[gregoa/zavai.git] / 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             zavai.info("Creating directory", self.trackdir)
271             os.makedirs(self.trackdir)
272
273     def request(self, tag):
274         "Request the GPX trace to be taken"
275         do_start = not self.requestors
276         self.requestors.add(tag)
277         if do_start:
278             zavai.info("Starting GPX trace subsystem")
279             gps = self.registry.resource("gps")
280             gps.position.connect(self.on_position_changed)
281
282     def release(self, tag):
283         "Release a GPX trace request"
284         if not self.requestors: return
285         self.requestors.discard(tag)
286         if not self.requestors:
287             zavai.info("Stopping GPX trace subsystem")
288             gps = self.registry.resource("gps")
289             gps.position.disconnect(self.on_position_changed)
290             self.stop()
291
292     def on_position_changed(self, fields, tstamp, lat, lon, alt):
293         self.last_pos = (fields, tstamp, lat, lon, alt)
294         self.trackpoint()
295
296     def start(basename = None, tstamp = None):
297         if basename is None:
298             # Compute basename for output files
299             basename = time.strftime("%Y-%m-%d-%H-%M-%S", time.localtime(tstamp))
300             basename = os.path.join(self.trackdir, self.basename)
301
302         self.trk = open(basename + "-trk.gpx", "wt")
303         print >>self.trk, """<?xml version="1.0" encoding="UTF-8"?>
304 <gpx
305     version="1.0"
306     creator="audiomap %s"
307     xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
308     xmlns="http://www.topografix.com/GPX/1/0"
309     xsi:schemaLocation="http://www.topografix.com/GPX/1/0 http://www.topografix.com/GPX/1/0/gpx.xsd">
310   <trk>
311     <trkseg>""" % VERSION
312
313         self.wpt = open(basename + "-wpt.gpx", "wt")
314         print >>self.wpt, """<?xml version="1.0" encoding="UTF-8"?>
315 <gpx
316     version="1.0"
317     creator="audiomap %s"
318     xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
319     xmlns="http://www.topografix.com/GPX/1/0"
320     xsi:schemaLocation="http://www.topografix.com/GPX/1/0 http://www.topografix.com/GPX/1/0/gpx.xsd">""" % VERSION
321
322         self.wpt_seq = 1;
323
324     def stop(self):
325         if self.trk is not None:
326             print >>self.trk, "</trkseg></trk></gpx>"
327             self.trk.close()
328             self.trk = None
329         if self.wpt is not None:
330             print >>self.wpt, "</gpx>"
331             self.wpt.close()
332             self.wpt = None
333         self.last_pos = None
334
335     def shutdown(self):
336         self.stop()
337
338     def trackpoint(self):
339         "Mark a track point"
340         if self.last_pos is None:
341             return
342
343         fields, tstamp, lat, lon, ele = self.last_pos
344
345         if not self.trk:
346             self.start(tstamp)
347
348         print >>self.trk, """<trkpt lat="%f" lon="%f">
349   <time>%s</time>
350   <ele>%f</ele>""" % (lat, lon, time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime(tstamp)), ele)
351         #if course is not None: print >>self.trk, "    <course>%f</course>" % course
352         #if speed is not None: print >>self.trk, "    <speed>%f</speed>" % speed
353         #if fix is not None: print >>self.trk, "    <fix>%f</fix>" % fix
354         #if hdop is not None: print >>self.trk, "    <hdop>%f</hdop>" % hdop
355         print >>self.trk, "</trkpt>"
356
357     def waypoint(self, name = None):
358         "Mark a waypoint"
359         if self.last_pos is None:
360             return
361
362         fields, tstamp, lat, lon, ele = self.last_pos
363
364         if not self.wpt:
365             self.start(tstamp)
366
367         if name is None:
368             name = "wpt_%d" % self.wpt_seq
369             self.wpt_seq += 1
370
371         print >>self.wpt, """<wpt lat="%f" lon="%f">
372     <name>%s</name>
373     <time>%s</time>
374     <ele>%f</ele>
375 </wpt>""" % (
376             lat, lon, name, time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime(tstamp)), ele)
377
378
379 #    def record(self):
380 #        self.audio = Audio(self.make_waypoint)
381 #        self.gps = GPS()
382 #        # Get a fix and start recording
383 #        if not self.gps.wait_for_fix(self.start_recording):
384 #            self.gps_monitor = GPSMonitor(self.gps)
385 #            self.gps_monitor.start()
386 #
387 #    def monitor(self):
388 #        self.audio = None
389 #        self.gps = GPS()
390 #        self.gps_monitor = GPSMonitor(self.gps)
391 #        self.gps_monitor.start()
392 #
393 #
394 #    def make_waypoint(self):
395 #        if self.gpx is None:
396 #            return
397 #        if self.last_pos is None:
398 #            self.last_pos = self.gps.gps_position.GetPosition()
399 #        (fields, tstamp, lat, lon, alt) = self.last_pos
400 #        self.gpx.waypoint(tstamp, lat, lon, alt)
401 #        zavai.info("Making waypoint at %s: %f, %f, %f" % (
402 #            time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(tstamp)), lat, lon, alt))