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