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