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