Started satellite monitor
[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 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
113 # For a list of dbus services, look in /etc/dbus-1/system.d/
114 class GPS(zavai.Resource):
115     def __init__(self, registry, name):
116         self.bus = registry.resource("dbus.system_bus")
117
118         # see mdbus -s org.freesmartphone.ousaged /org/freesmartphone/Usage
119         self.usage = self.bus.get_object('org.freesmartphone.ousaged', '/org/freesmartphone/Usage')
120         self.usage = dbus.Interface(self.usage, "org.freesmartphone.Usage")
121
122         # see mdbus -s org.freesmartphone.ogpsd /org/freedesktop/Gypsy
123         gps = self.bus.get_object('org.freesmartphone.ogpsd', '/org/freedesktop/Gypsy') 
124         self.gps = dbus.Interface(gps, "org.freedesktop.Gypsy.Device")
125         self.gps_time = dbus.Interface(gps, "org.freedesktop.Gypsy.Time")
126         self.gps_position = dbus.Interface(gps, 'org.freedesktop.Gypsy.Position')
127         self.gps_ubx = dbus.Interface(gps, 'org.freesmartphone.GPS.UBX')
128
129         self.monitor = GPSMonitor(self)
130
131         self.requestors = set()
132
133     def request(self, tag):
134         "Request usage of GPS by the subsystem with the given tag"
135         do_start = not self.requestors
136         self.requestors.add(tag)
137         if do_start:
138             # Request GPS resource
139             self.usage.RequestResource('GPS')
140             zavai.info("Acquired GPS")
141
142     def release(self, tag):
143         "Release usage of GPS by the subsystem with the given tag"
144         if not self.requestors: return
145         self.requestors.discard(tag)
146         if not self.requestors:
147             self.usage.ReleaseResource('GPS')
148             zavai.info("Released GPS")
149
150     def shutdown(self):
151         if self.requestors:
152             self.requestors.clear()
153             self.usage.ReleaseResource('GPS')
154             zavai.info("Released GPS")
155
156 #    def wait_for_fix(self, callback):
157 #        status = self.gps.GetFixStatus()
158 #        if status in [2, 3]:
159 #            zavai.info("We already have a fix, good.")
160 #            callback()
161 #            return True
162 #        else:
163 #            zavai.info("Waiting for a fix...")
164 #            self.waiting_for_fix = callback
165 #            self.bus.add_signal_receiver(
166 #                self.on_fix_status_changed, 'FixStatusChanged', 'org.freedesktop.Gypsy.Device',
167 #                'org.freesmartphone.ogpsd', '/org/freedesktop/Gypsy')
168 #            return False
169 #
170 #    def on_fix_status_changed(self, status):
171 #        if status not in [2, 3]: return
172 #
173 #        zavai.info("Got GPS fix")
174 #        self.bus.remove_signal_receiver(
175 #            self.on_fix_status_changed, 'FixStatusChanged', 'org.freedesktop.Gypsy.Device',
176 #            'org.freesmartphone.ogpsd', '/org/freedesktop/Gypsy')
177 #
178 #        if self.waiting_for_fix:
179 #            self.waiting_for_fix()
180 #            self.waiting_for_fix = None
181 #
182 #    def track_position(self, callback):
183 #        self.bus.add_signal_receiver(
184 #            callback, 'PositionChanged', 'org.freedesktop.Gypsy.Position',
185 #            'org.freesmartphone.ogpsd', '/org/freedesktop/Gypsy')
186
187
188 class GPX(zavai.Resource):
189     "Write GPX track and waypoint files"
190     def __init__(self, resource, name):
191         self.resource = resource
192         self.trk = None
193         self.wpt = None
194
195     def start(basename = None, tstamp = None):
196         if basename is None:
197             # compute it from GPS
198             pass
199
200         self.trk = open(basename + "-trk.gpx", "wt")
201         print >>self.trk, """<?xml version="1.0" encoding="UTF-8"?>
202 <gpx
203     version="1.0"
204     creator="audiomap %s"
205     xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
206     xmlns="http://www.topografix.com/GPX/1/0"
207     xsi:schemaLocation="http://www.topografix.com/GPX/1/0 http://www.topografix.com/GPX/1/0/gpx.xsd">
208   <trk>
209     <trkseg>""" % VERSION
210
211         self.wpt = open(basename + "-wpt.gpx", "wt")
212         print >>self.wpt, """<?xml version="1.0" encoding="UTF-8"?>
213 <gpx
214     version="1.0"
215     creator="audiomap %s"
216     xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
217     xmlns="http://www.topografix.com/GPX/1/0"
218     xsi:schemaLocation="http://www.topografix.com/GPX/1/0 http://www.topografix.com/GPX/1/0/gpx.xsd">""" % VERSION
219
220         self.wpt_seq = 1;
221
222     def stop(self):
223         if self.trk is not None:
224             print >>self.trk, "</trkseg></trk></gpx>"
225             self.trk.close()
226             self.trk = None
227         if self.wpt is not None:
228             print >>self.wpt, "</gpx>"
229             self.wpt.close()
230             self.wpt = None
231
232     def shutdown(self):
233         self.stop()
234
235     def waypoint(self, tstamp, lat, lon, ele = None, name = None):
236         "Mark a waypoint"
237         if name is None:
238             name = "wpt_%d" % self.wpt_seq
239             self.wpt_seq += 1
240
241         print >>self.wpt, """<wpt lat="%f" lon="%f">
242     <name>%s</name>
243     <time>%s</time>""" % (
244             lat, lon, name, time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime(tstamp)))
245         if ele is not None:
246             print >>self.wpt, "    <ele>%f</ele>""" % ele
247         print >>self.wpt, "</wpt>"
248
249     def trackpoint(self, tstamp, lat, lon, ele = None, course = None, speed = None, fix = None, hdop = None):
250         print >>self.trk, """<trkpt lat="%f" lon="%f">
251   <time>%s</time>""" % (lat, lon, time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime(tstamp)))
252         if ele is not None: print >>self.trk, "    <ele>%f</ele>" % ele
253         if course is not None: print >>self.trk, "    <course>%f</course>" % course
254         if speed is not None: print >>self.trk, "    <speed>%f</speed>" % speed
255         if fix is not None: print >>self.trk, "    <fix>%f</fix>" % fix
256         if hdop is not None: print >>self.trk, "    <hdop>%f</hdop>" % hdop
257         print >>self.trk, "</trkpt>"
258
259
260
261
262 #    def record(self):
263 #        self.audio = Audio(self.make_waypoint)
264 #        self.gps = GPS()
265 #        # Get a fix and start recording
266 #        if not self.gps.wait_for_fix(self.start_recording):
267 #            self.gps_monitor = GPSMonitor(self.gps)
268 #            self.gps_monitor.start()
269 #
270 #    def monitor(self):
271 #        self.audio = None
272 #        self.gps = GPS()
273 #        self.gps_monitor = GPSMonitor(self.gps)
274 #        self.gps_monitor.start()
275 #
276 #    def start_recording(self):
277 #        if self.gps_monitor:
278 #            self.gps_monitor.stop()
279 #            self.gps_monitor = None
280 #
281 #        if not self.audio:
282 #            return
283 #
284 #        # Sync system time
285 #        gpstime = self.gps.gps_time.GetTime()
286 #        subprocess.call(["date", "-s", "@%d" % gpstime])
287 #        subprocess.call(["hwclock", "--systohc"])
288 #
289 #        # Compute basename for output files
290 #        self.basename = time.strftime("%Y-%m-%d-%H-%M-%S", time.localtime(gpstime))
291 #        self.basename = os.path.join(AUDIODIR, self.basename)
292 #
293 #        # Start recording the GPX track
294 #        self.gpx = GPX(self.basename)
295 #        self.gps.track_position(self.on_position_changed)
296 #
297 #        # Start recording in background forking arecord
298 #        self.audio.set_basename(self.basename)
299 #        self.audio.start_recording()
300 #
301 #    def on_position_changed(self, fields, tstamp, lat, lon, alt):
302 #        self.last_pos = (fields, tstamp, lat, lon, alt)
303 #        if self.gpx:
304 #            self.gpx.trackpoint(tstamp, lat, lon, alt)
305 #
306 #    def make_waypoint(self):
307 #        if self.gpx is None:
308 #            return
309 #        if self.last_pos is None:
310 #            self.last_pos = self.gps.gps_position.GetPosition()
311 #        (fields, tstamp, lat, lon, alt) = self.last_pos
312 #        self.gpx.waypoint(tstamp, lat, lon, alt)
313 #        zavai.info("Making waypoint at %s: %f, %f, %f" % (
314 #            time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(tstamp)), lat, lon, alt))