2 * Copyright © 2009 Keith Packard <keithp@keithp.com>
4 * This program is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation; either version 2 of the License, or
7 * (at your option) any later version.
9 * This program is distributed in the hope that it will be useful, but
10 * WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12 * General Public License for more details.
14 * You should have received a copy of the GNU General Public License along
15 * with this program; if not, write to the Free Software Foundation, Inc.,
16 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
20 #define AO_TICK_TYPE uint32_t
22 #define ao_tick_count 0
26 #include <sys/types.h>
31 #define AO_GPS_NUM_SAT_MASK (0xf << 0)
32 #define AO_GPS_NUM_SAT_SHIFT (0)
34 #define AO_GPS_NEW_DATA 1
35 #define AO_GPS_NEW_TRACKING 2
37 #define AO_GPS_VALID (1 << 4)
38 #define AO_GPS_RUNNING (1 << 5)
39 #define AO_GPS_DATE_VALID (1 << 6)
40 #define AO_GPS_COURSE_VALID (1 << 7)
42 struct ao_telemetry_location {
50 int32_t latitude; /* degrees * 10⁷ */
51 int32_t longitude; /* degrees * 10⁷ */
52 int16_t altitude_low; /* m */
53 uint16_t ground_speed; /* cm/s */
54 uint8_t course; /* degrees / 2 */
55 uint8_t pdop; /* * 5 */
56 uint8_t hdop; /* * 5 */
57 uint8_t vdop; /* * 5 */
58 int16_t climb_rate; /* cm/s */
59 uint16_t h_error; /* m */
60 uint16_t v_error; /* m */
61 int16_t altitude_high; /* m */
64 typedef int32_t gps_alt_t;
65 #define AO_TELEMETRY_LOCATION_ALTITUDE(l) (((gps_alt_t) (l)->altitude_high << 16) | ((l)->altitude_low))
66 #define AO_GPS_ORIG_ALTITUDE(l) AO_TELEMETRY_LOCATION_ALTITUDE(l)
67 #define AO_TELEMETRY_LOCATION_SET_ALTITUDE(l,a) (((l)->altitude_high = (a) >> 16), \
68 ((l)->altitude_low = (a)))
70 #define UBLOX_SAT_STATE_ACQUIRED (1 << 0)
71 #define UBLOX_SAT_STATE_CARRIER_PHASE_VALID (1 << 1)
72 #define UBLOX_SAT_BIT_SYNC_COMPLETE (1 << 2)
73 #define UBLOX_SAT_SUBFRAME_SYNC_COMPLETE (1 << 3)
74 #define UBLOX_SAT_CARRIER_PULLIN_COMPLETE (1 << 4)
75 #define UBLOX_SAT_CODE_LOCKED (1 << 5)
76 #define UBLOX_SAT_ACQUISITION_FAILED (1 << 6)
77 #define UBLOX_SAT_EPHEMERIS_AVAILABLE (1 << 7)
79 struct ao_telemetry_satellite_info {
84 #define AO_TELEMETRY_SATELLITE_MAX_SAT 12
86 struct ao_telemetry_satellite {
88 struct ao_telemetry_satellite_info sats[AO_TELEMETRY_SATELLITE_MAX_SAT];
91 #define ao_gps_orig ao_telemetry_location
92 #define ao_gps_tracking_orig ao_telemetry_satellite
93 #define ao_gps_sat_orig ao_telemetry_satellite_info
95 extern struct ao_telemetry_location ao_gps_data;
96 extern struct ao_telemetry_satellite ao_gps_tracking_data;
101 ao_mutex_get(uint8_t *mutex)
106 ao_mutex_put(uint8_t *mutex)
110 static int ao_gps_fd;
111 static FILE *ao_gps_file;
121 sprintf (line, "\n");
123 sprintf (line, "\\%02x", ((int) c) & 0xff);
125 sprintf (line, "%c", c);
127 write(1, line, strlen(line));
131 #include <sys/time.h>
137 gettimeofday(&tv, NULL);
138 return tv.tv_sec * 1000 + tv.tv_usec / 1000;
141 static uint8_t in_message[4096];
143 static uint16_t recv_len;
145 static void check_ublox_message(char *which, uint8_t *msg);
154 i = getc(ao_gps_file);
161 if (in_len || uc == 0xb5) {
162 in_message[in_len++] = c;
164 recv_len = in_message[4] | (in_message[5] << 8);
165 } else if (in_len > 6 && in_len == recv_len + 8) {
166 check_ublox_message("recv", in_message + 2);
174 #define MESSAGE_LEN 4096
176 static uint8_t message[MESSAGE_LEN];
177 static int message_len;
178 static uint16_t send_len;
181 ao_gps_putchar(char c)
184 uint8_t uc = (uint8_t) c;
186 if (message_len || uc == 0xb5) {
187 if (message_len < MESSAGE_LEN)
188 message[message_len++] = uc;
189 if (message_len == 6) {
190 send_len = message[4] | (message[5] << 8);
191 } else if (message_len > 6 && message_len == send_len + 8) {
192 check_ublox_message("send", message + 2);
198 i = write(ao_gps_fd, &c, 1);
201 if (i < 0 && (errno == EINTR || errno == EAGAIN))
208 #define AO_SERIAL_SPEED_4800 0
209 #define AO_SERIAL_SPEED_9600 1
210 #define AO_SERIAL_SPEED_57600 2
211 #define AO_SERIAL_SPEED_115200 3
214 ao_gps_set_speed(uint8_t speed)
217 struct termios termios;
219 printf ("\t\tset speed %d\n", speed);
221 tcgetattr(fd, &termios);
223 case AO_SERIAL_SPEED_4800:
224 cfsetspeed(&termios, B4800);
226 case AO_SERIAL_SPEED_9600:
227 cfsetspeed(&termios, B9600);
229 case AO_SERIAL_SPEED_57600:
230 cfsetspeed(&termios, B57600);
232 case AO_SERIAL_SPEED_115200:
233 cfsetspeed(&termios, B115200);
236 tcsetattr(fd, TCSAFLUSH, &termios);
237 tcflush(fd, TCIFLUSH);
242 uint8_t ao_task_minimize_latency;
244 #define ao_usb_getchar() 0
246 #include "ao_gps_print.c"
247 #include "ao_gps_show.c"
248 #include "ao_gps_ublox.c"
251 check_ublox_message(char *which, uint8_t *msg)
253 uint8_t class = msg[0];
255 uint16_t len = msg[2] | (msg[3] << 8);
257 struct ao_ublox_cksum cksum_msg = { .a = msg[4 + len],
258 .b = msg[4 + len + 1] };
259 struct ao_ublox_cksum cksum= { 0, 0 };
261 for (i = 0; i < 4 + len; i++) {
262 add_cksum(&cksum, msg[i]);
264 if (cksum.a != cksum_msg.a || cksum.b != cksum_msg.b) {
265 printf ("\t%s: cksum mismatch %02x,%02x != %02x,%02x\n",
276 case UBLOX_NAV_DOP: ;
277 struct ublox_nav_dop *nav_dop = (void *) msg;
278 printf ("\tnav-dop iTOW %9u gDOP %5u dDOP %5u tDOP %5u vDOP %5u hDOP %5u nDOP %5u eDOP %5u\n",
288 case UBLOX_NAV_POSLLH: ;
289 struct ublox_nav_posllh *nav_posllh = (void *) msg;
290 printf ("\tnav-posllh iTOW %9u lon %12.7f lat %12.7f height %10.3f hMSL %10.3f hAcc %10.3f vAcc %10.3f\n",
292 nav_posllh->lon / 1e7,
293 nav_posllh->lat / 1e7,
294 nav_posllh->height / 1e3,
295 nav_posllh->hmsl / 1e3,
296 nav_posllh->hacc / 1e3,
297 nav_posllh->vacc / 1e3);
299 case UBLOX_NAV_SOL: ;
300 struct ublox_nav_sol *nav_sol = (struct ublox_nav_sol *) msg;
301 printf ("\tnav-sol iTOW %9u fTOW %9d week %5d gpsFix %2d flags %02x\n",
302 nav_sol->itow, nav_sol->ftow, nav_sol->week,
303 nav_sol->gpsfix, nav_sol->flags);
305 case UBLOX_NAV_SVINFO: ;
306 struct ublox_nav_svinfo *nav_svinfo = (struct ublox_nav_svinfo *) msg;
307 printf ("\tnav-svinfo iTOW %9u numCH %3d globalFlags %02x\n",
308 nav_svinfo->itow, nav_svinfo->numch, nav_svinfo->globalflags);
310 for (i = 0; i < nav_svinfo->numch; i++) {
311 struct ublox_nav_svinfo_block *nav_svinfo_block = (void *) (msg + 12 + 12 * i);
312 printf ("\t\tchn %3u svid %3u flags %02x quality %3u cno %3u elev %3d azim %6d prRes %9d\n",
313 nav_svinfo_block->chn,
314 nav_svinfo_block->svid,
315 nav_svinfo_block->flags,
316 nav_svinfo_block->quality,
317 nav_svinfo_block->cno,
318 nav_svinfo_block->elev,
319 nav_svinfo_block->azim,
320 nav_svinfo_block->prres);
323 case UBLOX_NAV_VELNED: ;
324 struct ublox_nav_velned *nav_velned = (void *) msg;
325 printf ("\tnav-velned iTOW %9u velN %10.2f velE %10.2f velD %10.2f speed %10.2f gSpeed %10.2f heading %10.5f sAcc %10.2f cAcc %10.5f\n",
327 nav_velned->veln / 1e2,
328 nav_velned->vele / 1e2,
329 nav_velned->veld / 1e2,
330 nav_velned->speed / 1e2,
331 nav_velned->gspeed / 1e2,
332 nav_velned->heading / 1e5,
333 nav_velned->sacc / 1e5,
334 nav_velned->cacc / 1e6);
336 case UBLOX_NAV_TIMEUTC:;
337 struct ublox_nav_timeutc *nav_timeutc = (void *) msg;
338 printf ("\tnav-timeutc iTOW %9u tAcc %5u nano %5d %4u-%2d-%2d %2d:%02d:%02d flags %02x\n",
354 printf ("\t%s: class %02x id %02x len %d:", which, class & 0xff, id & 0xff, len & 0xffff);
355 for (i = 0; i < len; i++)
356 printf (" %02x", msg[4 + i]);
357 printf (" cksum %02x %02x", cksum_msg.a & 0xff, cksum_msg.b & 0xff);
363 ao_dump_state(void *wchan)
365 if (wchan == &ao_gps_new)
371 ao_gps_open(const char *tty)
373 struct termios termios;
376 fd = open (tty, O_RDWR);
380 tcgetattr(fd, &termios);
382 cfsetspeed(&termios, B4800);
383 tcsetattr(fd, TCSAFLUSH, &termios);
386 tcflush(fd, TCIFLUSH);
392 static const struct option options[] = {
393 { .name = "tty", .has_arg = 1, .val = 'T' },
397 static void usage(char *program)
399 fprintf(stderr, "usage: %s [--tty <tty-name>]\n", program);
404 main (int argc, char **argv)
406 char *tty = "/dev/ttyUSB0";
409 while ((c = getopt_long(argc, argv, "T:", options, NULL)) != -1) {
419 ao_gps_fd = ao_gps_open(tty);
424 ao_gps_file = fdopen(ao_gps_fd, "r");