mygps.c : Updated command line usage to be compliant with Luceor
[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   Warning: on google maps, point are : latitude,longitude (not
10     longitude,latitude)
11 */
12
13 #include <stdio.h>
14 #include <stdlib.h>
15 #include <string.h>
16 #include <gps.h>
17 #include <errno.h>
18 #include <sys/select.h>
19
20 struct timeval tv_now;
21
22 double gps_lat_ori = 48.82408;
23 double gps_long_ori = 2.261415;
24
25 void gps_time_set(struct gps_data_t *gps) {
26         static double prev_fix_time = 0.0;
27         double cur_diff;
28         static double pre_diff = 0.0;
29
30         if (gps->fix.time > prev_fix_time) {
31                 double now = (double)tv_now.tv_sec +
32                         (double)tv_now.tv_usec * 0.000001;
33                 cur_diff = gps->fix.time - now;
34                 printf("Fix time: %f s (diff: %f, vdiff: %f)\n", gps->fix.time,
35                        cur_diff, cur_diff - pre_diff);
36                 pre_diff = cur_diff;
37                 prev_fix_time = gps->fix.time;
38         } else {
39                 printf("Fix time: %f s\n", gps->fix.time);
40         }
41 }
42
43 void gps_timerr_set(struct gps_data_t *gps) {
44         /* Apparently, this is set to 0.005 s by default, so we probably
45          * cannot rely on it*/
46         printf("Fix time error: %f s\n", gps->fix.ept);
47 }
48
49 void gps_latlon_set(struct gps_data_t *gps) {
50         double d;
51
52         d = earth_distance(gps_lat_ori, gps_long_ori,
53                            gps->fix.latitude, gps->fix.longitude);
54         printf("Latitude: %f °, Longitude: %f ° (Distance: %f m)\n",
55                gps->fix.latitude, gps->fix.longitude, d);
56 }
57
58 void gps_altitude_set(struct gps_data_t *gps) {
59         printf("Altitude: %f m\n", gps->fix.altitude);
60 }
61
62 void gps_speed_set(struct gps_data_t *gps) {
63         printf("Horizontal speed: %f m/s\n", gps->fix.speed);
64 }
65
66 void gps_track_set(struct gps_data_t *gps) {
67         /* what does this mean ? */
68         printf("Track: %f °\n", gps->fix.track);
69 }
70
71 void gps_climb_set(struct gps_data_t *gps) {
72         printf("Vertical speed: %f m/s\n", gps->fix.climb);
73 }
74
75 void gps_status_set(struct gps_data_t *gps) {
76         const char *status_values[] = {
77                 [STATUS_NO_FIX]         = "No",
78                 [STATUS_FIX]            = "Yes (without DGPS)",
79                 [STATUS_DGPS_FIX]       = "Yes (with DGPS)" };
80         printf("Fix:%s\n", status_values[gps->status]);
81 }
82
83 void gps_mode_set(struct gps_data_t *gps) {
84
85         const char *mode_values[] = {
86                 [MODE_NOT_SEEN] = "Not seen",
87                 [MODE_NO_FIX]   = "No FIX ",
88                 [MODE_2D]       = "2D FIX",
89                 [MODE_3D]       = "3D FIX" };
90
91         printf("Mode: %s\n", mode_values[gps->fix.mode]);
92 }
93
94 void gps_version_set(struct gps_data_t *gps) {
95
96         printf("Running libgps API version %d.%d (%s %s)\n",
97                gps->version.proto_major,
98                gps->version.proto_minor,
99                gps->version.release, gps->version.rev);
100 }
101
102 void gps_herr_set(struct gps_data_t *gps) {
103         printf("Latitude error: %f m, Longitude error: %f m\n",
104                gps->fix.epy, gps->fix.epx);
105 }
106
107 void gps_verr_set(struct gps_data_t *gps) {
108         printf("Altitude error: %f m\n", gps->fix.epv);
109 }
110
111 void gps_policy_set(struct gps_data_t *gps) {
112         printf("watcher:%d json:%d nmea:%d timing:%d devpath:%s\n",
113                gps->policy.watcher, gps->policy.json,
114                gps->policy.nmea, gps->policy.timing,
115                gps->policy.devpath);
116 }
117
118 void gps_satellite_set(struct gps_data_t *gps) {
119         int i;
120
121         printf("Satellites in view: %d\n",
122                gps->satellites_visible);
123         for (i = 0; i < gps->satellites_visible; i++) {
124                 printf("    %2.2d: %2.2d %3.3d %3.0f dB, %c\n",
125                        gps->PRN[i], gps->elevation[i],
126                        gps->azimuth[i], gps->ss[i],
127                        gps->used[i] ? 'Y' : 'N');
128         }
129 }
130
131 void gps_speederr_set(struct gps_data_t *gps) {
132         printf("Horizontal speed error: %f m/s\n", gps->fix.eps);
133 }
134
135 void gps_trackerr_set(struct gps_data_t *gps) {
136         printf("Track error: %f °\n", gps->fix.epd);
137 }
138
139 void gps_climberr_set(struct gps_data_t *gps) {
140         printf("Vertical speed error: %f m/s\n", gps->fix.epc);
141 }
142
143 void gps_device_set(struct gps_data_t *gps) {
144         printf("Device is %s, driver is %s\n",
145                gps->dev.path, gps->dev.driver);
146 }
147
148 void gps_devicelist_set(struct gps_data_t *gps) {
149
150         int i;
151                         
152         printf("Time: %f s, %d device(s) :",
153                gps->devices.time, gps->devices.ndevices);
154         for (i=0; i<gps->devices.ndevices; i++) {
155                 printf("\t%s\n", gps->devices.list[i].path);
156         }
157 }
158
159 void gps_packet_set(struct gps_data_t *gps) {
160         /* display nothing on purpose */
161 }
162
163 struct gps_handler_t {
164         gps_mask_t set;
165         void (*handler)(struct gps_data_t *gps);
166 } gps_handlers[] = {
167         { ONLINE_SET,           NULL },
168         { TIME_SET,             gps_time_set },
169         { TIMERR_SET,           gps_timerr_set },
170         { LATLON_SET,           gps_latlon_set },
171         { ALTITUDE_SET,         gps_altitude_set },
172         { SPEED_SET,            gps_speed_set },
173         { TRACK_SET,            gps_track_set },
174         { CLIMB_SET,            gps_climb_set },
175         { STATUS_SET,           gps_status_set },
176         { MODE_SET,             gps_mode_set },
177         { DOP_SET,              NULL },
178         { VERSION_SET,          gps_version_set },
179         { HERR_SET,             gps_herr_set },
180         { VERR_SET,             gps_verr_set },
181         { ATTITUDE_SET,         NULL },
182         { POLICY_SET,           gps_policy_set },
183         { SATELLITE_SET,        gps_satellite_set },
184         /*{ RAW_SET,            NULL },*/
185         /*{ USED_SET,           NULL },*/
186         { SPEEDERR_SET,         gps_speederr_set },
187         { TRACKERR_SET,         gps_trackerr_set },
188         { CLIMBERR_SET,         gps_climberr_set },
189         { DEVICE_SET,           gps_device_set },
190         { DEVICELIST_SET,       gps_devicelist_set },
191         { DEVICEID_SET,         NULL },
192         /*{ ERROR_SET,          NULL },*/
193         { RTCM2_SET,            NULL },
194         { RTCM3_SET,            NULL },
195         { AIS_SET,              NULL },
196         { PACKET_SET,           gps_packet_set },
197 };
198
199 #define sizetab(tab) (sizeof(tab)/sizeof(tab[0]))
200
201 void usage() {
202         printf("usage: mygps [-set longitude,latitude] [-get|-log]\n");
203         exit (-1);
204 }
205
206 int main(int argc, const char *argv[]) {
207         int i;
208         struct gps_data_t gpsdata, *gps = &gpsdata;
209         int get_mode = 0, get_mode_done = 0;
210         int log_mode = 0;
211         int set_mode = 0;
212         double ref_longitude, ref_latitude;
213         int debug_mode = 0;
214
215         for (i=1; i<argc; i++) {
216           if (strcmp(argv[i],"-get")==0) {
217             get_mode = 1;
218           } else if (strcmp(argv[i],"-log")==0) {
219             log_mode = 1;
220           } else if (strcmp(argv[i],"-set")==0 && i+1<argc) {
221             set_mode = 1;
222             if (sscanf(argv[i+1],"%lf,%lf", &ref_longitude, &ref_latitude) != 2)
223               usage();
224             i++;
225           } else if (strcmp(argv[i],"-debug")==0) {
226             debug_mode = 1;
227           } else {
228             usage();
229           }
230         }
231
232         if (!get_mode && !log_mode)
233           usage();
234
235         if (debug_mode) {
236           printf("Compiled libgps API version %d.%d\n",
237                  GPSD_API_MAJOR_VERSION, GPSD_API_MINOR_VERSION);
238         }
239
240         do {
241           
242           /* NULL, NULL matches localhost and the default GPSD port */
243 #if GPS_API_MAJOR_VERSION < 5
244           if ((gps = gps_open(NULL, NULL)) == NULL) {
245 #endif
246 #if GPSD_API_MAJOR_VERSION == 5
247           if (gps_open(NULL, NULL, &gpsdata) != 0) {
248 #endif
249             fprintf(stderr, "gps_open: %s\n", gps_errstr(errno));
250             return -1;
251           }
252           
253           if (gps_stream(gps, WATCH_ENABLE, NULL) != 0) {
254             fprintf(stderr, "gps_stream() failed\n");
255             return -1;
256           }
257           
258           do {
259             int i, r;
260             fd_set fdr;
261             struct timeval tv = { 10, 0 };
262             gps_mask_t set;
263             
264             /* wait for data of gps->gps_fd */
265             
266             FD_ZERO(&fdr);
267             FD_SET(gps->gps_fd, &fdr);
268             
269             r = select(FD_SETSIZE, &fdr, NULL, NULL, &tv);
270             if (r == 0) {
271               fprintf(stderr,"Time-out reading from GPS daemon\n");
272               break;
273             }
274             
275             /* Depending on the version of libgps, you need to
276                call gps_read() or gps_poll() */
277             if (gps_read(gps) < 0) {
278               perror("gps_read");
279               break;
280             }
281             
282             gettimeofday(&tv_now, NULL);
283             
284             if (debug_mode) {
285               printf("--- %lu.%06lu ---\n", 
286                      tv_now.tv_sec, tv_now.tv_usec);
287               
288               set = gps->set;
289               for (i=0; i<sizetab(gps_handlers); i++) {
290                 if (set & gps_handlers[i].set &&
291                     gps_handlers[i].handler) {
292                   set &= ~gps_handlers[i].set;
293                   gps_handlers[i].handler(gps);
294                 }
295               }
296               if (set) {
297                 printf("=> gps->set not decoded : 0x%x\n",
298                        set);
299               }
300             }
301             
302             if ((log_mode || get_mode) &&
303                 (gps->set & LATLON_SET) &&
304                 (gps->set & ALTITUDE_SET) &&
305                 (gps->set & HERR_SET) &&
306                 (gps->set & VERR_SET)) {
307
308               /* <time recv>
309                  <longitude in °> <latitude in °> <altitude in m>
310                  <distance with reference point or 0 >
311                  <longitude error in m> <latitude error in m> 
312                  <altitude error in m>
313                  <horizontal speed in m/s> <vertical speed in m/s>
314                  <horizontal speed error in m/s>
315                  <vertical speed error in m/s>
316               */
317               printf("%u.%06lu-%f %f %f %f %f %f %f %f %f %f %f %f\n",
318                      /* time recv */
319                      tv_now.tv_sec, tv_now.tv_usec, gps->fix.time,
320                      /* longitude in °, latitude in °, altitude in m */
321                      gps->fix.longitude, gps->fix.latitude, gps->fix.altitude,
322                      /* distance with ref point */
323                      set_mode ? earth_distance(gps->fix.latitude, gps->fix.longitude,
324                                                ref_latitude, ref_longitude) : 0.0,
325                      /* longitude error in m, latitude error in m, altitude error in m */
326                      gps->fix.epx, gps->fix.epy, gps->fix.epv,
327                      /* horizontal speed in m/s */
328                      gps->set & SPEED_SET ? gps->fix.speed : 0.0,
329                      /* vertical speed in m/s */
330                      gps->set & CLIMB_SET ? gps->fix.climb : 0.0,
331                      /* horizontal speed error in m/s */
332                      gps->set & SPEEDERR_SET ? gps->fix.eps : 0.0,
333                      /* vertical speed error in m/s */
334                      gps->set & CLIMBERR_SET ? gps->fix.epc : 0.0);
335               fflush(stdout);
336               if (get_mode)
337                 get_mode_done = 1;
338             }
339           } while (!get_mode_done);
340             
341           gps_close(gps);
342
343         } while (!get_mode_done);
344         
345         return 0;
346 }