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; version 2 of the License.
   7  *
   8  * This program is distributed in the hope that it will be useful, but
   9  * WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11  * General Public License for more details.
  12  *
  13  * You should have received a copy of the GNU General Public License along
  14  * with this program; if not, write to the Free Software Foundation, Inc.,
  15  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  16  */
  17
  18 #define AO_GPS_TEST
  19 #include "ao_host.h"
  20 #include <termios.h>
  21 #include <errno.h>
  22 #include <sys/types.h>
  23 #include <sys/stat.h>
  24 #include <fcntl.h>
  25 #include <unistd.h>
  26 #define AO_GPS_NUM_SAT_MASK     (0xf << 0)
  27 #define AO_GPS_NUM_SAT_SHIFT    (0)
  28
  29 #define AO_GPS_VALID            (1 << 4)
  30 #define AO_GPS_RUNNING          (1 << 5)
  31 #define AO_GPS_DATE_VALID       (1 << 6)
  32 #define AO_GPS_COURSE_VALID     (1 << 7)
  33
  34 struct ao_telemetry_location {
  35         uint8_t                 year;
  36         uint8_t                 month;
  37         uint8_t                 day;
  38         uint8_t                 hour;
  39         uint8_t                 minute;
  40         uint8_t                 second;
  41         uint8_t                 flags;
  42         int32_t                 latitude;       /* degrees * 10⁷ */
  43         int32_t                 longitude;      /* degrees * 10⁷ */
  44         int16_t                 altitude;       /* m */
  45         uint16_t                ground_speed;   /* cm/s */
  46         uint8_t                 course;         /* degrees / 2 */
  47         uint8_t                 pdop;           /* * 5 */
  48         uint8_t                 hdop;           /* * 5 */
  49         uint8_t                 vdop;           /* * 5 */
  50         int16_t                 climb_rate;     /* cm/s */
  51         uint16_t                h_error;        /* m */
  52         uint16_t                v_error;        /* m */
  53 };
  54
  55 #define UBLOX_SAT_STATE_ACQUIRED                (1 << 0)
  56 #define UBLOX_SAT_STATE_CARRIER_PHASE_VALID     (1 << 1)
  57 #define UBLOX_SAT_BIT_SYNC_COMPLETE             (1 << 2)
  58 #define UBLOX_SAT_SUBFRAME_SYNC_COMPLETE        (1 << 3)
  59 #define UBLOX_SAT_CARRIER_PULLIN_COMPLETE       (1 << 4)
  60 #define UBLOX_SAT_CODE_LOCKED                   (1 << 5)
  61 #define UBLOX_SAT_ACQUISITION_FAILED            (1 << 6)
  62 #define UBLOX_SAT_EPHEMERIS_AVAILABLE           (1 << 7)
  63
  64 struct ao_telemetry_satellite_info {
  65         uint8_t         svid;
  66         uint8_t         c_n_1;
  67 };
  68
  69 #define AO_TELEMETRY_SATELLITE_MAX_SAT  12
  70
  71 struct ao_telemetry_satellite {
  72         uint8_t                                 channels;
  73         struct ao_telemetry_satellite_info      sats[AO_TELEMETRY_SATELLITE_MAX_SAT];
  74 };
  75
  76 #define ao_gps_orig ao_telemetry_location
  77 #define ao_gps_tracking_orig ao_telemetry_satellite
  78 #define ao_gps_sat_orig ao_telemetry_satellite_info
  79
  80 void
  81 ao_mutex_get(uint8_t *mutex)
  82 {
  83 }
  84
  85 void
  86 ao_mutex_put(uint8_t *mutex)
  87 {
  88 }
  89
  90 static int ao_gps_fd;
  91 static FILE *ao_gps_file;
  92
  93 #if 0
  94 static void
  95 ao_dbg_char(char c)
  96 {
  97         char    line[128];
  98         line[0] = '\0';
  99         if (c < ' ') {
 100                 if (c == '\n')
 101                         sprintf (line, "\n");
 102                 else
 103                         sprintf (line, "\\%02x", ((int) c) & 0xff);
 104         } else {
 105                 sprintf (line, "%c", c);
 106         }
 107         write(1, line, strlen(line));
 108 }
 109 #endif
 110
 111 #include <sys/time.h>
 112
 113 int
 114 get_millis(void)
 115 {
 116         struct timeval  tv;
 117         gettimeofday(&tv, NULL);
 118         return tv.tv_sec * 1000 + tv.tv_usec / 1000;
 119 }
 120
 121 static uint8_t  in_message[4096];
 122 static int      in_len;
 123 static uint16_t recv_len;
 124
 125 static void check_ublox_message(char *which, uint8_t *msg);
 126
 127 char
 128 ao_serial1_getchar(void)
 129 {
 130         char    c;
 131         uint8_t uc;
 132         int     i;
 133
 134         i = getc(ao_gps_file);
 135         if (i == EOF) {
 136                 perror("getchar");
 137                 exit(1);
 138         }
 139         c = i;
 140         uc = (uint8_t) c;
 141         if (in_len || uc == 0xb5) {
 142                 in_message[in_len++] = c;
 143                 if (in_len == 6) {
 144                         recv_len = in_message[4] | (in_message[5] << 8);
 145                 } else if (in_len > 6 && in_len == recv_len + 8) {
 146                         check_ublox_message("recv", in_message + 2);
 147                         in_len = 0;
 148                 }
 149
 150         }
 151         return c;
 152 }
 153
 154 #define MESSAGE_LEN     4096
 155
 156 static uint8_t  message[MESSAGE_LEN];
 157 static int      message_len;
 158 static uint16_t send_len;
 159
 160 void
 161 ao_serial1_putchar(char c)
 162 {
 163         int     i;
 164         uint8_t uc = (uint8_t) c;
 165
 166         if (message_len || uc == 0xb5) {
 167                 if (message_len < MESSAGE_LEN)
 168                         message[message_len++] = uc;
 169                 if (message_len == 6) {
 170                         send_len = message[4] | (message[5] << 8);
 171                 } else if (message_len > 6 && message_len == send_len + 8) {
 172                         check_ublox_message("send", message + 2);
 173                         message_len = 0;
 174                 }
 175         }
 176
 177         for (;;) {
 178                 i = write(ao_gps_fd, &c, 1);
 179                 if (i == 1)
 180                         break;
 181                 if (i < 0 && (errno == EINTR || errno == EAGAIN))
 182                         continue;
 183                 perror("putchar");
 184                 exit(1);
 185         }
 186 }
 187
 188 #define AO_SERIAL_SPEED_4800    0
 189 #define AO_SERIAL_SPEED_9600    1
 190 #define AO_SERIAL_SPEED_57600   2
 191 #define AO_SERIAL_SPEED_115200  3
 192
 193 static void
 194 ao_serial1_set_speed(uint8_t speed)
 195 {
 196         int     fd = ao_gps_fd;
 197         struct termios  termios;
 198
 199         printf ("\t\tset speed %d\n", speed);
 200         tcdrain(fd);
 201         tcgetattr(fd, &termios);
 202         switch (speed) {
 203         case AO_SERIAL_SPEED_4800:
 204                 cfsetspeed(&termios, B4800);
 205                 break;
 206         case AO_SERIAL_SPEED_9600:
 207                 cfsetspeed(&termios, B9600);
 208                 break;
 209         case AO_SERIAL_SPEED_57600:
 210                 cfsetspeed(&termios, B57600);
 211                 break;
 212         case AO_SERIAL_SPEED_115200:
 213                 cfsetspeed(&termios, B115200);
 214                 break;
 215         }
 216         tcsetattr(fd, TCSAFLUSH, &termios);
 217         tcflush(fd, TCIFLUSH);
 218 }
 219
 220 #define ao_time() 0
 221
 222 uint8_t ao_task_minimize_latency;
 223
 224 #define ao_usb_getchar()        0
 225
 226 #include "ao_gps_print.c"
 227 #include "ao_gps_ublox.c"
 228
 229 static void
 230 check_ublox_message(char *which, uint8_t *msg)
 231 {
 232         uint8_t class = msg[0];
 233         uint8_t id = msg[1];
 234         uint16_t len = msg[2] | (msg[3] << 8);
 235         uint16_t i;
 236         struct ao_ublox_cksum   cksum_msg = { .a = msg[4 + len],
 237                                               .b = msg[4 + len + 1] };
 238         struct ao_ublox_cksum   cksum= { 0, 0 };
 239
 240         for (i = 0; i < 4 + len; i++) {
 241                 add_cksum(&cksum, msg[i]);
 242         }
 243         if (cksum.a != cksum_msg.a || cksum.b != cksum_msg.b) {
 244                 printf ("\t%s: cksum mismatch %02x,%02x != %02x,%02x\n",
 245                         which,
 246                         cksum_msg.a & 0xff,
 247                         cksum_msg.b & 0xff,
 248                         cksum.a & 0xff,
 249                         cksum.b & 0xff);
 250                 return;
 251         }
 252         switch (class) {
 253         case UBLOX_NAV:
 254                 switch (id) {
 255                 case UBLOX_NAV_DOP: ;
 256                         struct ublox_nav_dop    *nav_dop = (void *) msg;
 257                         printf ("\tnav-dop    iTOW %9u gDOP %5u dDOP %5u tDOP %5u vDOP %5u hDOP %5u nDOP %5u eDOP %5u\n",
 258                                 nav_dop->itow,
 259                                 nav_dop->gdop,
 260                                 nav_dop->ddop,
 261                                 nav_dop->tdop,
 262                                 nav_dop->vdop,
 263                                 nav_dop->hdop,
 264                                 nav_dop->ndop,
 265                                 nav_dop->edop);
 266                         return;
 267                 case UBLOX_NAV_POSLLH: ;
 268                         struct ublox_nav_posllh *nav_posllh = (void *) msg;
 269                         printf ("\tnav-posllh iTOW %9u lon %12.7f lat %12.7f height %10.3f hMSL %10.3f hAcc %10.3f vAcc %10.3f\n",
 270                                 nav_posllh->itow,
 271                                 nav_posllh->lon / 1e7,
 272                                 nav_posllh->lat / 1e7,
 273                                 nav_posllh->height / 1e3,
 274                                 nav_posllh->hmsl / 1e3,
 275                                 nav_posllh->hacc / 1e3,
 276                                 nav_posllh->vacc / 1e3);
 277                         return;
 278                 case UBLOX_NAV_SOL: ;
 279                         struct ublox_nav_sol    *nav_sol = (struct ublox_nav_sol *) msg;
 280                         printf ("\tnav-sol    iTOW %9u fTOW %9d week %5d gpsFix %2d flags %02x\n",
 281                                 nav_sol->itow, nav_sol->ftow, nav_sol->week,
 282                                 nav_sol->gpsfix, nav_sol->flags);
 283                         return;
 284                 case UBLOX_NAV_SVINFO: ;
 285                         struct ublox_nav_svinfo *nav_svinfo = (struct ublox_nav_svinfo *) msg;
 286                         printf ("\tnav-svinfo iTOW %9u numCH %3d globalFlags %02x\n",
 287                                 nav_svinfo->itow, nav_svinfo->numch, nav_svinfo->globalflags);
 288                         int i;
 289                         for (i = 0; i < nav_svinfo->numch; i++) {
 290                                 struct ublox_nav_svinfo_block *nav_svinfo_block = (void *) (msg + 12 + 12 * i);
 291                                 printf ("\t\tchn %3u svid %3u flags %02x quality %3u cno %3u elev %3d azim %6d prRes %9d\n",
 292                                         nav_svinfo_block->chn,
 293                                         nav_svinfo_block->svid,
 294                                         nav_svinfo_block->flags,
 295                                         nav_svinfo_block->quality,
 296                                         nav_svinfo_block->cno,
 297                                         nav_svinfo_block->elev,
 298                                         nav_svinfo_block->azim,
 299                                         nav_svinfo_block->prres);
 300                         }
 301                         return;
 302                 case UBLOX_NAV_VELNED: ;
 303                         struct ublox_nav_velned *nav_velned = (void *) msg;
 304                         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",
 305                                 nav_velned->itow,
 306                                 nav_velned->veln / 1e2,
 307                                 nav_velned->vele / 1e2,
 308                                 nav_velned->veld / 1e2,
 309                                 nav_velned->speed / 1e2,
 310                                 nav_velned->gspeed / 1e2,
 311                                 nav_velned->heading / 1e5,
 312                                 nav_velned->sacc / 1e5,
 313                                 nav_velned->cacc / 1e6);
 314                         return;
 315                 case UBLOX_NAV_TIMEUTC:;
 316                         struct ublox_nav_timeutc *nav_timeutc = (void *) msg;
 317                         printf ("\tnav-timeutc iTOW %9u tAcc %5u nano %5d %4u-%2d-%2d %2d:%02d:%02d flags %02x\n",
 318                                 nav_timeutc->itow,
 319                                 nav_timeutc->tacc,
 320                                 nav_timeutc->nano,
 321                                 nav_timeutc->year,
 322                                 nav_timeutc->month,
 323                                 nav_timeutc->day,
 324                                 nav_timeutc->hour,
 325                                 nav_timeutc->min,
 326                                 nav_timeutc->sec,
 327                                 nav_timeutc->valid);
 328                         return;
 329                 }
 330                 break;
 331         }
 332 #if 1
 333         printf ("\t%s: class %02x id %02x len %d:", which, class & 0xff, id & 0xff, len & 0xffff);
 334         for (i = 0; i < len; i++)
 335                 printf (" %02x", msg[4 + i]);
 336         printf (" cksum %02x %02x", cksum_msg.a & 0xff, cksum_msg.b & 0xff);
 337 #endif
 338         printf ("\n");
 339 }
 340
 341 void
 342 ao_dump_state(void *wchan)
 343 {
 344         if (wchan == &ao_gps_data)
 345                 ao_gps_print(&ao_gps_data);
 346         else
 347                 ao_gps_tracking_print(&ao_gps_tracking_data);
 348         putchar('\n');
 349         return;
 350 }
 351
 352 int
 353 ao_gps_open(const char *tty)
 354 {
 355         struct termios  termios;
 356         int fd;
 357
 358         fd = open (tty, O_RDWR);
 359         if (fd < 0)
 360                 return -1;
 361
 362         tcgetattr(fd, &termios);
 363         cfmakeraw(&termios);
 364         cfsetspeed(&termios, B4800);
 365         tcsetattr(fd, TCSAFLUSH, &termios);
 366
 367         tcdrain(fd);
 368         tcflush(fd, TCIFLUSH);
 369         return fd;
 370 }
 371
 372 #include <getopt.h>
 373
 374 static const struct option options[] = {
 375         { .name = "tty", .has_arg = 1, .val = 'T' },
 376         { 0, 0, 0, 0},
 377 };
 378
 379 static void usage(char *program)
 380 {
 381         fprintf(stderr, "usage: %s [--tty <tty-name>]\n", program);
 382         exit(1);
 383 }
 384
 385 int
 386 main (int argc, char **argv)
 387 {
 388         char    *tty = "/dev/ttyUSB0";
 389         int     c;
 390
 391         while ((c = getopt_long(argc, argv, "T:", options, NULL)) != -1) {
 392                 switch (c) {
 393                 case 'T':
 394                         tty = optarg;
 395                         break;
 396                 default:
 397                         usage(argv[0]);
 398                         break;
 399                 }
 400         }
 401         ao_gps_fd = ao_gps_open(tty);
 402         if (ao_gps_fd < 0) {
 403                 perror (tty);
 404                 exit (1);
 405         }
 406         ao_gps_file = fdopen(ao_gps_fd, "r");
 407         ao_gps();
 408         return 0;
 409 }