git.gag.com Git - fw/altos/blob - src/drivers/ao_gps_ublox.c

   1 /*
   2  * Copyright © 2013 Keith Packard <keithp@keithp.com>
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation; either version 2 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * This program is distributed in the hope that it will be useful, but
  10  * WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  12  * General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License along
  15  * with this program; if not, write to the Free Software Foundation, Inc.,
  16  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  17  */
  18
  19 #ifndef AO_GPS_TEST
  20 #include "ao.h"
  21 #endif
  22
  23 #include "ao_gps_ublox.h"
  24
  25 #define AO_UBLOX_DEBUG 0
  26
  27 #ifndef AO_UBLOX_VERSION
  28 #define AO_UBLOX_VERSION        8
  29 #endif
  30
  31 #include <stdarg.h>
  32
  33 uint8_t ao_gps_new;
  34 uint8_t ao_gps_mutex;
  35 AO_TICK_TYPE ao_gps_tick;
  36 AO_TICK_TYPE ao_gps_utc_tick;
  37 struct ao_telemetry_location    ao_gps_data;
  38 struct ao_telemetry_satellite   ao_gps_tracking_data;
  39
  40 #undef AO_SERIAL_SPEED_UBLOX
  41
  42 #ifndef AO_SERIAL_SPEED_UBLOX
  43 #define AO_SERIAL_SPEED_UBLOX AO_SERIAL_SPEED_9600
  44 #endif
  45
  46 #if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_57600
  47 #define SERIAL_SPEED_STRING     "57600"
  48 #define SERIAL_SPEED_CHECKSUM   "2d"
  49 #endif
  50 #if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_19200
  51 #define SERIAL_SPEED_STRING     "19200"
  52 #define SERIAL_SPEED_CHECKSUM   "23"
  53 #endif
  54 #if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_9600
  55 #define SERIAL_SPEED_STRING     "9600"
  56 #define SERIAL_SPEED_CHECKSUM   "16"
  57 #endif
  58
  59 static const char ao_gps_set_nmea[] =
  60         "\r\n$PUBX,41,1,3,1," SERIAL_SPEED_STRING ",0*" SERIAL_SPEED_CHECKSUM "\r\n";
  61
  62 struct ao_ublox_cksum {
  63         uint8_t a, b;
  64 };
  65
  66 static struct ao_ublox_cksum ao_ublox_cksum;
  67 static uint16_t ao_ublox_len;
  68
  69 #if AO_UBLOX_DEBUG
  70
  71
  72 #define DBG_PROTO       1
  73 #define DBG_CHAR        2
  74 #define DBG_INIT        4
  75
  76 static uint8_t ao_gps_dbg_enable = 0;
  77
  78 static void ao_gps_dbg(int level, char *format, ...) {
  79         va_list a;
  80
  81         if (level & ao_gps_dbg_enable) {
  82                 va_start(a, format);
  83                 vprintf(format, a);
  84                 va_end(a);
  85                 flush();
  86         }
  87 }
  88
  89 #else
  90 #define ao_gps_dbg(fmt, ...)
  91 #endif
  92
  93 static inline uint8_t ao_ublox_byte(void) {
  94         uint8_t c = (uint8_t) ao_gps_getchar();
  95         ao_gps_dbg(DBG_CHAR, " %02x", c);
  96         return c;
  97 }
  98
  99 static inline void add_cksum(struct ao_ublox_cksum *cksum, uint8_t c)
 100 {
 101         cksum->a += c;
 102         cksum->b += cksum->a;
 103 }
 104
 105 static void ao_ublox_init_cksum(void)
 106 {
 107         ao_ublox_cksum.a = ao_ublox_cksum.b = 0;
 108 }
 109
 110 static void ao_ublox_put_u8(uint8_t c)
 111 {
 112         add_cksum(&ao_ublox_cksum, c);
 113         ao_gps_dbg(DBG_CHAR, " (%02x)", c);
 114         ao_gps_putchar(c);
 115 }
 116
 117 static void ao_ublox_put_i8(int8_t c)
 118 {
 119         ao_ublox_put_u8((uint8_t) c);
 120 }
 121
 122 static void ao_ublox_put_u16(uint16_t c)
 123 {
 124         ao_ublox_put_u8((uint8_t) c);
 125         ao_ublox_put_u8((uint8_t) (c>>8));
 126 }
 127
 128 #if 0
 129 static void ao_ublox_put_i16(int16_t c)
 130 {
 131         ao_ublox_put_u16((uint16_t) c);
 132 }
 133 #endif
 134
 135 static void ao_ublox_put_u32(uint32_t c)
 136 {
 137         ao_ublox_put_u8((uint8_t) c);
 138         ao_ublox_put_u8((uint8_t) (c>>8));
 139         ao_ublox_put_u8((uint8_t) (c>>16));
 140         ao_ublox_put_u8((uint8_t) (c>>24));
 141 }
 142
 143 static void ao_ublox_put_i32(int32_t c)
 144 {
 145         ao_ublox_put_u32((uint32_t) c);
 146 }
 147
 148 static uint8_t header_byte(void)
 149 {
 150         uint8_t c = ao_ublox_byte();
 151         add_cksum(&ao_ublox_cksum, c);
 152         return c;
 153 }
 154
 155 static uint8_t data_byte(void)
 156 {
 157         --ao_ublox_len;
 158         return header_byte();
 159 }
 160
 161 static char *ublox_target;
 162
 163 static void ublox_u16(uint8_t offset)
 164 {
 165         uint16_t *ptr = (uint16_t *) (void *) (ublox_target + offset);
 166         uint16_t val;
 167
 168         val = data_byte();
 169         val |= (uint16_t) ((uint16_t) data_byte () << 8);
 170         *ptr = val;
 171 }
 172
 173 static void ublox_u8(uint8_t offset)
 174 {
 175         uint8_t *ptr = (uint8_t *) (ublox_target + offset);
 176         uint8_t val;
 177
 178         val = data_byte ();
 179         *ptr = val;
 180 }
 181
 182 static void ublox_u32(uint8_t offset)
 183 {
 184         uint32_t *ptr = (uint32_t *) (void *) (ublox_target + offset);
 185         uint32_t val;
 186
 187         val = ((uint32_t) data_byte ());
 188         val |= ((uint32_t) data_byte ()) << 8;
 189         val |= ((uint32_t) data_byte ()) << 16;
 190         val |= ((uint32_t) data_byte ()) << 24;
 191         *ptr = val;
 192 }
 193
 194 static void ublox_discard(uint8_t len)
 195 {
 196         while (len--)
 197                 data_byte();
 198 }
 199
 200 #define UBLOX_END       0
 201 #define UBLOX_DISCARD   1
 202 #define UBLOX_U8        2
 203 #define UBLOX_U16       3
 204 #define UBLOX_U32       4
 205
 206 struct ublox_packet_parse {
 207         uint8_t type;
 208         uint8_t offset;
 209 };
 210
 211 static void
 212 ao_ublox_parse(void *target, const struct ublox_packet_parse *parse)
 213 {
 214         uint8_t i, offset;
 215
 216         ublox_target = target;
 217         for (i = 0; ; i++) {
 218                 offset = parse[i].offset;
 219                 switch (parse[i].type) {
 220                 case UBLOX_END:
 221                         return;
 222                 case UBLOX_DISCARD:
 223                         ublox_discard(offset);
 224                         break;
 225                 case UBLOX_U8:
 226                         ublox_u8(offset);
 227                         break;
 228                 case UBLOX_U16:
 229                         ublox_u16(offset);
 230                         break;
 231                 case UBLOX_U32:
 232                         ublox_u32(offset);
 233                         break;
 234                 }
 235         }
 236         ao_gps_dbg(DBG_PROTO, "\n");
 237 }
 238
 239 /*
 240  * NAV-DOP message parsing
 241  */
 242
 243 static struct nav_dop {
 244         uint16_t        pdop;
 245         uint16_t        hdop;
 246         uint16_t        vdop;
 247 } nav_dop;
 248
 249 static const struct ublox_packet_parse nav_dop_packet[] = {
 250         { UBLOX_DISCARD, 6 },                                   /* 0 GPS Millisecond Time of Week, gDOP */
 251         { UBLOX_U16, offsetof(struct nav_dop, pdop) },          /* 6 pDOP */
 252         { UBLOX_DISCARD, 2 },                                   /* 8 tDOP */
 253         { UBLOX_U16, offsetof(struct nav_dop, vdop) },          /* 10 vDOP */
 254         { UBLOX_U16, offsetof(struct nav_dop, hdop) },          /* 12 hDOP */
 255         { UBLOX_DISCARD, 4 },                                   /* 14 nDOP, eDOP */
 256         { UBLOX_END, 0 }
 257 };
 258
 259 static void
 260 ao_ublox_parse_nav_dop(void)
 261 {
 262         ao_ublox_parse(&nav_dop, nav_dop_packet);
 263 }
 264
 265 static const struct ublox_packet_parse ack_ack_packet[] = {
 266         { UBLOX_DISCARD, 2 },                                   /* 0 class ID, msg ID */
 267         { UBLOX_END, 0 },
 268 };
 269
 270 static void
 271 ao_ublox_parse_ack_ack(void)
 272 {
 273         ao_ublox_parse(NULL, ack_ack_packet);
 274 }
 275
 276 static struct ack_nak {
 277         uint8_t class_id;
 278         uint8_t msg_id;
 279 } ack_nak;
 280
 281 static const struct ublox_packet_parse ack_nak_packet[] = {
 282         { UBLOX_U8, offsetof(struct ack_nak, class_id) },       /* 0 class ID */
 283         { UBLOX_U8, offsetof(struct ack_nak, msg_id) },         /* 1 msg ID */
 284         { UBLOX_END, 0 },
 285 };
 286
 287 static void
 288 ao_ublox_parse_ack_nak(void)
 289 {
 290         ao_ublox_parse(&ack_nak, ack_nak_packet);
 291 #if AO_UBLOX_DEBUG
 292         ao_gps_dbg(DBG_PROTO, "NAK class 0x%02x msg 0x%02x\n",
 293                    ack_nak.class_id, ack_nak.msg_id);
 294 #endif
 295 }
 296
 297 #if AO_UBLOX_VERSION < 10
 298 /*
 299  * NAV-POSLLH message parsing
 300  */
 301 static struct nav_posllh {
 302         int32_t         lat;
 303         int32_t         lon;
 304         int32_t         alt_msl;
 305 } nav_posllh;
 306
 307 static const struct ublox_packet_parse nav_posllh_packet[] = {
 308         { UBLOX_DISCARD, 4 },                                           /* 0 GPS Millisecond Time of Week */
 309         { UBLOX_U32, offsetof(struct nav_posllh, lon) },                /* 4 Longitude */
 310         { UBLOX_U32, offsetof(struct nav_posllh, lat) },                /* 8 Latitude */
 311         { UBLOX_DISCARD, 4 },                                           /* 12 Height above Ellipsoid */
 312         { UBLOX_U32, offsetof(struct nav_posllh, alt_msl) },            /* 16 Height above mean sea level */
 313         { UBLOX_DISCARD, 8 },                                           /* 20 hAcc, vAcc */
 314         { UBLOX_END, 0 },                                               /* 28 */
 315 };
 316
 317 static void
 318 ao_ublox_parse_nav_posllh(void)
 319 {
 320         ao_ublox_parse(&nav_posllh, nav_posllh_packet);
 321 }
 322
 323 /*
 324  * NAV-SOL message parsing
 325  */
 326 static struct nav_sol {
 327         uint8_t         gps_fix;
 328         uint8_t         flags;
 329         uint8_t         nsat;
 330 } nav_sol;
 331
 332 static const struct ublox_packet_parse nav_sol_packet[] = {
 333         { UBLOX_DISCARD, 10 },                                          /* 0 iTOW, fTOW, week */
 334         { UBLOX_U8, offsetof(struct nav_sol, gps_fix) },                /* 10 gpsFix */
 335         { UBLOX_U8, offsetof(struct nav_sol, flags) },                  /* 11 flags */
 336         { UBLOX_DISCARD, 35 },                                          /* 12 ecefX, ecefY, ecefZ, pAcc, ecefVX, ecefVY, ecefVZ, sAcc, pDOP, reserved1 */
 337         { UBLOX_U8, offsetof(struct nav_sol, nsat) },                   /* 47 numSV */
 338         { UBLOX_DISCARD, 4 },                                           /* 48 reserved2 */
 339         { UBLOX_END, 0 }
 340 };
 341
 342 #define NAV_SOL_FLAGS_GPSFIXOK          0
 343 #define NAV_SOL_FLAGS_DIFFSOLN          1
 344 #define NAV_SOL_FLAGS_WKNSET            2
 345 #define NAV_SOL_FLAGS_TOWSET            3
 346
 347 static void
 348 ao_ublox_parse_nav_sol(void)
 349 {
 350         ao_ublox_parse(&nav_sol, nav_sol_packet);
 351 }
 352
 353 /*
 354  * NAV-SVINFO message parsing
 355  */
 356
 357 static struct nav_svinfo {
 358         uint8_t num_ch;
 359         uint8_t flags;
 360 } nav_svinfo;
 361
 362 static const struct ublox_packet_parse nav_svinfo_packet[] = {
 363         { UBLOX_DISCARD, 4 },                                           /* 0 iTOW */
 364         { UBLOX_U8, offsetof(struct nav_svinfo, num_ch) },              /* 4 numCh */
 365         { UBLOX_U8, offsetof(struct nav_svinfo, flags) },               /* 5 globalFlags */
 366         { UBLOX_DISCARD, 2 },                                           /* 6 reserved2 */
 367         { UBLOX_END, 0 }
 368 };
 369
 370 #define NAV_SVINFO_MAX_SAT      16
 371
 372 static struct nav_svinfo_sat {
 373         uint8_t chn;
 374         uint8_t svid;
 375         uint8_t flags;
 376         uint8_t quality;
 377         uint8_t cno;
 378 } nav_svinfo_sat[NAV_SVINFO_MAX_SAT];
 379
 380 static uint8_t  nav_svinfo_nsat;
 381
 382 static const struct ublox_packet_parse nav_svinfo_sat_packet[] = {
 383         { UBLOX_U8, offsetof(struct nav_svinfo_sat, chn) },             /* 8 + 12*N chn */
 384         { UBLOX_U8, offsetof(struct nav_svinfo_sat, svid) },            /* 9 + 12*N svid */
 385         { UBLOX_U8, offsetof(struct nav_svinfo_sat, flags) },           /* 10 + 12*N flags */
 386         { UBLOX_U8, offsetof(struct nav_svinfo_sat, quality) },         /* 11 + 12*N quality */
 387         { UBLOX_U8, offsetof(struct nav_svinfo_sat, cno) },             /* 12 + 12*N cno */
 388         { UBLOX_DISCARD, 7 },                                           /* 13 + 12*N elev, azim, prRes */
 389         { UBLOX_END, 0 }
 390 };
 391
 392 #define NAV_SVINFO_SAT_FLAGS_SVUSED             0
 393 #define NAV_SVINFO_SAT_FLAGS_DIFFCORR           1
 394 #define NAV_SVINFO_SAT_FLAGS_ORBITAVAIL         2
 395 #define NAV_SVINFO_SAT_FLAGS_ORBITEPH           3
 396 #define NAV_SVINFO_SAT_FLAGS_UNHEALTHY          4
 397 #define NAV_SVINFO_SAT_FLAGS_ORBITALM           5
 398 #define NAV_SVINFO_SAT_FLAGS_ORBITAOP           6
 399 #define NAV_SVINFO_SAT_FLAGS_SMOOTHED           7
 400
 401 #define NAV_SVINFO_SAT_QUALITY_IDLE             0
 402 #define NAV_SVINFO_SAT_QUALITY_SEARCHING        1
 403 #define NAV_SVINFO_SAT_QUALITY_ACQUIRED         2
 404 #define NAV_SVINFO_SAT_QUALITY_UNUSABLE         3
 405 #define NAV_SVINFO_SAT_QUALITY_LOCKED           4
 406 #define NAV_SVINFO_SAT_QUALITY_RUNNING          5
 407
 408 static void
 409 ao_ublox_parse_nav_svinfo(void)
 410 {
 411         uint8_t nsat;
 412         nav_svinfo_nsat = 0;
 413
 414         ao_ublox_parse(&nav_svinfo, nav_svinfo_packet);
 415         for (nsat = 0; nsat < nav_svinfo.num_ch && ao_ublox_len >= 12; nsat++) {
 416                 if (nsat < NAV_SVINFO_MAX_SAT) {
 417                         ao_ublox_parse(&nav_svinfo_sat[nav_svinfo_nsat++], nav_svinfo_sat_packet);
 418                 } else {
 419                         ublox_discard(12);
 420                 }
 421         }
 422 #if AO_UBLOX_DEBUG
 423         ao_gps_dbg(DBG_PROTO, "svinfo num_ch %d flags %02x\n", nav_svinfo.num_ch, nav_svinfo.flags);
 424         for (nsat = 0; nsat < nav_svinfo.num_ch; nsat++)
 425                 ao_gps_dbg(DBG_PROTO, "\t%d: chn %d svid %d flags %02x quality %d cno %d\n",
 426                             nsat,
 427                             nav_svinfo_sat[nsat].chn,
 428                             nav_svinfo_sat[nsat].svid,
 429                             nav_svinfo_sat[nsat].flags,
 430                             nav_svinfo_sat[nsat].quality,
 431                             nav_svinfo_sat[nsat].cno);
 432 #endif
 433 }
 434
 435 /*
 436  * NAV-TIMEUTC message parsing
 437  */
 438 static struct nav_timeutc {
 439         int32_t         nano;
 440         uint16_t        year;
 441         uint8_t         month;
 442         uint8_t         day;
 443         uint8_t         hour;
 444         uint8_t         min;
 445         uint8_t         sec;
 446         uint8_t         valid;
 447 } nav_timeutc;
 448
 449 #define NAV_TIMEUTC_VALID_TOW   0
 450 #define NAV_TIMEUTC_VALID_WKN   1
 451 #define NAV_TIMEUTC_VALID_UTC   2
 452
 453 static const struct ublox_packet_parse nav_timeutc_packet[] = {
 454         { UBLOX_DISCARD, 8 },                                           /* 0 iTOW, tAcc */
 455         { UBLOX_U32, offsetof(struct nav_timeutc, nano) },              /* 8 nano */
 456         { UBLOX_U16, offsetof(struct nav_timeutc, year) },              /* 12 year */
 457         { UBLOX_U8, offsetof(struct nav_timeutc, month) },              /* 14 month */
 458         { UBLOX_U8, offsetof(struct nav_timeutc, day) },                /* 15 day */
 459         { UBLOX_U8, offsetof(struct nav_timeutc, hour) },               /* 16 hour */
 460         { UBLOX_U8, offsetof(struct nav_timeutc, min) },                /* 17 min */
 461         { UBLOX_U8, offsetof(struct nav_timeutc, sec) },                /* 18 sec */
 462         { UBLOX_U8, offsetof(struct nav_timeutc, valid) },              /* 19 valid */
 463         { UBLOX_END, 0 }
 464 };
 465
 466 static void
 467 ao_ublox_parse_nav_timeutc(void)
 468 {
 469         ao_ublox_parse(&nav_timeutc, nav_timeutc_packet);
 470 }
 471
 472 /*
 473  * NAV-VELNED message parsing
 474  */
 475
 476 static struct nav_velned {
 477         int32_t         vel_d;
 478         uint32_t        g_speed;
 479         int32_t         heading;
 480 } nav_velned;
 481
 482 static const struct ublox_packet_parse nav_velned_packet[] = {
 483         { UBLOX_DISCARD, 12 },                                          /* 0 iTOW, velN, velE */
 484         { UBLOX_U32, offsetof(struct nav_velned, vel_d) },              /* 12 velD */
 485         { UBLOX_DISCARD, 4 },                                           /* 16 speed */
 486         { UBLOX_U32, offsetof(struct nav_velned, g_speed) },            /* 20 gSpeed */
 487         { UBLOX_U32, offsetof(struct nav_velned, heading) },            /* 24 heading */
 488         { UBLOX_DISCARD, 8 },                                           /* 28 sAcc, cAcc */
 489         { UBLOX_END, 0 }
 490 };
 491
 492 static void
 493 ao_ublox_parse_nav_velned(void)
 494 {
 495         ao_ublox_parse(&nav_velned, nav_velned_packet);
 496 }
 497
 498 #else   /* AO_UBLOX_VERSION < 10 */
 499
 500 /*
 501  * NAV-PVT message parsing
 502  */
 503
 504 static struct nav_pvt {
 505         uint16_t        year;
 506         uint8_t         month;
 507         uint8_t         day;
 508         uint8_t         hour;
 509         uint8_t         min;
 510         uint8_t         sec;
 511         uint8_t         valid;
 512         int32_t         nano;
 513         uint8_t         flags;
 514         uint8_t         num_sv;
 515         int32_t         lat;
 516         int32_t         lon;
 517         int32_t         alt_msl;
 518         int32_t         vel_d;
 519         int32_t         g_speed;
 520         int32_t         heading;
 521 } nav_pvt;
 522
 523 static const struct ublox_packet_parse nav_pvt_packet[] = {
 524         { UBLOX_DISCARD, 4 },                                           /* 0 iTOW */
 525         { UBLOX_U16, offsetof(struct nav_pvt, year) },                  /* 4 year */
 526         { UBLOX_U8, offsetof(struct nav_pvt, month) },                  /* 6 month */
 527         { UBLOX_U8, offsetof(struct nav_pvt, day) },                    /* 7 day */
 528         { UBLOX_U8, offsetof(struct nav_pvt, hour) },                   /* 8 hour */
 529         { UBLOX_U8, offsetof(struct nav_pvt, min) },                    /* 9 min */
 530         { UBLOX_U8, offsetof(struct nav_pvt, sec) },                    /* 10 sec */
 531         { UBLOX_U8, offsetof(struct nav_pvt, valid) },                  /* 11 valid */
 532         { UBLOX_DISCARD, 4 },                                           /* 12 tAcc */
 533         { UBLOX_U32, offsetof(struct nav_pvt, nano) },                  /* 16 nano */
 534         { UBLOX_DISCARD, 1 },                                           /* 20 fixType */
 535         { UBLOX_U8, offsetof(struct nav_pvt, flags) },                  /* 21 gpsFix */
 536         { UBLOX_DISCARD, 1 },                                           /* 22 flags2 */
 537         { UBLOX_U8, offsetof(struct nav_pvt, num_sv) },                 /* 23 numSV */
 538         { UBLOX_U32, offsetof(struct nav_pvt, lon) },                   /* 24 Longitude */
 539         { UBLOX_U32, offsetof(struct nav_pvt, lat) },                   /* 28 Latitude */
 540         { UBLOX_DISCARD, 4 },                                           /* 32 height above ellipsoid */
 541         { UBLOX_U32, offsetof(struct nav_pvt, alt_msl) },               /* 36 Height above mean sea level */
 542         { UBLOX_DISCARD, 16 },                                          /* 40 hAcc, vAcc, velN, velE */
 543         { UBLOX_U32, offsetof(struct nav_pvt, vel_d) },                 /* 56 velD */
 544         { UBLOX_U32, offsetof(struct nav_pvt, g_speed) },               /* 60 gSpeed */
 545         { UBLOX_U32, offsetof(struct nav_pvt, heading) },               /* 64 headMot */
 546         { UBLOX_DISCARD, 92 - 68 },                                     /* 68 sAcc .. magAcc  */
 547         { UBLOX_END, 0 }
 548 };
 549
 550 #define NAV_PVT_VALID_DATE              0
 551 #define NAV_PVT_VALID_TIME              1
 552 #define NAV_PVT_VALID_FULLY_RESOLVED    2
 553 #define NAV_PVT_VALID_MAG               3
 554
 555 #define NAV_PVT_FLAGS_GNSSFIXOK         0
 556 #define NAV_PVT_FLAGS_DIFFSOLN          1
 557 #define NAV_PVT_FLAGS_PSM_STATE         2
 558 #define NAV_PVT_FLAGS_HEAD_VEH_VALID    5
 559 #define NAV_PVT_FLAGS_CARR_SOLN         6
 560
 561 static void
 562 ao_ublox_parse_nav_pvt(void)
 563 {
 564         ao_ublox_parse(&nav_pvt, nav_pvt_packet);
 565 #if AO_UBLOX_DEBUG
 566         ao_gps_dbg(DBG_PROTO, "\t%d-%d-%d %02d:%02d:%02d %ld (%02x)\n",
 567                    nav_pvt.year, nav_pvt.month, nav_pvt.day,
 568                    nav_pvt.hour, nav_pvt.min, nav_pvt.sec,
 569                    (long) nav_pvt.nano, nav_pvt.valid);
 570         ao_gps_dbg(DBG_PROTO, "\tflags %02x numSV %d lon %ld lat %ld alt %ld\n",
 571                    nav_pvt.flags, nav_pvt.num_sv,
 572                    (long) nav_pvt.lon, (long) nav_pvt.lat, (long) nav_pvt.alt_msl);
 573 #endif
 574 }
 575
 576 /*
 577  * NAV-SAT message parsing
 578  */
 579
 580 static struct nav_sat {
 581         uint8_t num_svs;
 582 } nav_sat;
 583
 584 static const struct ublox_packet_parse nav_sat_packet[] = {
 585         { UBLOX_DISCARD, 4 },                                           /* 0 iTOW */
 586         { UBLOX_DISCARD, 1 },                                           /* 4 version */
 587         { UBLOX_U8, offsetof(struct nav_sat, num_svs) },                        /* 4 numSvs */
 588         { UBLOX_DISCARD, 2 },                                           /* 6 reserved0 */
 589         { UBLOX_END, 0 }
 590 };
 591
 592 #define NAV_SAT_MAX_SAT         AO_TELEMETRY_SATELLITE_MAX_SAT
 593
 594 static struct nav_sat_sat {
 595         uint8_t svid;
 596         uint8_t cno;
 597 } nav_sat_sat[NAV_SAT_MAX_SAT];
 598
 599 static uint8_t  nav_sat_nsat;
 600
 601 static struct nav_sat_real_sat {
 602         uint8_t svid;
 603         uint8_t cno;
 604         uint32_t flags;
 605 } nav_sat_real_sat;
 606
 607 static const struct ublox_packet_parse nav_sat_sat_packet[] = {
 608         { UBLOX_DISCARD, 1 },                                           /* 8 + 12*N gnssid */
 609         { UBLOX_U8, offsetof(struct nav_sat_real_sat, svid) },          /* 9 + 12*N svid */
 610         { UBLOX_U8, offsetof(struct nav_sat_real_sat, cno) },           /* 10 + 12*N cno */
 611         { UBLOX_DISCARD, 5 },                                           /* 11 + 12*N elev, azim, prRes */
 612         { UBLOX_U32, offsetof(struct nav_sat_real_sat, flags) },        /* 16 + 12*N flags */
 613         { UBLOX_END, 0 }
 614 };
 615
 616 static uint32_t
 617 ao_ublox_sat_quality(struct nav_sat_real_sat *sat)
 618 {
 619         return (sat->flags >> UBLOX_NAV_SAT_FLAGS_QUALITY) & UBLOX_NAV_SAT_FLAGS_QUALITY_MASK;
 620 }
 621
 622 static uint32_t
 623 ao_ublox_sat_health(struct nav_sat_real_sat *sat)
 624 {
 625         return (sat->flags >> UBLOX_NAV_SAT_FLAGS_SV_HEALTH) & UBLOX_NAV_SAT_FLAGS_SV_HEALTH_MASK;
 626 }
 627
 628 static void
 629 ao_ublox_parse_nav_sat(void)
 630 {
 631         uint8_t nsat;
 632         nav_sat_nsat = 0;
 633
 634         ao_ublox_parse(&nav_sat, nav_sat_packet);
 635         for (nsat = 0; nsat < nav_sat.num_svs && ao_ublox_len >= 12; nsat++) {
 636                 ao_ublox_parse(&nav_sat_real_sat, nav_sat_sat_packet);
 637                 if (nav_sat_nsat < NAV_SAT_MAX_SAT &&
 638                     ao_ublox_sat_health(&nav_sat_real_sat) == UBLOX_NAV_SAT_FLAGS_SV_HEALTH_HEALTHY &&
 639                     ao_ublox_sat_quality(&nav_sat_real_sat) >= UBLOX_NAV_SAT_FLAGS_QUALITY_ACQUIRED)
 640                 {
 641                         nav_sat_sat[nav_sat_nsat].svid = nav_sat_real_sat.svid;
 642                         nav_sat_sat[nav_sat_nsat].cno = nav_sat_real_sat.cno;
 643                         nav_sat_nsat++;
 644                 }
 645         }
 646 #if AO_UBLOX_DEBUG
 647         ao_gps_dbg(DBG_PROTO, "sat num_svs %d\n", nav_sat.num_svs);
 648         for (nsat = 0; nsat < nav_sat.num_svs; nsat++) {
 649                 if (nsat < NAV_SAT_MAX_SAT) {
 650                 ao_gps_dbg(DBG_PROTO, "\t%d: svid %d cno %d\n",
 651                            nsat,
 652                            nav_sat_sat[nsat].svid,
 653                            nav_sat_sat[nsat].cno);
 654                 } else {
 655                         ao_gps_dbg(DBG_PROTO, "\t%d: skipped\n", nsat);
 656                 }
 657         }
 658 #endif
 659 }
 660
 661 #endif  /* else AO_UBLOX_VERSION < 10 */
 662
 663 /*
 664  * Set the protocol mode and baud rate
 665  */
 666
 667 static void
 668 ao_gps_delay(void)
 669 {
 670         uint8_t i;
 671
 672         /*
 673          * A bunch of nulls so the start bit
 674          * is clear
 675          */
 676
 677         for (i = 0; i < 64; i++)
 678                 ao_gps_putchar(0x00);
 679 }
 680
 681 static void
 682 ao_gps_setup(void)
 683 {
 684         uint8_t i, k;
 685
 686         ao_delay(AO_SEC_TO_TICKS(3));
 687
 688         ao_gps_dbg(DBG_INIT, "Set speed 9600\n");
 689         ao_gps_set_speed(AO_SERIAL_SPEED_9600);
 690
 691         /*
 692          * Send the baud-rate setting and protocol-setting
 693          * command three times
 694          */
 695         for (k = 0; k < 3; k++) {
 696                 ao_gps_delay();
 697
 698                 ao_gps_dbg(DBG_INIT, "Send initial setting\n");
 699                 for (i = 0; i < sizeof (ao_gps_set_nmea); i++)
 700                         ao_gps_putchar(ao_gps_set_nmea[i]);
 701         }
 702
 703         ao_gps_delay();
 704
 705 #if AO_SERIAL_SPEED_UBLOX != AO_SERIAL_SPEED_9600
 706         ao_gps_dbg(DBG_INIT, "Set speed high\n");
 707         /*
 708          * Increase the baud rate
 709          */
 710         ao_gps_set_speed(AO_SERIAL_SPEED_UBLOX);
 711 #endif
 712
 713         ao_gps_delay();
 714 }
 715
 716 static void
 717 ao_ublox_putstart(uint8_t class, uint8_t id, uint16_t len)
 718 {
 719         ao_ublox_init_cksum();
 720         ao_gps_putchar(0xb5);
 721         ao_gps_putchar(0x62);
 722         ao_ublox_put_u8(class);
 723         ao_ublox_put_u8(id);
 724         ao_ublox_put_u8((uint8_t) len);
 725         ao_ublox_put_u8((uint8_t) (len >> 8));
 726 }
 727
 728 static void
 729 ao_ublox_putend(void)
 730 {
 731         ao_gps_putchar(ao_ublox_cksum.a);
 732         ao_gps_putchar(ao_ublox_cksum.b);
 733 }
 734
 735 static void
 736 ao_ublox_set_message_rate(uint8_t class, uint8_t msgid, uint8_t rate)
 737 {
 738         ao_ublox_putstart(UBLOX_CFG, UBLOX_CFG_MSG, 3);
 739         ao_ublox_put_u8(class);
 740         ao_ublox_put_u8(msgid);
 741         ao_ublox_put_u8(rate);
 742         ao_ublox_putend();
 743 }
 744
 745 static void
 746 ao_ublox_set_navigation_settings(uint16_t mask,
 747                                  uint8_t dyn_model,
 748                                  uint8_t fix_mode,
 749                                  int32_t fixed_alt,
 750                                  uint32_t fixed_alt_var,
 751                                  int8_t min_elev,
 752                                  uint8_t dr_limit,
 753                                  uint16_t pdop,
 754                                  uint16_t tdop,
 755                                  uint16_t pacc,
 756                                  uint16_t tacc,
 757                                  uint8_t static_hold_thresh,
 758                                  uint8_t dgps_time_out)
 759 {
 760         ao_ublox_putstart(UBLOX_CFG, UBLOX_CFG_NAV5, 36);
 761         ao_ublox_put_u16(mask);
 762         ao_ublox_put_u8(dyn_model);
 763         ao_ublox_put_u8(fix_mode);
 764         ao_ublox_put_i32(fixed_alt);
 765         ao_ublox_put_u32(fixed_alt_var);
 766         ao_ublox_put_i8(min_elev);
 767         ao_ublox_put_u8(dr_limit);
 768         ao_ublox_put_u16(pdop);
 769         ao_ublox_put_u16(tdop);
 770         ao_ublox_put_u16(pacc);
 771         ao_ublox_put_u16(tacc);
 772         ao_ublox_put_u8(static_hold_thresh);
 773         ao_ublox_put_u8(dgps_time_out);
 774         ao_ublox_put_u32(0);
 775         ao_ublox_put_u32(0);
 776         ao_ublox_put_u32(0);
 777         ao_ublox_putend();
 778 }
 779
 780 /*
 781  * Disable all MON message
 782  */
 783 static const uint8_t ublox_disable_mon[] = {
 784         0x0b, 0x09, 0x02, 0x06, 0x07, 0x21, 0x08, 0x04
 785 };
 786
 787 /*
 788  * Disable all NAV messages. The desired
 789  * ones will be explicitly re-enabled
 790  */
 791
 792 static const uint8_t ublox_disable_nav[] = {
 793         0x60, 0x22, 0x31, 0x04, 0x40, 0x01, 0x02, 0x32,
 794         0x06, 0x03, 0x30, 0x20, 0x21, 0x11, 0x12
 795 };
 796
 797 /*
 798  * Enable enough messages to get all of the data we want
 799  */
 800 static const uint8_t ublox_enable_nav[] = {
 801         UBLOX_NAV_DOP,          /* both */
 802 #if AO_UBLOX_VERSION >= 10
 803         UBLOX_NAV_PVT,          /* new */
 804         UBLOX_NAV_SAT,          /* new */
 805 #else
 806         UBLOX_NAV_POSLLH,       /* both, but redundant with PVT */
 807         UBLOX_NAV_SOL,          /* old */
 808         UBLOX_NAV_SVINFO,       /* old */
 809         UBLOX_NAV_VELNED,       /* both, but redundant with PVT */
 810         UBLOX_NAV_TIMEUTC       /* both, but redundant with PVT */
 811 #endif
 812 };
 813
 814 void
 815 ao_gps_set_rate(uint8_t rate)
 816 {
 817         uint8_t i;
 818         for (i = 0; i < sizeof (ublox_enable_nav); i++)
 819                 ao_ublox_set_message_rate(UBLOX_NAV, ublox_enable_nav[i], rate);
 820 }
 821
 822 void
 823 ao_gps(void)
 824 {
 825         uint8_t                 class, id;
 826         struct ao_ublox_cksum   cksum;
 827         uint8_t                 i;
 828         AO_TICK_TYPE            packet_start_tick;
 829         AO_TICK_TYPE            solution_tick = 0;
 830
 831         ao_gps_setup();
 832
 833         /* Disable all messages */
 834         for (i = 0; i < sizeof (ublox_disable_mon); i++)
 835                 ao_ublox_set_message_rate(0x0a, ublox_disable_mon[i], 0);
 836         for (i = 0; i < sizeof (ublox_disable_nav); i++)
 837                 ao_ublox_set_message_rate(UBLOX_NAV, ublox_disable_nav[i], 0);
 838
 839         /* Enable all of the messages we want */
 840         ao_gps_set_rate(1);
 841
 842         ao_ublox_set_navigation_settings((1 << UBLOX_CFG_NAV5_MASK_DYN) | (1 << UBLOX_CFG_NAV5_MASK_FIXMODE),
 843                                          UBLOX_CFG_NAV5_DYNMODEL_AIRBORNE_4G,
 844                                          UBLOX_CFG_NAV5_FIXMODE_3D,
 845                                          0,
 846                                          0,
 847                                          0,
 848                                          0,
 849                                          0,
 850                                          0,
 851                                          0,
 852                                          0,
 853                                          0,
 854                                          0);
 855
 856         for (;;) {
 857                 /* Locate the begining of the next record */
 858                 while (ao_ublox_byte() != (uint8_t) 0xb5)
 859                         ;
 860                 packet_start_tick = ao_tick_count;
 861
 862                 if (ao_ublox_byte() != (uint8_t) 0x62)
 863                         continue;
 864
 865                 ao_ublox_init_cksum();
 866
 867                 class = header_byte();
 868                 id = header_byte();
 869
 870                 /* Length */
 871                 ao_ublox_len = header_byte();
 872                 ao_ublox_len |= (uint16_t) ((uint16_t) header_byte() << 8);
 873
 874                 ao_gps_dbg(DBG_PROTO, "%6u class %02x id %02x len %d\n", packet_start_tick, class, id, ao_ublox_len);
 875
 876                 if (ao_ublox_len > 1023)
 877                         continue;
 878
 879                 bool gps_ready = false;
 880
 881                 switch (class) {
 882                 case UBLOX_NAV:
 883                         switch (id) {
 884                         case UBLOX_NAV_DOP:
 885                                 if (ao_ublox_len != 18)
 886                                         break;
 887                                 ao_ublox_parse_nav_dop();
 888 #if AO_UBLOX_VERSION >= 10
 889                                 gps_ready = true;
 890 #endif
 891                                 break;
 892 #if AO_UBLOX_VERSION >= 10
 893                         case UBLOX_NAV_PVT:
 894                                 if (ao_ublox_len != 92)
 895                                         break;
 896                                 ao_ublox_parse_nav_pvt();
 897                                 solution_tick = packet_start_tick;
 898                                 break;
 899                         case UBLOX_NAV_SAT:
 900                                 if (ao_ublox_len < 8)
 901                                         break;
 902                                 ao_ublox_parse_nav_sat();
 903                                 break;
 904 #else
 905                         case UBLOX_NAV_POSLLH:
 906                                 if (ao_ublox_len != 28)
 907                                         break;
 908                                 ao_ublox_parse_nav_posllh();
 909                                 break;
 910                         case UBLOX_NAV_SOL:
 911                                 if (ao_ublox_len != 52)
 912                                         break;
 913                                 ao_ublox_parse_nav_sol();
 914                                 solution_tick = packet_start_tick;
 915                                 break;
 916                         case UBLOX_NAV_SVINFO:
 917                                 if (ao_ublox_len < 8)
 918                                         break;
 919                                 ao_ublox_parse_nav_svinfo();
 920                                 break;
 921                         case UBLOX_NAV_VELNED:
 922                                 if (ao_ublox_len != 36)
 923                                         break;
 924                                 ao_ublox_parse_nav_velned();
 925                                 break;
 926                         case UBLOX_NAV_TIMEUTC:
 927                                 if (ao_ublox_len != 20)
 928                                         break;
 929                                 ao_ublox_parse_nav_timeutc();
 930                                 gps_ready = true;
 931                                 break;
 932 #endif
 933                         }
 934                         break;
 935                 case UBLOX_ACK:
 936                         switch (id) {
 937                         case UBLOX_ACK_ACK:
 938                                 if (ao_ublox_len != 2)
 939                                         break;
 940                                 ao_ublox_parse_ack_ack();
 941                                 break;
 942                         case UBLOX_ACK_NAK:
 943                                 if (ao_ublox_len != 2)
 944                                         break;
 945                                 ao_ublox_parse_ack_nak();
 946                                 break;
 947                         }
 948                 }
 949
 950                 if (ao_ublox_len != 0) {
 951                         ao_gps_dbg(DBG_PROTO, "len left %d\n", ao_ublox_len);
 952                         continue;
 953                 }
 954
 955                 /* verify checksum and end sequence */
 956                 cksum.a = ao_ublox_byte();
 957                 cksum.b = ao_ublox_byte();
 958                 if (ao_ublox_cksum.a != cksum.a || ao_ublox_cksum.b != cksum.b)
 959                         continue;
 960
 961                 if (!gps_ready)
 962                         continue;
 963
 964                 ao_mutex_get(&ao_gps_mutex);
 965                 ao_gps_data.flags = 0;
 966                 ao_gps_data.flags |= AO_GPS_RUNNING;
 967                 ao_gps_tick = solution_tick;
 968
 969                 /* we report dop scaled by 10, but ublox provides dop scaled by 100
 970                  */
 971                 ao_gps_data.pdop = (uint8_t) (nav_dop.pdop / 10);
 972                 ao_gps_data.hdop = (uint8_t) (nav_dop.hdop / 10);
 973                 ao_gps_data.vdop = (uint8_t) (nav_dop.vdop / 10);
 974
 975 #if AO_UBLOX_VERSION >= 10
 976                 ao_gps_utc_tick = packet_start_tick + (AO_TICK_TYPE) AO_NS_TO_TICKS(nav_pvt.nano);
 977                 if (nav_pvt.flags & (1 << NAV_PVT_FLAGS_GNSSFIXOK)) {
 978                         uint8_t nsat = nav_pvt.num_sv;
 979                         ao_gps_data.flags |= AO_GPS_VALID | AO_GPS_COURSE_VALID;
 980                         if (nsat > 15)
 981                                 nsat = 15;
 982                         ao_gps_data.flags |= nsat;
 983                 }
 984                 if (nav_pvt.valid & (1 << NAV_PVT_VALID_DATE))
 985                         ao_gps_data.flags |= AO_GPS_DATE_VALID;
 986                 AO_TELEMETRY_LOCATION_SET_ALTITUDE(&ao_gps_data, nav_pvt.alt_msl / 1000);
 987                 ao_gps_data.latitude = nav_pvt.lat;
 988                 ao_gps_data.longitude = nav_pvt.lon;
 989
 990                 ao_gps_data.year = (uint8_t) (nav_pvt.year - 2000);
 991                 ao_gps_data.month = nav_pvt.month;
 992                 ao_gps_data.day = nav_pvt.day;
 993
 994                 ao_gps_data.hour = nav_pvt.hour;
 995                 ao_gps_data.minute = nav_pvt.min;
 996                 ao_gps_data.second = nav_pvt.sec;
 997
 998                 /* ublox speeds are mm/s */
 999                 ao_gps_data.ground_speed = (uint16_t) (nav_pvt.g_speed / 10);
1000                 ao_gps_data.climb_rate = -(int16_t) (nav_pvt.vel_d / 10);
1001                 ao_gps_data.course = (uint8_t) (nav_pvt.heading / 200000);
1002 #else
1003                 ao_gps_utc_tick = packet_start_tick + (AO_TICK_TYPE) AO_NS_TO_TICKS(nav_timeutc.nano);
1004                 if (nav_sol.gps_fix & (1 << NAV_SOL_FLAGS_GPSFIXOK)) {
1005                         uint8_t nsat = nav_sol.nsat;
1006                         ao_gps_data.flags |= AO_GPS_VALID | AO_GPS_COURSE_VALID;
1007                         if (nsat > 15)
1008                                 nsat = 15;
1009                         ao_gps_data.flags |= nsat;
1010                 }
1011                 if (nav_timeutc.valid & (1 << NAV_TIMEUTC_VALID_UTC))
1012                         ao_gps_data.flags |= AO_GPS_DATE_VALID;
1013
1014                 AO_TELEMETRY_LOCATION_SET_ALTITUDE(&ao_gps_data, nav_posllh.alt_msl / 1000);
1015                 ao_gps_data.latitude = nav_posllh.lat;
1016                 ao_gps_data.longitude = nav_posllh.lon;
1017
1018                 ao_gps_data.year = (uint8_t) (nav_timeutc.year - 2000);
1019                 ao_gps_data.month = nav_timeutc.month;
1020                 ao_gps_data.day = nav_timeutc.day;
1021
1022                 ao_gps_data.hour = nav_timeutc.hour;
1023                 ao_gps_data.minute = nav_timeutc.min;
1024                 ao_gps_data.second = nav_timeutc.sec;
1025
1026                 /* ublox speeds are cm/s */
1027                 ao_gps_data.ground_speed = (uint16_t) nav_velned.g_speed;
1028                 ao_gps_data.climb_rate = -(int16_t) nav_velned.vel_d;
1029                 ao_gps_data.course = (uint8_t) (nav_velned.heading / 200000);
1030 #endif
1031
1032                 ao_gps_tracking_data.channels = 0;
1033
1034                 struct ao_telemetry_satellite_info *dst = &ao_gps_tracking_data.sats[0];
1035 #if AO_UBLOX_VERSION >= 10
1036                 struct nav_sat_sat *src = &nav_sat_sat[0];
1037
1038                 for (i = 0; i < nav_sat_nsat; i++) {
1039                         if (ao_gps_tracking_data.channels < AO_TELEMETRY_SATELLITE_MAX_SAT) {
1040                                 dst->svid = src->svid;
1041                                 dst->c_n_1 = src->cno;
1042                                 dst++;
1043                                 ao_gps_tracking_data.channels++;
1044                         }
1045                         src++;
1046                 }
1047 #else
1048                 struct nav_svinfo_sat *src = &nav_svinfo_sat[0];
1049
1050                 for (i = 0; i < nav_svinfo_nsat; i++) {
1051                         if (!(src->flags & (1 << NAV_SVINFO_SAT_FLAGS_UNHEALTHY)) &&
1052                             src->quality >= NAV_SVINFO_SAT_QUALITY_ACQUIRED)
1053                         {
1054                                 if (ao_gps_tracking_data.channels < AO_TELEMETRY_SATELLITE_MAX_SAT) {
1055                                         dst->svid = src->svid;
1056                                         dst->c_n_1 = src->cno;
1057                                         dst++;
1058                                         ao_gps_tracking_data.channels++;
1059                                 }
1060                         }
1061                         src++;
1062                 }
1063 #endif
1064                 ao_mutex_put(&ao_gps_mutex);
1065                 ao_gps_new = AO_GPS_NEW_DATA | AO_GPS_NEW_TRACKING;
1066                 ao_wakeup(&ao_gps_new);
1067         }
1068 }
1069
1070 #if AO_UBLOX_DEBUG
1071 static void ao_gps_option(void)
1072 {
1073         uint8_t r = (uint8_t) ao_cmd_hex();
1074         if (ao_cmd_status != ao_cmd_success) {
1075                 ao_cmd_status = ao_cmd_success;
1076                 ao_gps_show();
1077         } else {
1078                 ao_gps_dbg_enable = r;
1079                 printf ("gps debug set to %d\n", ao_gps_dbg_enable);
1080         }
1081 }
1082 #else
1083 #define ao_gps_option ao_gps_show
1084 #endif
1085
1086 const struct ao_cmds ao_gps_cmds[] = {
1087         { ao_gps_option,        "g\0Display GPS" },
1088         { 0, NULL },
1089 };
1090
1091 struct ao_task ao_gps_task;
1092
1093 void
1094 ao_gps_init(void)
1095 {
1096         ao_cmd_register(&ao_gps_cmds[0]);
1097         ao_add_task(&ao_gps_task, ao_gps, "gps");
1098 }