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