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