git.gag.com Git - fw/altos/blob - src/drivers/ao_gps_ublox.c

   1 /*
   2  * Copyright © 2013 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 #ifndef AO_GPS_TEST
  19 #include "ao.h"
  20 #endif
  21
  22 #include "ao_gps_ublox.h"
  23
  24 #define AO_UBLOX_DEBUG 1
  25
  26 #include <stdarg.h>
  27
  28 __xdata uint8_t ao_gps_mutex;
  29 __pdata uint16_t ao_gps_tick;
  30 __xdata struct ao_telemetry_location    ao_gps_data;
  31 __xdata struct ao_telemetry_satellite   ao_gps_tracking_data;
  32
  33 #ifndef AO_SERIAL_SPEED_UBLOX
  34 #define AO_SERIAL_SPEED_UBLOX AO_SERIAL_SPEED_57600
  35 #endif
  36
  37 #if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_57600
  38 #define SERIAL_SPEED_STRING     "57600"
  39 #endif
  40 #if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_19200
  41 #define SERIAL_SPEED_STRING     "19200"
  42 #endif
  43 #if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_9600
  44 #define SERIAL_SPEED_STRING     "9600"
  45 #endif
  46
  47 static const char ao_gps_set_nmea[] = "\r\n$PUBX,41,1,3,1," SERIAL_SPEED_STRING ",0*2d\r\n";
  48
  49 struct ao_ublox_cksum {
  50         uint8_t a, b;
  51 };
  52
  53 static __pdata struct ao_ublox_cksum ao_ublox_cksum;
  54 static __pdata uint16_t ao_ublox_len;
  55
  56 #if AO_UBLOX_DEBUG
  57
  58 static uint8_t ao_gps_dbg_enable;
  59
  60 #define DBG_PROTO       1
  61 #define DBG_CHAR        2
  62 #define DBG_INIT        4
  63
  64 static void ao_gps_dbg(int level, char *format, ...) {
  65         va_list a;
  66
  67         if (level & ao_gps_dbg_enable) {
  68                 va_start(a, format);
  69                 vprintf(format, a);
  70                 va_end(a);
  71                 flush();
  72         }
  73 }
  74
  75 #else
  76 #define ao_gps_dbg(fmt, ...)
  77 #endif
  78
  79 static inline uint8_t ao_ublox_byte(void) {
  80         uint8_t c = (uint8_t) ao_gps_getchar();
  81         ao_gps_dbg(DBG_CHAR, " %02x", c);
  82         return c;
  83 }
  84
  85 static inline void add_cksum(struct ao_ublox_cksum *cksum, uint8_t c)
  86 {
  87         cksum->a += c;
  88         cksum->b += cksum->a;
  89 }
  90
  91 static void ao_ublox_init_cksum(void)
  92 {
  93         ao_ublox_cksum.a = ao_ublox_cksum.b = 0;
  94 }
  95
  96 static void ao_ublox_put_u8(uint8_t c)
  97 {
  98         add_cksum(&ao_ublox_cksum, c);
  99         ao_gps_dbg(DBG_CHAR, " (%02x)", c);
 100         ao_gps_putchar(c);
 101 }
 102
 103 static void ao_ublox_put_i8(int8_t c)
 104 {
 105         ao_ublox_put_u8((uint8_t) c);
 106 }
 107
 108 static void ao_ublox_put_u16(uint16_t c)
 109 {
 110         ao_ublox_put_u8(c);
 111         ao_ublox_put_u8(c>>8);
 112 }
 113
 114 #if 0
 115 static void ao_ublox_put_i16(int16_t c)
 116 {
 117         ao_ublox_put_u16((uint16_t) c);
 118 }
 119 #endif
 120
 121 static void ao_ublox_put_u32(uint32_t c)
 122 {
 123         ao_ublox_put_u8(c);
 124         ao_ublox_put_u8(c>>8);
 125         ao_ublox_put_u8(c>>16);
 126         ao_ublox_put_u8(c>>24);
 127 }
 128
 129 static void ao_ublox_put_i32(int32_t c)
 130 {
 131         ao_ublox_put_u32((uint32_t) c);
 132 }
 133
 134 static uint8_t header_byte(void)
 135 {
 136         uint8_t c = ao_ublox_byte();
 137         add_cksum(&ao_ublox_cksum, c);
 138         return c;
 139 }
 140
 141 static uint8_t data_byte(void)
 142 {
 143         --ao_ublox_len;
 144         return header_byte();
 145 }
 146
 147 static char __xdata *ublox_target;
 148
 149 static void ublox_u16(uint8_t offset)
 150 {
 151         uint16_t __xdata *ptr = (uint16_t __xdata *) (ublox_target + offset);
 152         uint16_t val;
 153
 154         val = data_byte();
 155         val |= data_byte () << 8;
 156         *ptr = val;
 157 }
 158
 159 static void ublox_u8(uint8_t offset)
 160 {
 161         uint8_t __xdata *ptr = (uint8_t __xdata *) (ublox_target + offset);
 162         uint8_t val;
 163
 164         val = data_byte ();
 165         *ptr = val;
 166 }
 167
 168 static void ublox_u32(uint8_t offset) __reentrant
 169 {
 170         uint32_t __xdata *ptr = (uint32_t __xdata *) (ublox_target + offset);
 171         uint32_t val;
 172
 173         val = ((uint32_t) data_byte ());
 174         val |= ((uint32_t) data_byte ()) << 8;
 175         val |= ((uint32_t) data_byte ()) << 16;
 176         val |= ((uint32_t) data_byte ()) << 24;
 177         *ptr = val;
 178 }
 179
 180 static void ublox_discard(uint8_t len)
 181 {
 182         while (len--)
 183                 data_byte();
 184 }
 185
 186 #define UBLOX_END       0
 187 #define UBLOX_DISCARD   1
 188 #define UBLOX_U8        2
 189 #define UBLOX_U16       3
 190 #define UBLOX_U32       4
 191
 192 struct ublox_packet_parse {
 193         uint8_t type;
 194         uint8_t offset;
 195 };
 196
 197 static void
 198 ao_ublox_parse(void __xdata *target, const struct ublox_packet_parse *parse) __reentrant
 199 {
 200         uint8_t i, offset;
 201
 202         ublox_target = target;
 203         for (i = 0; ; i++) {
 204                 offset = parse[i].offset;
 205                 switch (parse[i].type) {
 206                 case UBLOX_END:
 207                         return;
 208                 case UBLOX_DISCARD:
 209                         ublox_discard(offset);
 210                         break;
 211                 case UBLOX_U8:
 212                         ublox_u8(offset);
 213                         break;
 214                 case UBLOX_U16:
 215                         ublox_u16(offset);
 216                         break;
 217                 case UBLOX_U32:
 218                         ublox_u32(offset);
 219                         break;
 220                 }
 221         }
 222         ao_gps_dbg(DBG_PROTO, "\n");
 223 }
 224
 225 /*
 226  * NAV-DOP message parsing
 227  */
 228
 229 static struct nav_dop {
 230         uint16_t        pdop;
 231         uint16_t        hdop;
 232         uint16_t        vdop;
 233 } nav_dop;
 234
 235 static const struct ublox_packet_parse nav_dop_packet[] = {
 236         { UBLOX_DISCARD, 6 },                                   /* 0 GPS Millisecond Time of Week, gDOP */
 237         { UBLOX_U16, offsetof(struct nav_dop, pdop) },          /* 6 pDOP */
 238         { UBLOX_DISCARD, 2 },                                   /* 8 tDOP */
 239         { UBLOX_U16, offsetof(struct nav_dop, vdop) },          /* 10 vDOP */
 240         { UBLOX_U16, offsetof(struct nav_dop, hdop) },          /* 12 hDOP */
 241         { UBLOX_DISCARD, 4 },                                   /* 14 nDOP, eDOP */
 242         { UBLOX_END, 0 }
 243 };
 244
 245 static void
 246 ao_ublox_parse_nav_dop(void)
 247 {
 248         ao_ublox_parse(&nav_dop, nav_dop_packet);
 249 }
 250
 251 /*
 252  * NAV-POSLLH message parsing
 253  */
 254 static struct nav_posllh {
 255         int32_t         lat;
 256         int32_t         lon;
 257         int32_t         alt_msl;
 258 } nav_posllh;
 259
 260 static const struct ublox_packet_parse nav_posllh_packet[] = {
 261         { UBLOX_DISCARD, 4 },                                           /* 0 GPS Millisecond Time of Week */
 262         { UBLOX_U32, offsetof(struct nav_posllh, lon) },                /* 4 Longitude */
 263         { UBLOX_U32, offsetof(struct nav_posllh, lat) },                /* 8 Latitude */
 264         { UBLOX_DISCARD, 4 },                                           /* 12 Height above Ellipsoid */
 265         { UBLOX_U32, offsetof(struct nav_posllh, alt_msl) },            /* 16 Height above mean sea level */
 266         { UBLOX_DISCARD, 8 },                                           /* 20 hAcc, vAcc */
 267         { UBLOX_END, 0 },                                               /* 28 */
 268 };
 269
 270 static void
 271 ao_ublox_parse_nav_posllh(void)
 272 {
 273         ao_ublox_parse(&nav_posllh, nav_posllh_packet);
 274 }
 275
 276 /*
 277  * NAV-SOL message parsing
 278  */
 279 static struct nav_sol {
 280         uint8_t         gps_fix;
 281         uint8_t         flags;
 282         uint8_t         nsat;
 283 } nav_sol;
 284
 285 static const struct ublox_packet_parse nav_sol_packet[] = {
 286         { UBLOX_DISCARD, 10 },                                          /* 0 iTOW, fTOW, week */
 287         { UBLOX_U8, offsetof(struct nav_sol, gps_fix) },                /* 10 gpsFix */
 288         { UBLOX_U8, offsetof(struct nav_sol, flags) },                  /* 11 flags */
 289         { UBLOX_DISCARD, 35 },                                          /* 12 ecefX, ecefY, ecefZ, pAcc, ecefVX, ecefVY, ecefVZ, sAcc, pDOP, reserved1 */
 290         { UBLOX_U8, offsetof(struct nav_sol, nsat) },                   /* 47 numSV */
 291         { UBLOX_DISCARD, 4 },                                           /* 48 reserved2 */
 292         { UBLOX_END, 0 }
 293 };
 294
 295 #define NAV_SOL_FLAGS_GPSFIXOK          0
 296 #define NAV_SOL_FLAGS_DIFFSOLN          1
 297 #define NAV_SOL_FLAGS_WKNSET            2
 298 #define NAV_SOL_FLAGS_TOWSET            3
 299
 300 static void
 301 ao_ublox_parse_nav_sol(void)
 302 {
 303         ao_ublox_parse(&nav_sol, nav_sol_packet);
 304 }
 305
 306 /*
 307  * NAV-SVINFO message parsing
 308  */
 309
 310 static struct nav_svinfo {
 311         uint8_t num_ch;
 312         uint8_t flags;
 313 } nav_svinfo;
 314
 315 static const struct ublox_packet_parse nav_svinfo_packet[] = {
 316         { UBLOX_DISCARD, 4 },                                           /* 0 iTOW */
 317         { UBLOX_U8, offsetof(struct nav_svinfo, num_ch) },              /* 4 numCh */
 318         { UBLOX_U8, offsetof(struct nav_svinfo, flags) },               /* 5 globalFlags */
 319         { UBLOX_DISCARD, 2 },                                           /* 6 reserved2 */
 320         { UBLOX_END, 0 }
 321 };
 322
 323 #define NAV_SVINFO_MAX_SAT      16
 324
 325 static struct nav_svinfo_sat {
 326         uint8_t chn;
 327         uint8_t svid;
 328         uint8_t flags;
 329         uint8_t quality;
 330         uint8_t cno;
 331 } nav_svinfo_sat[NAV_SVINFO_MAX_SAT];
 332
 333 static uint8_t  nav_svinfo_nsat;
 334
 335 static const struct ublox_packet_parse nav_svinfo_sat_packet[] = {
 336         { UBLOX_U8, offsetof(struct nav_svinfo_sat, chn) },             /* 8 + 12*N chn */
 337         { UBLOX_U8, offsetof(struct nav_svinfo_sat, svid) },            /* 9 + 12*N svid */
 338         { UBLOX_U8, offsetof(struct nav_svinfo_sat, flags) },           /* 10 + 12*N flags */
 339         { UBLOX_U8, offsetof(struct nav_svinfo_sat, quality) },         /* 11 + 12*N quality */
 340         { UBLOX_U8, offsetof(struct nav_svinfo_sat, cno) },             /* 12 + 12*N cno */
 341         { UBLOX_DISCARD, 7 },                                           /* 13 + 12*N elev, azim, prRes */
 342         { UBLOX_END, 0 }
 343 };
 344
 345 #define NAV_SVINFO_SAT_FLAGS_SVUSED             0
 346 #define NAV_SVINFO_SAT_FLAGS_DIFFCORR           1
 347 #define NAV_SVINFO_SAT_FLAGS_ORBITAVAIL         2
 348 #define NAV_SVINFO_SAT_FLAGS_ORBITEPH           3
 349 #define NAV_SVINFO_SAT_FLAGS_UNHEALTHY          4
 350 #define NAV_SVINFO_SAT_FLAGS_ORBITALM           5
 351 #define NAV_SVINFO_SAT_FLAGS_ORBITAOP           6
 352 #define NAV_SVINFO_SAT_FLAGS_SMOOTHED           7
 353
 354 #define NAV_SVINFO_SAT_QUALITY_IDLE             0
 355 #define NAV_SVINFO_SAT_QUALITY_SEARCHING        1
 356 #define NAV_SVINFO_SAT_QUALITY_ACQUIRED         2
 357 #define NAV_SVINFO_SAT_QUALITY_UNUSABLE         3
 358 #define NAV_SVINFO_SAT_QUALITY_LOCKED           4
 359 #define NAV_SVINFO_SAT_QUALITY_RUNNING          5
 360
 361 static void
 362 ao_ublox_parse_nav_svinfo(void)
 363 {
 364         uint8_t nsat;
 365         nav_svinfo_nsat = 0;
 366
 367         ao_ublox_parse(&nav_svinfo, nav_svinfo_packet);
 368         for (nsat = 0; nsat < nav_svinfo.num_ch && ao_ublox_len >= 12; nsat++) {
 369                 if (nsat < NAV_SVINFO_MAX_SAT) {
 370                         ao_ublox_parse(&nav_svinfo_sat[nav_svinfo_nsat++], nav_svinfo_sat_packet);
 371                 } else {
 372                         ublox_discard(12);
 373                 }
 374         }
 375 #if AO_UBLOX_DEBUG
 376         ao_gps_dbg(DBG_PROTO, "svinfo num_ch %d flags %02x\n", nav_svinfo.num_ch, nav_svinfo.flags);
 377         for (nsat = 0; nsat < nav_svinfo.num_ch; nsat++)
 378                 ao_gps_dbg(DBG_PROTO, "\t%d: chn %d svid %d flags %02x quality %d cno %d\n",
 379                             nsat,
 380                             nav_svinfo_sat[nsat].chn,
 381                             nav_svinfo_sat[nsat].svid,
 382                             nav_svinfo_sat[nsat].flags,
 383                             nav_svinfo_sat[nsat].quality,
 384                             nav_svinfo_sat[nsat].cno);
 385 #endif
 386 }
 387
 388 /*
 389  * NAV-TIMEUTC message parsing
 390  */
 391 static struct nav_timeutc {
 392         uint16_t        year;
 393         uint8_t         month;
 394         uint8_t         day;
 395         uint8_t         hour;
 396         uint8_t         min;
 397         uint8_t         sec;
 398         uint8_t         valid;
 399 } nav_timeutc;
 400
 401 #define NAV_TIMEUTC_VALID_TOW   0
 402 #define NAV_TIMEUTC_VALID_WKN   1
 403 #define NAV_TIMEUTC_VALID_UTC   2
 404
 405 static const struct ublox_packet_parse nav_timeutc_packet[] = {
 406         { UBLOX_DISCARD, 12 },                                          /* 0 iTOW, tAcc, nano */
 407         { UBLOX_U16, offsetof(struct nav_timeutc, year) },              /* 12 year */
 408         { UBLOX_U8, offsetof(struct nav_timeutc, month) },              /* 14 month */
 409         { UBLOX_U8, offsetof(struct nav_timeutc, day) },                /* 15 day */
 410         { UBLOX_U8, offsetof(struct nav_timeutc, hour) },               /* 16 hour */
 411         { UBLOX_U8, offsetof(struct nav_timeutc, min) },                /* 17 min */
 412         { UBLOX_U8, offsetof(struct nav_timeutc, sec) },                /* 18 sec */
 413         { UBLOX_U8, offsetof(struct nav_timeutc, valid) },              /* 19 valid */
 414         { UBLOX_END, 0 }
 415 };
 416
 417 static void
 418 ao_ublox_parse_nav_timeutc(void)
 419 {
 420         ao_ublox_parse(&nav_timeutc, nav_timeutc_packet);
 421 }
 422
 423 /*
 424  * NAV-VELNED message parsing
 425  */
 426
 427 static struct nav_velned {
 428         int32_t         vel_d;
 429         uint32_t        g_speed;
 430         int32_t         heading;
 431 } nav_velned;
 432
 433 static const struct ublox_packet_parse nav_velned_packet[] = {
 434         { UBLOX_DISCARD, 12 },                                          /* 0 iTOW, velN, velE */
 435         { UBLOX_U32, offsetof(struct nav_velned, vel_d) },              /* 12 velD */
 436         { UBLOX_DISCARD, 4 },                                           /* 16 speed */
 437         { UBLOX_U32, offsetof(struct nav_velned, g_speed) },            /* 20 gSpeed */
 438         { UBLOX_U32, offsetof(struct nav_velned, heading) },            /* 24 heading */
 439         { UBLOX_DISCARD, 8 },                                           /* 28 sAcc, cAcc */
 440         { UBLOX_END, 0 }
 441 };
 442
 443 static void
 444 ao_ublox_parse_nav_velned(void)
 445 {
 446         ao_ublox_parse(&nav_velned, nav_velned_packet);
 447 }
 448
 449 /*
 450  * Set the protocol mode and baud rate
 451  */
 452
 453 static void
 454 ao_gps_delay(void)
 455 {
 456         uint8_t i;
 457
 458         /*
 459          * A bunch of nulls so the start bit
 460          * is clear
 461          */
 462
 463         for (i = 0; i < 64; i++)
 464                 ao_gps_putchar(0x00);
 465 }
 466
 467 static void
 468 ao_gps_setup(void)
 469 {
 470         uint8_t i, k;
 471
 472         ao_delay(AO_SEC_TO_TICKS(3));
 473
 474         ao_gps_dbg(DBG_INIT, "Set speed 9600\n");
 475         ao_gps_set_speed(AO_SERIAL_SPEED_9600);
 476
 477         /*
 478          * Send the baud-rate setting and protocol-setting
 479          * command three times
 480          */
 481         for (k = 0; k < 3; k++) {
 482                 ao_gps_delay();
 483
 484                 ao_gps_dbg(DBG_INIT, "Send initial setting\n");
 485                 for (i = 0; i < sizeof (ao_gps_set_nmea); i++)
 486                         ao_gps_putchar(ao_gps_set_nmea[i]);
 487         }
 488
 489         ao_gps_delay();
 490
 491 #if AO_SERIAL_SPEED_UBLOX != AO_SERIAL_SPEED_9600
 492         ao_gps_dbg(DBG_INIT, "Set speed high\n");
 493         /*
 494          * Increase the baud rate
 495          */
 496         ao_gps_set_speed(AO_SERIAL_SPEED_UBLOX);
 497 #endif
 498
 499         ao_gps_delay();
 500 }
 501
 502 static void
 503 ao_ublox_putstart(uint8_t class, uint8_t id, uint16_t len)
 504 {
 505         ao_ublox_init_cksum();
 506         ao_gps_putchar(0xb5);
 507         ao_gps_putchar(0x62);
 508         ao_ublox_put_u8(class);
 509         ao_ublox_put_u8(id);
 510         ao_ublox_put_u8(len);
 511         ao_ublox_put_u8(len >> 8);
 512 }
 513
 514 static void
 515 ao_ublox_putend(void)
 516 {
 517         ao_gps_putchar(ao_ublox_cksum.a);
 518         ao_gps_putchar(ao_ublox_cksum.b);
 519 }
 520
 521 static void
 522 ao_ublox_set_message_rate(uint8_t class, uint8_t msgid, uint8_t rate)
 523 {
 524         ao_ublox_putstart(0x06, 0x01, 3);
 525         ao_ublox_put_u8(class);
 526         ao_ublox_put_u8(msgid);
 527         ao_ublox_put_u8(rate);
 528         ao_ublox_putend();
 529 }
 530
 531 static void
 532 ao_ublox_set_navigation_settings(uint16_t mask,
 533                                  uint8_t dyn_model,
 534                                  uint8_t fix_mode,
 535                                  int32_t fixed_alt,
 536                                  uint32_t fixed_alt_var,
 537                                  int8_t min_elev,
 538                                  uint8_t dr_limit,
 539                                  uint16_t pdop,
 540                                  uint16_t tdop,
 541                                  uint16_t pacc,
 542                                  uint16_t tacc,
 543                                  uint8_t static_hold_thresh,
 544                                  uint8_t dgps_time_out)
 545 {
 546         ao_ublox_putstart(UBLOX_CFG, UBLOX_CFG_NAV5, 36);
 547         ao_ublox_put_u16(mask);
 548         ao_ublox_put_u8(dyn_model);
 549         ao_ublox_put_u8(fix_mode);
 550         ao_ublox_put_i32(fixed_alt);
 551         ao_ublox_put_u32(fixed_alt_var);
 552         ao_ublox_put_i8(min_elev);
 553         ao_ublox_put_u8(dr_limit);
 554         ao_ublox_put_u16(pdop);
 555         ao_ublox_put_u16(tdop);
 556         ao_ublox_put_u16(pacc);
 557         ao_ublox_put_u16(tacc);
 558         ao_ublox_put_u8(static_hold_thresh);
 559         ao_ublox_put_u8(dgps_time_out);
 560         ao_ublox_put_u32(0);
 561         ao_ublox_put_u32(0);
 562         ao_ublox_put_u32(0);
 563         ao_ublox_putend();
 564 }
 565
 566
 567 /*
 568  * Disable all MON message
 569  */
 570 static const uint8_t ublox_disable_mon[] = {
 571         0x0b, 0x09, 0x02, 0x06, 0x07, 0x21, 0x08, 0x04
 572 };
 573
 574 /*
 575  * Disable all NAV messages. The desired
 576  * ones will be explicitly re-enabled
 577  */
 578
 579 static const uint8_t ublox_disable_nav[] = {
 580         0x60, 0x22, 0x31, 0x04, 0x40, 0x01, 0x02, 0x32,
 581         0x06, 0x03, 0x30, 0x20, 0x21, 0x11, 0x12
 582 };
 583
 584 /*
 585  * Enable enough messages to get all of the data we want
 586  */
 587 static const uint8_t ublox_enable_nav[] = {
 588         UBLOX_NAV_DOP,
 589         UBLOX_NAV_POSLLH,
 590         UBLOX_NAV_SOL,
 591         UBLOX_NAV_SVINFO,
 592         UBLOX_NAV_VELNED,
 593         UBLOX_NAV_TIMEUTC
 594 };
 595
 596 void
 597 ao_gps(void) __reentrant
 598 {
 599         uint8_t                 class, id;
 600         struct ao_ublox_cksum   cksum;
 601         uint8_t                 i;
 602
 603         ao_gps_setup();
 604
 605         /* Disable all messages */
 606         for (i = 0; i < sizeof (ublox_disable_mon); i++)
 607                 ao_ublox_set_message_rate(0x0a, ublox_disable_mon[i], 0);
 608         for (i = 0; i < sizeof (ublox_disable_nav); i++)
 609                 ao_ublox_set_message_rate(UBLOX_NAV, ublox_disable_nav[i], 0);
 610
 611         /* Enable all of the messages we want */
 612         for (i = 0; i < sizeof (ublox_enable_nav); i++)
 613                 ao_ublox_set_message_rate(UBLOX_NAV, ublox_enable_nav[i], 1);
 614
 615         ao_ublox_set_navigation_settings((1 << UBLOX_CFG_NAV5_MASK_DYN) | (1 << UBLOX_CFG_NAV5_MASK_FIXMODE),
 616                                          UBLOX_CFG_NAV5_DYNMODEL_AIRBORNE_4G,
 617                                          UBLOX_CFG_NAV5_FIXMODE_3D,
 618                                          0,
 619                                          0,
 620                                          0,
 621                                          0,
 622                                          0,
 623                                          0,
 624                                          0,
 625                                          0,
 626                                          0,
 627                                          0);
 628
 629         for (;;) {
 630                 /* Locate the begining of the next record */
 631                 while (ao_ublox_byte() != (uint8_t) 0xb5)
 632                         ;
 633                 if (ao_ublox_byte() != (uint8_t) 0x62)
 634                         continue;
 635
 636                 ao_ublox_init_cksum();
 637
 638                 class = header_byte();
 639                 id = header_byte();
 640
 641                 /* Length */
 642                 ao_ublox_len = header_byte();
 643                 ao_ublox_len |= header_byte() << 8;
 644
 645                 ao_gps_dbg(DBG_PROTO, "class %02x id %02x len %d\n", class, id, ao_ublox_len);
 646
 647                 if (ao_ublox_len > 1023)
 648                         continue;
 649
 650                 switch (class) {
 651                 case UBLOX_NAV:
 652                         switch (id) {
 653                         case UBLOX_NAV_DOP:
 654                                 if (ao_ublox_len != 18)
 655                                         break;
 656                                 ao_ublox_parse_nav_dop();
 657                                 break;
 658                         case UBLOX_NAV_POSLLH:
 659                                 if (ao_ublox_len != 28)
 660                                         break;
 661                                 ao_ublox_parse_nav_posllh();
 662                                 break;
 663                         case UBLOX_NAV_SOL:
 664                                 if (ao_ublox_len != 52)
 665                                         break;
 666                                 ao_ublox_parse_nav_sol();
 667                                 break;
 668                         case UBLOX_NAV_SVINFO:
 669                                 if (ao_ublox_len < 8)
 670                                         break;
 671                                 ao_ublox_parse_nav_svinfo();
 672                                 break;
 673                         case UBLOX_NAV_VELNED:
 674                                 if (ao_ublox_len != 36)
 675                                         break;
 676                                 ao_ublox_parse_nav_velned();
 677                                 break;
 678                         case UBLOX_NAV_TIMEUTC:
 679                                 if (ao_ublox_len != 20)
 680                                         break;
 681                                 ao_ublox_parse_nav_timeutc();
 682                                 break;
 683                         }
 684                         break;
 685                 }
 686
 687                 if (ao_ublox_len != 0) {
 688                         ao_gps_dbg(DBG_PROTO, "len left %d\n", ao_ublox_len);
 689                         continue;
 690                 }
 691
 692                 /* verify checksum and end sequence */
 693                 cksum.a = ao_ublox_byte();
 694                 cksum.b = ao_ublox_byte();
 695                 if (ao_ublox_cksum.a != cksum.a || ao_ublox_cksum.b != cksum.b)
 696                         continue;
 697
 698                 switch (class) {
 699                 case 0x01:
 700                         switch (id) {
 701                         case 0x21:
 702                                 ao_mutex_get(&ao_gps_mutex);
 703                                 ao_gps_tick = ao_time();
 704
 705                                 ao_gps_data.flags = 0;
 706                                 ao_gps_data.flags |= AO_GPS_RUNNING;
 707                                 if (nav_sol.gps_fix & (1 << NAV_SOL_FLAGS_GPSFIXOK)) {
 708                                         uint8_t nsat = nav_sol.nsat;
 709                                         ao_gps_data.flags |= AO_GPS_VALID;
 710                                         if (nsat > 15)
 711                                                 nsat = 15;
 712                                         ao_gps_data.flags |= nsat;
 713                                 }
 714                                 if (nav_timeutc.valid & (1 << NAV_TIMEUTC_VALID_UTC))
 715                                         ao_gps_data.flags |= AO_GPS_DATE_VALID;
 716
 717                                 ao_gps_data.altitude = nav_posllh.alt_msl / 1000;
 718                                 ao_gps_data.latitude = nav_posllh.lat;
 719                                 ao_gps_data.longitude = nav_posllh.lon;
 720
 721                                 ao_gps_data.year = nav_timeutc.year - 2000;
 722                                 ao_gps_data.month = nav_timeutc.month;
 723                                 ao_gps_data.day = nav_timeutc.day;
 724
 725                                 ao_gps_data.hour = nav_timeutc.hour;
 726                                 ao_gps_data.minute = nav_timeutc.min;
 727                                 ao_gps_data.second = nav_timeutc.sec;
 728
 729                                 ao_gps_data.pdop = nav_dop.pdop;
 730                                 ao_gps_data.hdop = nav_dop.hdop;
 731                                 ao_gps_data.vdop = nav_dop.vdop;
 732
 733                                 /* mode is not set */
 734
 735                                 ao_gps_data.ground_speed = nav_velned.g_speed;
 736                                 ao_gps_data.climb_rate = -nav_velned.vel_d;
 737                                 ao_gps_data.course = nav_velned.heading / 200000;
 738
 739                                 ao_gps_tracking_data.channels = 0;
 740
 741                                 struct ao_telemetry_satellite_info *dst = &ao_gps_tracking_data.sats[0];
 742
 743                                 for (i = 0; i < nav_svinfo_nsat; i++) {
 744                                         struct nav_svinfo_sat *src = &nav_svinfo_sat[i];
 745
 746                                         if (!(src->flags & (1 << NAV_SVINFO_SAT_FLAGS_UNHEALTHY)) &&
 747                                             src->quality >= NAV_SVINFO_SAT_QUALITY_ACQUIRED)
 748                                         {
 749                                                 dst->svid = src->svid;
 750                                                 dst->c_n_1 = src->cno;
 751                                                 dst++;
 752                                                 ao_gps_tracking_data.channels++;
 753                                         }
 754                                 }
 755
 756                                 ao_mutex_put(&ao_gps_mutex);
 757                                 ao_wakeup(&ao_gps_data);
 758                                 ao_wakeup(&ao_gps_tracking_data);
 759                                 break;
 760                         }
 761                         break;
 762                 }
 763         }
 764 }
 765
 766 #if AO_UBLOX_DEBUG
 767 static void ao_gps_option(void)
 768 {
 769         ao_cmd_hex();
 770         if (ao_cmd_status != ao_cmd_success) {
 771                 ao_cmd_status = ao_cmd_success;
 772                 ao_gps_show();
 773         } else {
 774                 ao_gps_dbg_enable = ao_cmd_lex_i;
 775                 printf ("gps debug set to %d\n", ao_gps_dbg_enable);
 776         }
 777 }
 778 #else
 779 #define ao_gps_option ao_gps_show
 780 #endif
 781
 782 __code struct ao_cmds ao_gps_cmds[] = {
 783         { ao_gps_option,        "g\0Display GPS" },
 784         { 0, NULL },
 785 };
 786
 787 __xdata struct ao_task ao_gps_task;
 788
 789 void
 790 ao_gps_init(void)
 791 {
 792         ao_cmd_register(&ao_gps_cmds[0]);
 793         ao_add_task(&ao_gps_task, ao_gps, "gps");
 794 }