Initial source code using libgps (http://gpsd.berlios.de/ and various script
[gps] / mygps.c
1 /*
2   Author : Benoit Papillault <benoit.papillault@free.fr>
3   Creation : 2011-06-08
4   Goal : sample example to use libgps
5
6   In order to compile :
7   # aptitude install libgps-dev
8 */
9
10 #include <stdio.h>
11 #include <gps.h>
12 #include <errno.h>
13
14 struct timeval tv_now;
15
16 double gps_lat_ori = 48.82408;
17 double gps_long_ori = 2.261415;
18
19 void gps_time_set(struct gps_data_t *gps) {
20         static double prev_fix_time = 0.0;
21         double cur_diff;
22         static double pre_diff = 0.0;
23
24         if (gps->fix.time > prev_fix_time) {
25                 double now = (double)tv_now.tv_sec +
26                         (double)tv_now.tv_usec * 0.000001;
27                 cur_diff = gps->fix.time - now;
28                 printf("Fix time: %f s (diff: %f, vdiff: %f)\n", gps->fix.time,
29                        cur_diff, cur_diff - pre_diff);
30                 pre_diff = cur_diff;
31                 prev_fix_time = gps->fix.time;
32         } else {
33                 printf("Fix time: %f s\n", gps->fix.time);
34         }
35 }
36
37 void gps_timerr_set(struct gps_data_t *gps) {
38         /* Apparently, this is set to 0.005 s by default, so we probably
39          * cannot rely on it*/
40         printf("Fix time error: %f s\n", gps->fix.ept);
41 }
42
43 void gps_latlon_set(struct gps_data_t *gps) {
44         double d;
45
46         d = earth_distance(gps_lat_ori, gps_long_ori,
47                            gps->fix.latitude, gps->fix.longitude);
48         printf("Latitude: %f °, Longitude: %f ° (Distance: %f m)\n",
49                gps->fix.latitude, gps->fix.longitude, d);
50 }
51
52 void gps_altitude_set(struct gps_data_t *gps) {
53         printf("Altitude: %f m\n", gps->fix.altitude);
54 }
55
56 void gps_speed_set(struct gps_data_t *gps) {
57         printf("Horizontal speed: %f m/s\n", gps->fix.speed);
58 }
59
60 void gps_track_set(struct gps_data_t *gps) {
61         /* what does this mean ? */
62         printf("Track: %f °\n", gps->fix.track);
63 }
64
65 void gps_climb_set(struct gps_data_t *gps) {
66         printf("Vertical speed: %f m/s\n", gps->fix.climb);
67 }
68
69 void gps_status_set(struct gps_data_t *gps) {
70         const char *status_values[] = {
71                 [STATUS_NO_FIX]         = "No",
72                 [STATUS_FIX]            = "Yes (without DGPS)",
73                 [STATUS_DGPS_FIX]       = "Yes (with DGPS)" };
74         printf("Fix:%s\n", status_values[gps->status]);
75 }
76
77 void gps_mode_set(struct gps_data_t *gps) {
78
79         const char *mode_values[] = {
80                 [MODE_NOT_SEEN] = "Not seen",
81                 [MODE_NO_FIX]   = "No FIX ",
82                 [MODE_2D]       = "2D FIX",
83                 [MODE_3D]       = "3D FIX" };
84
85         printf("Mode: %s\n", mode_values[gps->fix.mode]);
86 }
87
88 void gps_version_set(struct gps_data_t *gps) {
89
90         printf("Running libgps API version %d.%d (%s %s)\n",
91                gps->version.proto_major,
92                gps->version.proto_minor,
93                gps->version.release, gps->version.rev);
94 }
95
96 void gps_herr_set(struct gps_data_t *gps) {
97         printf("Latitude error: %f m, Longitude error: %f m\n",
98                gps->fix.epy, gps->fix.epx);
99 }
100
101 void gps_verr_set(struct gps_data_t *gps) {
102         printf("Altitude error: %f m\n", gps->fix.epv);
103 }
104
105 void gps_policy_set(struct gps_data_t *gps) {
106         printf("watcher:%d json:%d nmea:%d timing:%d devpath:%s\n",
107                gps->policy.watcher, gps->policy.json,
108                gps->policy.nmea, gps->policy.timing,
109                gps->policy.devpath);
110 }
111
112 void gps_satellite_set(struct gps_data_t *gps) {
113         int i;
114
115         printf("Satellites in view: %d\n",
116                gps->satellites_visible);
117         for (i = 0; i < gps->satellites_visible; i++) {
118                 printf("    %2.2d: %2.2d %3.3d %3.0f dB, %c\n",
119                        gps->PRN[i], gps->elevation[i],
120                        gps->azimuth[i], gps->ss[i],
121                        gps->used[i] ? 'Y' : 'N');
122         }
123 }
124
125 void gps_speederr_set(struct gps_data_t *gps) {
126         printf("Horizontal speed error: %f m/s\n", gps->fix.eps);
127 }
128
129 void gps_trackerr_set(struct gps_data_t *gps) {
130         printf("Track error: %f °\n", gps->fix.epd);
131 }
132
133 void gps_climberr_set(struct gps_data_t *gps) {
134         printf("Vertical speed error: %f m/s\n", gps->fix.epc);
135 }
136
137 void gps_device_set(struct gps_data_t *gps) {
138         printf("Device is %s, driver is %s\n",
139                gps->dev.path, gps->dev.driver);
140 }
141
142 void gps_devicelist_set(struct gps_data_t *gps) {
143
144         int i;
145                         
146         printf("Time: %f s, %d device(s) :",
147                gps->devices.time, gps->devices.ndevices);
148         for (i=0; i<gps->devices.ndevices; i++) {
149                 printf("\t%s\n", gps->devices.list[i].path);
150         }
151 }
152
153 void gps_packet_set(struct gps_data_t *gps) {
154         /* display nothing on purpose */
155 }
156
157 struct gps_handler_t {
158         gps_mask_t set;
159         void (*handler)(struct gps_data_t *gps);
160 } gps_handlers[] = {
161         { ONLINE_SET,           NULL },
162         { TIME_SET,             gps_time_set },
163         { TIMERR_SET,           gps_timerr_set },
164         { LATLON_SET,           gps_latlon_set },
165         { ALTITUDE_SET,         gps_altitude_set },
166         { SPEED_SET,            gps_speed_set },
167         { TRACK_SET,            gps_track_set },
168         { CLIMB_SET,            gps_climb_set },
169         { STATUS_SET,           gps_status_set },
170         { MODE_SET,             gps_mode_set },
171         { DOP_SET,              NULL },
172         { VERSION_SET,          gps_version_set },
173         { HERR_SET,             gps_herr_set },
174         { VERR_SET,             gps_verr_set },
175         { ATTITUDE_SET,         NULL },
176         { POLICY_SET,           gps_policy_set },
177         { SATELLITE_SET,        gps_satellite_set },
178         { RAW_SET,              NULL },
179         { USED_SET,             NULL },
180         { SPEEDERR_SET,         gps_speederr_set },
181         { TRACKERR_SET,         gps_trackerr_set },
182         { CLIMBERR_SET,         gps_climberr_set },
183         { DEVICE_SET,           gps_device_set },
184         { DEVICELIST_SET,       gps_devicelist_set },
185         { DEVICEID_SET,         NULL },
186         { ERROR_SET,            NULL },
187         { RTCM2_SET,            NULL },
188         { RTCM3_SET,            NULL },
189         { AIS_SET,              NULL },
190         { PACKET_SET,           gps_packet_set },
191 };
192
193 #define sizetab(tab) (sizeof(tab)/sizeof(tab[0]))
194
195 int main() {
196         struct gps_data_t *gps;
197
198         printf("Compiled libgps API version %d.%d\n",
199                GPSD_API_MAJOR_VERSION, GPSD_API_MINOR_VERSION);
200
201         /* NULL, NULL matches localhost and the default GPSD port */
202         if ((gps = gps_open(NULL, NULL)) == NULL) {
203                 fprintf(stderr, "gps_open: %s\n", gps_errstr(errno));
204                 return -1;
205         }
206
207         gps_stream(gps, WATCH_ENABLE, NULL);
208
209         for (;;) {
210                 int i;
211
212                 if (gps_read(gps) < 0) {
213                         perror("gps_read");
214                         break;
215                 }
216
217                 gettimeofday(&tv_now, NULL);
218                 printf("--- %lu.%06lu ---\n", 
219                        tv_now.tv_sec, tv_now.tv_usec);
220
221                 for (i=0; i<sizetab(gps_handlers); i++) {
222                         if (gps->set & gps_handlers[i].set &&
223                             gps_handlers[i].handler) {
224                                 gps->set &= ~gps_handlers[i].set;
225                                 gps_handlers[i].handler(gps);
226                         }
227                 }
228                 fflush(stdout);
229
230                 if (gps->set) {
231                         printf("=> gps->set not decoded : 0x%x\n", gps->set);
232                 }
233         }
234
235
236         gps_close(gps);
237
238         return 0;
239 }