git.gag.com Git - fw/altos/blob - src/test/ao_gps_test_ublox.c

   1 /*
   2  * Copyright © 2009 Keith Packard <keithp@keithp.com>
   3  *
   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.
   8  *
   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.
  13  *
  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.
  17  */
  18
  19 #define AO_GPS_TEST
  20 #define AO_TICK_TYPE uint32_t
  21 #define HAS_GPS 1
  22 #define ao_tick_count 0
  23 #include "ao_host.h"
  24 #include <termios.h>
  25 #include <errno.h>
  26 #include <sys/types.h>
  27 #include <sys/stat.h>
  28 #include <stdbool.h>
  29 #include <fcntl.h>
  30 #include <unistd.h>
  31 #define AO_GPS_NUM_SAT_MASK     (0xf << 0)
  32 #define AO_GPS_NUM_SAT_SHIFT    (0)
  33
  34 #define AO_GPS_NEW_DATA         1
  35 #define AO_GPS_NEW_TRACKING     2
  36
  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)
  41
  42 struct ao_telemetry_location {
  43         uint8_t                 year;
  44         uint8_t                 month;
  45         uint8_t                 day;
  46         uint8_t                 hour;
  47         uint8_t                 minute;
  48         uint8_t                 second;
  49         uint8_t                 flags;
  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 */
  62 };
  63
  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)))
  69
  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)
  78
  79 struct ao_telemetry_satellite_info {
  80         uint8_t         svid;
  81         uint8_t         c_n_1;
  82 };
  83
  84 #define AO_TELEMETRY_SATELLITE_MAX_SAT  12
  85
  86 struct ao_telemetry_satellite {
  87         uint8_t                                 channels;
  88         struct ao_telemetry_satellite_info      sats[AO_TELEMETRY_SATELLITE_MAX_SAT];
  89 };
  90
  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
  94
  95 extern struct ao_telemetry_location     ao_gps_data;
  96 extern struct ao_telemetry_satellite    ao_gps_tracking_data;
  97
  98 uint8_t ao_gps_mutex;
  99
 100 void
 101 ao_mutex_get(uint8_t *mutex)
 102 {
 103 }
 104
 105 void
 106 ao_mutex_put(uint8_t *mutex)
 107 {
 108 }
 109
 110 static int ao_gps_fd;
 111 static FILE *ao_gps_file;
 112
 113 #if 0
 114 static void
 115 ao_dbg_char(char c)
 116 {
 117         char    line[128];
 118         line[0] = '\0';
 119         if (c < ' ') {
 120                 if (c == '\n')
 121                         sprintf (line, "\n");
 122                 else
 123                         sprintf (line, "\\%02x", ((int) c) & 0xff);
 124         } else {
 125                 sprintf (line, "%c", c);
 126         }
 127         write(1, line, strlen(line));
 128 }
 129 #endif
 130
 131 #include <sys/time.h>
 132
 133 int
 134 get_millis(void)
 135 {
 136         struct timeval  tv;
 137         gettimeofday(&tv, NULL);
 138         return tv.tv_sec * 1000 + tv.tv_usec / 1000;
 139 }
 140
 141 static uint8_t  in_message[4096];
 142 static int      in_len;
 143 static uint16_t recv_len;
 144
 145 static void check_ublox_message(char *which, uint8_t *msg);
 146
 147 char
 148 ao_gps_getchar(void)
 149 {
 150         char    c;
 151         uint8_t uc;
 152         int     i;
 153
 154         i = getc(ao_gps_file);
 155         if (i == EOF) {
 156                 perror("getchar");
 157                 exit(1);
 158         }
 159         c = i;
 160         uc = (uint8_t) c;
 161         if (in_len || uc == 0xb5) {
 162                 in_message[in_len++] = c;
 163                 if (in_len == 6) {
 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);
 167                         in_len = 0;
 168                 }
 169
 170         }
 171         return c;
 172 }
 173
 174 #define MESSAGE_LEN     4096
 175
 176 static uint8_t  message[MESSAGE_LEN];
 177 static int      message_len;
 178 static uint16_t send_len;
 179
 180 void
 181 ao_gps_putchar(char c)
 182 {
 183         int     i;
 184         uint8_t uc = (uint8_t) c;
 185
 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);
 193                         message_len = 0;
 194                 }
 195         }
 196
 197         for (;;) {
 198                 i = write(ao_gps_fd, &c, 1);
 199                 if (i == 1)
 200                         break;
 201                 if (i < 0 && (errno == EINTR || errno == EAGAIN))
 202                         continue;
 203                 perror("putchar");
 204                 exit(1);
 205         }
 206 }
 207
 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
 212
 213 static void
 214 ao_gps_set_speed(uint8_t speed)
 215 {
 216         int     fd = ao_gps_fd;
 217         struct termios  termios;
 218
 219         printf ("\t\tset speed %d\n", speed);
 220         tcdrain(fd);
 221         tcgetattr(fd, &termios);
 222         switch (speed) {
 223         case AO_SERIAL_SPEED_4800:
 224                 cfsetspeed(&termios, B4800);
 225                 break;
 226         case AO_SERIAL_SPEED_9600:
 227                 cfsetspeed(&termios, B9600);
 228                 break;
 229         case AO_SERIAL_SPEED_57600:
 230                 cfsetspeed(&termios, B57600);
 231                 break;
 232         case AO_SERIAL_SPEED_115200:
 233                 cfsetspeed(&termios, B115200);
 234                 break;
 235         }
 236         tcsetattr(fd, TCSAFLUSH, &termios);
 237         tcflush(fd, TCIFLUSH);
 238 }
 239
 240 #define ao_time() 0
 241
 242 uint8_t ao_task_minimize_latency;
 243
 244 #define ao_usb_getchar()        0
 245
 246 #include "ao_gps_print.c"
 247 #include "ao_gps_show.c"
 248 #include "ao_gps_ublox.c"
 249
 250 static void
 251 check_ublox_message(char *which, uint8_t *msg)
 252 {
 253         uint8_t class = msg[0];
 254         uint8_t id = msg[1];
 255         uint16_t len = msg[2] | (msg[3] << 8);
 256         uint16_t i;
 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 };
 260
 261         for (i = 0; i < 4 + len; i++) {
 262                 add_cksum(&cksum, msg[i]);
 263         }
 264         if (cksum.a != cksum_msg.a || cksum.b != cksum_msg.b) {
 265                 printf ("\t%s: cksum mismatch %02x,%02x != %02x,%02x\n",
 266                         which,
 267                         cksum_msg.a & 0xff,
 268                         cksum_msg.b & 0xff,
 269                         cksum.a & 0xff,
 270                         cksum.b & 0xff);
 271                 return;
 272         }
 273         switch (class) {
 274         case UBLOX_NAV:
 275                 switch (id) {
 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",
 279                                 nav_dop->itow,
 280                                 nav_dop->gdop,
 281                                 nav_dop->ddop,
 282                                 nav_dop->tdop,
 283                                 nav_dop->vdop,
 284                                 nav_dop->hdop,
 285                                 nav_dop->ndop,
 286                                 nav_dop->edop);
 287                         return;
 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",
 291                                 nav_posllh->itow,
 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);
 298                         return;
 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);
 304                         return;
 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);
 309                         int i;
 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);
 321                         }
 322                         return;
 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",
 326                                 nav_velned->itow,
 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);
 335                         return;
 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",
 339                                 nav_timeutc->itow,
 340                                 nav_timeutc->tacc,
 341                                 nav_timeutc->nano,
 342                                 nav_timeutc->year,
 343                                 nav_timeutc->month,
 344                                 nav_timeutc->day,
 345                                 nav_timeutc->hour,
 346                                 nav_timeutc->min,
 347                                 nav_timeutc->sec,
 348                                 nav_timeutc->valid);
 349                         return;
 350                 }
 351                 break;
 352         }
 353 #if 1
 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);
 358 #endif
 359         printf ("\n");
 360 }
 361
 362 void
 363 ao_dump_state(void *wchan)
 364 {
 365         if (wchan == &ao_gps_new)
 366                 ao_gps_show();
 367         return;
 368 }
 369
 370 int
 371 ao_gps_open(const char *tty)
 372 {
 373         struct termios  termios;
 374         int fd;
 375
 376         fd = open (tty, O_RDWR);
 377         if (fd < 0)
 378                 return -1;
 379
 380         tcgetattr(fd, &termios);
 381         cfmakeraw(&termios);
 382         cfsetspeed(&termios, B4800);
 383         tcsetattr(fd, TCSAFLUSH, &termios);
 384
 385         tcdrain(fd);
 386         tcflush(fd, TCIFLUSH);
 387         return fd;
 388 }
 389
 390 #include <getopt.h>
 391
 392 static const struct option options[] = {
 393         { .name = "tty", .has_arg = 1, .val = 'T' },
 394         { 0, 0, 0, 0},
 395 };
 396
 397 static void usage(char *program)
 398 {
 399         fprintf(stderr, "usage: %s [--tty <tty-name>]\n", program);
 400         exit(1);
 401 }
 402
 403 int
 404 main (int argc, char **argv)
 405 {
 406         char    *tty = "/dev/ttyUSB0";
 407         int     c;
 408
 409         while ((c = getopt_long(argc, argv, "T:", options, NULL)) != -1) {
 410                 switch (c) {
 411                 case 'T':
 412                         tty = optarg;
 413                         break;
 414                 default:
 415                         usage(argv[0]);
 416                         break;
 417                 }
 418         }
 419         ao_gps_fd = ao_gps_open(tty);
 420         if (ao_gps_fd < 0) {
 421                 perror (tty);
 422                 exit (1);
 423         }
 424         ao_gps_file = fdopen(ao_gps_fd, "r");
 425         ao_gps();
 426         return 0;
 427 }