e85d40e8d2506c0f3ad3b2fb6dc605baa3822cb8
[gregoa/zavai.git] / vala / gps.vala
1 /*
2  * gps - gps resource for zavai
3  *
4  * Copyright (C) 2009  Enrico Zini <enrico@enricozini.org>
5  *
6  * This program is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  *
11  * This program is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with this program; if not, write to the Free Software
18  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
19  */
20
21 using GLib;
22
23 namespace zavai {
24 namespace gps {
25
26 // For a list of dbus services, look in /etc/dbus-1/system.d/
27 public class GPS: zavai.Service
28 {
29         dynamic DBus.Object usage;
30
31         public GPS()
32         {
33                 name = "gps";
34
35                 // see mdbus -s org.freesmartphone.ousaged /org/freesmartphone/Usage
36                 usage = zavai.registry.sbus.get_object(
37                         "org.freesmartphone.ousaged",
38                         "/org/freesmartphone/Usage",
39                         "org.freesmartphone.Usage");
40
41 //         # see mdbus -s org.freesmartphone.ogpsd /org/freedesktop/Gypsy
42 //         gps = self.bus.get_object('org.freesmartphone.ogpsd', '/org/freedesktop/Gypsy') 
43 //         self.gps = dbus.Interface(gps, "org.freedesktop.Gypsy.Device")
44 //         self.gps_time = dbus.Interface(gps, "org.freedesktop.Gypsy.Time")
45 //         self.gps_position = dbus.Interface(gps, 'org.freedesktop.Gypsy.Position')
46 //         self.gps_ubx = dbus.Interface(gps, 'org.freesmartphone.GPS.UBX')
47                 usage.ResourceChanged += on_resourcechanged;
48         }
49
50         public void on_resourcechanged(dynamic DBus.Object pos, string name, bool state, HashTable<string, Value?> attributes)
51         {
52                 zavai.log.info("RESOURCE CHANGED " + name);
53         }
54
55         /// Request GPS resource
56         public override void start()
57         {
58                 if (started) return;
59                 try {
60                         usage.RequestResource("GPS");
61                         zavai.log.info("Acquired GPS");
62                         base.start();
63                 } catch (GLib.Error e) {
64                         zavai.log.error(e.message);
65                 }
66                 base.start();
67         }
68
69         // Release usage of GPS
70         public override void stop()
71         {
72                 if (!started) return;
73                 try {
74                         usage.ReleaseResource("GPS");
75                         zavai.log.info("Released GPS");
76                         base.stop();
77                 } catch (GLib.Error e) {
78                         zavai.log.error(e.message);
79                 }
80                 base.stop();
81         }
82 }
83
84 public class Monitor : zavai.Service
85 {
86         static const int NAV_STATUS=0;
87         static const int NAV_SVINFO=1;
88         static const int NAV_MAX=2;
89
90         dynamic DBus.Object ubx;
91         dynamic DBus.Object time;
92         /*
93         string[] filters = { "NAV-STATUS", "NAV_SVINFO" };
94
95         int debug_busy;
96         int debug_want;
97         int debug_have;
98         int debug_error;
99         */
100
101
102         public Monitor()
103         {
104                 name = "gps.monitor";
105
106                 ubx = zavai.registry.sbus.get_object(
107                         "org.freesmartphone.ogpsd",
108                         "/org/freedesktop/Gypsy",
109                         "org.freesmartphone.GPS.UBX");
110                 time = zavai.registry.sbus.get_object(
111                         "org.freesmartphone.ogpsd",
112                         "/org/freedesktop/Gypsy",
113                         "org.freedesktop.Gypsy.Time");
114
115                 zavai.log.info("SETSIG1");
116                 time.TimeChanged += timechanged;
117                 zavai.log.info("SETSIG2");
118
119                 zavai.log.info("DEBUG1");
120                 ubx.DebugPacket += on_ubxdebug_packet;
121                 zavai.log.info("DEBUG2");
122
123 /*
124                 // This piece of machinery is taken from Zhone
125                 debug_busy = -1;
126                 debug_want = (1 << NAV_STATUS) | (1 << NAV_SVINFO);
127                 debug_have = 0;
128                 debug_error = 0;
129 */
130         }
131
132         protected void timechanged(dynamic DBus.Object pos, int t)
133         {
134                 zavai.log.info("TIMECHANGED");
135         }
136
137 /*
138         protected void debug_update() throws GLib.Error
139         {
140                 zavai.log.debug("UPDATE");
141                 if (debug_busy != -1)
142                         return;
143                 zavai.log.debug("UPD1");
144                 int pending = debug_want & (~debug_have) & (~debug_error);
145                 if (pending == 0)
146                         return;
147                 zavai.log.debug("UPD2");
148                 for (int i = 0; i < NAV_MAX; ++i)
149                         if ((pending & (1<<i)) != 0)
150                         {
151                                 debug_busy = i;
152                                 break;
153                         }
154                 zavai.log.debug("UPD3 " + filters[debug_busy]);
155
156                 ubx.SetDebugFilter(
157                         filters[debug_busy],
158                         true,
159                         on_debug_reply
160 //                      on_debug_error
161                 );
162                 zavai.log.debug("UPD4");
163         }
164
165         protected void debug_request() throws GLib.Error
166         {
167                 debug_have = 0;
168                 debug_update();
169         }
170
171         protected void on_debug_reply() throws GLib.Error
172         {
173                 debug_have |= (1<<debug_busy);
174                 debug_busy = -1;
175                 debug_update();
176         }
177
178         protected void on_debug_error(string e) throws GLib.Error
179         {
180                 string name = debug_busy == -1 ? "none" : filters[debug_busy];
181                 zavai.log.error("error while requesting debug packet " + name + ": " + e);
182                 debug_error |= (1<<debug_busy);
183                 debug_busy = -1;
184                 debug_update();
185         }
186 */
187
188         /*
189         protected void on_satellites_changed(satellites)
190         {
191 //         zavai.info("gps monitor: satellites changed")
192 //         self.debug_request()
193         }
194         */
195
196         protected void on_ubxdebug_packet(dynamic DBus.Object ubx, string clid, int length,
197                         HashTable<string, Value?>[] data)
198         {
199                 zavai.log.info("gps monitor: UBX debug packet");
200                 message("ZAZA %s %d", clid, length);
201                 message("ZAZA %u %lu", data.length, sizeof(HashTable<string, Value?>));
202                 message("ZAZA %p %p", (void*)data.length, this);
203                 message("ZAZA %p", data[0]);
204                 message("ZAZA %p", data[1]);
205                 message("ZAZA %p", data[2]);
206                 /*
207                 message("ZAZA %u", data[0].size());
208                 foreach (string k in data[0].get_keys())
209                         message("ZAZA %s", k);
210                 */
211                 PtrArray< HashTable<string, Value?> >* prova = (PtrArray< HashTable<string, Value?> >)data;
212                 message("ZAZA %u", prova->len);
213                 /*
214                 foreach (string k in prova[0].get_keys())
215                         message("ZAZA %s", k);
216                         */
217                 /*
218                 for (int i = 0; data[i] != null; ++i)
219                 {
220                         zavai.log.info("ZAZA");
221                 }
222                 */
223                 //message("Size: %d", data.size());
224 //         self.notify("satellites", clid, length, data)
225         }
226
227         protected override void start()
228         {
229                 if (started) return;
230
231                 zavai.log.info("Starting GPS Monitor");
232                 gps.request("gps.monitor");
233 //         # TODO: find out how come sometimes these events are not sent
234 //         self.gps.bus.add_signal_receiver(
235 //             self.on_satellites_changed, 'SatellitesChanged', 'org.freedesktop.Gypsy.Satellite',
236 //             'org.freesmartphone.ogpsd', '/org/freedesktop/Gypsy')
237                 //ubx.DebugPacket += prova;
238                 //try {
239                         //ubx.SetDebugFilter("NAV-STATUS", true);
240                         //ubx.SetDebugFilter("NAV-SVINFO", true);
241                         //debug_request();
242                         base.start();
243                 //} catch (GLib.Error e) {
244                         //zavai.log.error(e.message);
245                 //}
246         }
247
248         protected override void stop()
249         {
250                 if (!started) return;
251                 zavai.log.info("Stopping GPS Monitor");
252 //         self.gps.bus.remove_signal_receiver(
253 //             self.on_satellites_changed, 'SatellitesChanged', 'org.freedesktop.Gypsy.Satellite',
254 //             'org.freesmartphone.ogpsd', '/org/freedesktop/Gypsy')
255                 try {
256                         ubx.SetDebugFilter("NAV-STATUS", false);
257                         ubx.SetDebugFilter("NAV-SVINFO", false);
258                 } catch (GLib.Error e) {
259                         zavai.log.error(e.message);
260                 }
261                 ubx.DebugPacket -= on_ubxdebug_packet;
262                 //ubx.DebugPacket -= prova;
263                 gps.release("gps.monitor");
264                 base.stop();
265         }
266 }
267
268
269 public class Position : zavai.Service
270 {
271         dynamic DBus.Object position;
272
273         public signal void position_changed(int fields, int tstamp, double lat, double lon, double alt);
274
275         public Position()
276         {
277                 name = "gps.position";
278                 position = zavai.registry.sbus.get_object(
279                         "org.freesmartphone.ogpsd",
280                         "/org/freedesktop/Gypsy",
281                         "org.freedesktop.Gypsy.Position");
282         }
283
284         public void on_position_changed(dynamic DBus.Object pos, int fields, int tstamp, double lat, double lon, double alt)
285         {
286                 zavai.log.info("gps position: position changed");
287                 position_changed(fields, tstamp, lat, lon, alt);
288         }
289
290         public override void start()
291         {
292                 if (started) return;
293                 zavai.log.info("Starting GPS position tracking");
294                 gps.request("gps.position");
295                 position.PositionChanged += on_position_changed;
296                 base.start();
297         }
298
299         public override void stop()
300         {
301                 if (!started) return;
302                 zavai.log.info("Stopping GPS position tracking");
303                 position.PositionChanged -= on_position_changed;
304                 gps.release("gps.position");
305                 base.stop();
306         }
307 }
308
309 // #    def wait_for_fix(self, callback):
310 // #        status = self.gps.GetFixStatus()
311 // #        if status in [2, 3]:
312 // #            zavai.info("We already have a fix, good.")
313 // #            callback()
314 // #            return True
315 // #        else:
316 // #            zavai.info("Waiting for a fix...")
317 // #            self.waiting_for_fix = callback
318 // #            self.bus.add_signal_receiver(
319 // #                self.on_fix_status_changed, 'FixStatusChanged', 'org.freedesktop.Gypsy.Device',
320 // #                'org.freesmartphone.ogpsd', '/org/freedesktop/Gypsy')
321 // #            return False
322 // #
323 // #    def on_fix_status_changed(self, status):
324 // #        if status not in [2, 3]: return
325 // #
326 // #        zavai.info("Got GPS fix")
327 // #        self.bus.remove_signal_receiver(
328 // #            self.on_fix_status_changed, 'FixStatusChanged', 'org.freedesktop.Gypsy.Device',
329 // #            'org.freesmartphone.ogpsd', '/org/freedesktop/Gypsy')
330 // #
331 // #        if self.waiting_for_fix:
332 // #            self.waiting_for_fix()
333 // #            self.waiting_for_fix = None
334 // #
335 // 
336 // #    def start_recording(self):
337 // #        if self.gps_monitor:
338 // #            self.gps_monitor.stop()
339 // #            self.gps_monitor = None
340 // #
341 // #        if not self.audio:
342 // #            return
343 // #
344 // #        # Sync system time
345 // #        gpstime = self.gps.gps_time.GetTime()
346 // #        subprocess.call(["date", "-s", "@%d" % gpstime])
347 // #        subprocess.call(["hwclock", "--systohc"])
348 // #
349 // #        # Compute basename for output files
350 // #        self.basename = time.strftime("%Y-%m-%d-%H-%M-%S", time.localtime(gpstime))
351 // #        self.basename = os.path.join(AUDIODIR, self.basename)
352 // #
353 // #        # Start recording the GPX track
354 // #        self.gpx = GPX(self.basename)
355 // #        self.gps.track_position(self.on_position_changed)
356 // #
357 // #        # Start recording in background forking arecord
358 // #        self.audio.set_basename(self.basename)
359 // #        self.audio.start_recording()
360 // #
361
362 // Write GPX track and waypoint files
363 public class GPX : Service
364 {
365         public bool tracking {
366                 get { return trk != null; }
367                 set {}
368         }
369         public signal void tracking_changed(bool tracking);
370
371         FileStream trk = null;
372         FileStream wpt = null;
373         int wpt_seq = 1;
374         bool last_valid = false;
375         int last_fields;
376         time_t last_tstamp;
377         double last_lat;
378         double last_lon;
379         double last_alt;
380
381         public GPX()
382         {
383                 name = "gps.gpx";
384         }
385
386         public override void start()
387         {       
388                 if (!started)
389                 {
390                         log.info("Starting GPX trace subsystem");
391                         position.request("gps.gpx");
392                         position.position_changed += on_position_changed;
393                         base.start();
394                 }
395         }
396
397         public override void stop()
398         {
399                 if (started)
400                 {
401                         log.info("Stopping GPX trace subsystem");
402                         position.release("gps.gpx");
403                         position.position_changed -= on_position_changed;
404                         stop_track();
405                         base.stop();
406                 }
407         }
408
409         public void on_position_changed(Position pos, int fields, int tstamp, double lat, double lon, double alt)
410         {
411                 last_fields = fields;
412                 last_tstamp = tstamp;
413                 last_lat = lat;
414                 last_lon = lon;
415                 last_alt = alt;
416                 last_valid = true;
417                 trackpoint();
418         }
419
420         public void start_track(time_t tstamp = 0, string? basename = null)
421         {
422                 string fname;
423                 if (basename != null)
424                         fname = basename;
425                 else
426                 {
427                         time_t now = tstamp == 0 ? time_t() : tstamp;
428
429                         // Compute basename for output files
430                         var t = Time.local(now);
431                         char[] res = new char[25];
432                         t.strftime(res, "%Y-%m-%d-%H-%M-%S");
433                         fname = zavai.config.homedir + "/" + (string)res;
434                 }
435  
436                 trk = FileStream.open(fname + "-trk.gpx", "wt");
437                 trk.puts("<?xml version=\"1.0\" encoding=\"UTF-8\"?>");
438                 trk.puts(" <gpx");
439                 trk.puts("     version=\"1.0\"");
440                 trk.printf("     creator=\"zavai %s\"\n", zavai.config.version);
441                 trk.puts("     xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\"");
442                 trk.puts("     xmlns=\"http://www.topografix.com/GPX/1/0\"");
443                 trk.puts("     xsi:schemaLocation=\"http://www.topografix.com/GPX/1/0 http://www.topografix.com/GPX/1/0/gpx.xsd\">");
444                 trk.puts("   <trk>");
445                 trk.puts("     <trkseg>");
446  
447                 wpt = FileStream.open(fname + "-wpt.gpx", "wt");
448                 wpt.puts("<?xml version=\"1.0\" encoding=\"UTF-8\"?>");
449                 wpt.puts(" <gpx");
450                 wpt.puts("     version=\"1.0\"");
451                 wpt.printf("     creator=\"zavai %s\"", zavai.config.version);
452                 wpt.puts("     xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\"");
453                 wpt.puts("     xmlns=\"http://www.topografix.com/GPX/1/0\"");
454                 wpt.puts("     xsi:schemaLocation=\"http://www.topografix.com/GPX/1/0 http://www.topografix.com/GPX/1/0/gpx.xsd\">");
455  
456                 wpt_seq = 1;
457                 tracking_changed(true);
458         }
459  
460         public void stop_track()
461         {
462                 if (trk != null)
463                 {
464                         trk.puts("</trkseg></trk></gpx>");
465                         trk.flush();
466                         trk = null;
467                 }
468                 if (wpt != null)
469                 {
470                         wpt.puts("</gpx>");
471                         wpt.flush();
472                         wpt = null;
473                 }
474                 last_valid = false;
475                 tracking_changed(false);
476         }
477
478         // Mark a track point
479         public void trackpoint()
480         {
481                 if (!last_valid) return;
482                 if (trk == null)
483                         start_track(last_tstamp);
484
485                 trk.printf("<trkpt lat=\"%f\" lon=\"%f\">\n", last_lat, last_lon);
486                 var t = Time.local(last_tstamp);
487                 char[] ts = new char[25];
488                 t.strftime(ts, "%Y-%m-%dT%H:%M:%SZ");
489                 trk.printf("  <time>%s</time>\n", (string)ts);
490                 trk.printf("  <ele>%f</ele>\n", last_alt);
491                 // if course is not None: print >>self.trk, "    <course>%f</course>" % course
492                 // if speed is not None: print >>self.trk, "    <speed>%f</speed>" % speed
493                 // if fix is not None: print >>self.trk, "    <fix>%f</fix>" % fix
494                 // if hdop is not None: print >>self.trk, "    <hdop>%f</hdop>" % hdop
495                 trk.puts("</trkpt>");
496         }
497
498         // Mark a waypoint
499         public void waypoint(string? name = null)
500         {
501                 if (!last_valid) return;
502                 if (wpt == null)
503                         start_track(last_tstamp);
504
505                 string wptname;
506                 if (name == null)
507                 {
508                         wptname = "wpt_%d".printf(wpt_seq);
509                         wpt_seq += 1;
510                 } else {
511                         wptname = name;
512                 }
513
514                 wpt.printf("<wpt lat=\"%f\" lon=\"%f\">\n", last_lat, last_lon);
515                 wpt.printf("  <name>%s</name>\n", wptname);
516                 var t = Time.local(last_tstamp);
517                 char[] ts = new char[25];
518                 t.strftime(ts, "%Y-%m-%dT%H:%M:%SZ");
519                 wpt.printf("  <time>%s</time>\n", (string)ts);
520                 wpt.printf("  <ele>%f</ele>\n", last_alt);
521                 wpt.puts("</wpt>");
522         }
523 }
524
525 // #    def record(self):
526 // #        self.audio = Audio(self.make_waypoint)
527 // #        self.gps = GPS()
528 // #        # Get a fix and start recording
529 // #        if not self.gps.wait_for_fix(self.start_recording):
530 // #            self.gps_monitor = GPSMonitor(self.gps)
531 // #            self.gps_monitor.start()
532 // #
533 // #    def monitor(self):
534 // #        self.audio = None
535 // #        self.gps = GPS()
536 // #        self.gps_monitor = GPSMonitor(self.gps)
537 // #        self.gps_monitor.start()
538 // #
539 // #
540 // #    def make_waypoint(self):
541 // #        if self.gpx is None:
542 // #            return
543 // #        if self.last_pos is None:
544 // #            self.last_pos = self.gps.gps_position.GetPosition()
545 // #        (fields, tstamp, lat, lon, alt) = self.last_pos
546 // #        self.gpx.waypoint(tstamp, lat, lon, alt)
547 // #        zavai.info("Making waypoint at %s: %f, %f, %f" % (
548 // #            time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(tstamp)), lat, lon, alt))
549
550 public zavai.gps.GPS gps = null;
551 public zavai.gps.Monitor monitor = null;
552 public zavai.gps.Position position = null;
553 public zavai.gps.GPX gpx = null;
554
555 public void init()
556 {
557         gps = new GPS();
558         monitor = new Monitor();
559         position = new Position();
560         gpx = new GPX();
561
562         zavai.registry.register_service(gps);
563         zavai.registry.register_service(position);
564         zavai.registry.register_service(monitor);
565         zavai.registry.register_service(gpx);
566 }
567
568 }
569 }