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