imuboard/gps_ubx.c

   1 #include <stdio.h>
   2 #include <stdlib.h>
   3 #include <stdint.h>
   4 #include <inttypes.h>
   5 #include <string.h>
   6
   7 #include <aversive.h>
   8 #include <aversive/endian.h>
   9 #include <aversive/wait.h>
  10 #include <callout.h>
  11 #include <uart.h>
  12
  13 #include "main.h"
  14 #include "gps.h"
  15 #include "gps_ubx.h"
  16
  17 #if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
  18 #define letoh16(x) (x)
  19 #define letoh32(x) (x)
  20 #define letoh64(x) (x)
  21 #define htole16(x) (x)
  22 #define htole32(x) (x)
  23 #define htole64(x) (x)
  24 #else
  25 #error todo
  26 #endif
  27
  28 /* note: enabling debug will make the code fail due to rx queue full */
  29 #define debug_printf(fmt, a...) do { } while (0)
  30 //#define debug_printf(fmt, ...) printf(fmt, ##__VA_ARGS__)
  31
  32 #define GPS_UART 0
  33 #define GPS_PERIOD_MS 2
  34
  35 #define START1 0xB5
  36 #define START2 0x62
  37
  38 struct gps_ubx_cksum {
  39         uint8_t ck_a;
  40         uint8_t ck_b;
  41 };
  42
  43 #define MAX_LEN 0x200
  44 struct gps_frame {
  45         uint8_t class;
  46         uint8_t id;
  47         uint16_t len;
  48         uint16_t cur_len;
  49         uint8_t data[MAX_LEN];
  50 };
  51
  52 static struct gps_frame rxframe;
  53 static struct gps_ubx_cksum rxcksum;
  54 static struct callout gps_timer;
  55
  56 /* Navigation Results: Position, Speed, Time, Acc, Heading, DOP, SVs
  57  * used */
  58 #define GPS_UBX_CLASS_NAV  0x01
  59
  60 #define GPS_UBX_ID_NAV_POSECEF   0x01
  61 #define GPS_UBX_ID_NAV_POSLLH    0x02
  62 #define GPS_UBX_ID_NAV_STATUS    0x03
  63 #define GPS_UBX_ID_NAV_DOP       0x04
  64 #define GPS_UBX_ID_NAV_SOL       0x06
  65 #define GPS_UBX_ID_NAV_VELECEF   0x11
  66 #define GPS_UBX_ID_NAV_VELNED    0x12
  67 #define GPS_UBX_ID_NAV_TIMEGPS   0x20
  68 #define GPS_UBX_ID_NAV_TIMEUTC   0x21
  69 #define GPS_UBX_ID_NAV_CLOCK     0x22
  70 #define GPS_UBX_ID_NAV_SVINFO    0x30
  71 #define GPS_UBX_ID_NAV_DGPS      0x31
  72 #define GPS_UBX_ID_NAV_SBAS      0x32
  73 #define GPS_UBX_ID_NAV_EKFSTATUS 0x40
  74 #define GPS_UBX_ID_NAV_AOPSTATUS 0x60
  75
  76 /* Receiver Manager Messages: Satellite Status, RTC Status */
  77 #define GPS_UBX_CLASS_RXM 0x02
  78
  79 #define GPS_UBX_ID_RXM_ALM    0x30
  80 #define GPS_UBX_ID_RXM_EPH    0x31
  81 #define GPS_UBX_ID_RXM_PMREQ  0x41
  82 #define GPS_UBX_ID_RXM_RAW    0x10
  83 #define GPS_UBX_ID_RXM_SFRB   0x11
  84 #define GPS_UBX_ID_RXM_SVSI   0x20
  85
  86 /* Information Messages: Printf-Style Messages, with IDs such as Error, Warning,
  87  * Notice */
  88 #define GPS_UBX_CLASS_INF 0x04
  89
  90 /* Ack/Nack Messages: as replies to CFG Input Messages */
  91 #define GPS_UBX_CLASS_ACK 0x05
  92
  93 /* Configuration Input Messages: Set Dynamic Model, Set DOP Mask, Set
  94  * Baud Rate, etc. */
  95 #define GPS_UBX_CLASS_CFG 0x06
  96
  97 #define GPS_UBX_ID_CFG_MSG  0x01
  98 #define GPS_UBX_ID_CFG_RATE 0x08
  99 #define GPS_UBX_ID_CFG_SBAS 0x16
 100
 101 /* Monitoring Messages: Comunication Status, CPU Load, Stack Usage, Task
 102  * Status */
 103 #define GPS_UBX_CLASS_MON 0x0A
 104
 105 /* AssistNow Aiding Messages: Ephemeris, Almanac, other A-GPS data
 106  * input */
 107 #define GPS_UBX_CLASS_AID 0x0B
 108
 109 /* Timing Messages: Timepulse Output, Timemark Results */
 110 #define GPS_UBX_CLASS_TIM 0x0D
 111
 112 #define GPS_UBX_CLASS_NMEA_STD 0xF0
 113 #define GPS_UBX_CLASS_NMEA_UBX 0xF1
 114
 115 #define GPS_UBX_ID_NMEA_GGA 0x00
 116 #define GPS_UBX_ID_NMEA_GLL 0x01
 117 #define GPS_UBX_ID_NMEA_GSA 0x02
 118 #define GPS_UBX_ID_NMEA_GSV 0x03
 119 #define GPS_UBX_ID_NMEA_RMC 0x04
 120 #define GPS_UBX_ID_NMEA_VTG 0x05
 121
 122 /********************* NAV class */
 123
 124 /* Position Solution in ECEF */
 125 struct gps_ubx_posecef {
 126         uint32_t iTOW; /* ms - GPS Millisecond Time of Week */
 127         int32_t ecefX; /* cm - ECEF X coordinate */
 128         int32_t ecefY; /* cm - ECEF Y coordinate */
 129         int32_t ecefZ; /* cm - ECEF Z coordinate */
 130         uint32_t pAcc; /* cm - Position Accuracy Estimate */
 131 };
 132
 133 /* Geodetic Position Solution */
 134 struct gps_ubx_posllh {
 135         uint32_t iTOW; /* ms - GPS Millisecond Time of Week */
 136         int32_t lon; /* 1e-7 deg - Longitude */
 137         int32_t lat; /* 1e-7 deg - Latitude */
 138         int32_t height; /* mm - Height above ellipsoid */
 139         int32_t heightMSL; /* mm - Height above mean sea level */
 140         uint32_t hAcc; /* mm - Horizontal Accuracy Estimate */
 141         uint32_t vAcc; /* mm - Vertical Accuracy Estimate */
 142 };
 143
 144 /* Velocity solution in NED */
 145 struct gps_ubx_velned {
 146         uint32_t iTOW; /* GPS Millisecond Time of Week */
 147         int32_t velN; /* NED north velocity */
 148         int32_t velE; /* NED east velocity */
 149         int32_t velD; /* NED down velocity */
 150         uint32_t speed; /* Speed (3-D) */
 151         uint32_t gSpeed; /* Ground Speed (2-D) */
 152         int32_t  heading; /* eading of motion 2-D */
 153         uint32_t sAcc; /* Speed Accuracy Estimate */
 154         uint32_t cAcc; /* Course / Heading Accuracy Estimate */
 155 };
 156
 157 /* This message combines Position, velocity and time solution in ECEF, including
 158  * accuracy figures */
 159 struct gps_ubx_nav_sol {
 160         uint32_t iTOW;
 161         int32_t fTOW;   /* Fractional Nanoseconds remainder of rounded ms above,
 162                          * range -500000 .. 500000 */
 163         int16_t week;   /* GPS week (GPS time) */
 164         uint8_t gpsFix; /* GPSfix Type, 0x00 = No Fix, 0x01 = Dead Reckoning only,
 165                          * 0x02 = 2D-Fix 0x03 = 3D-Fix, 0x04 = GPS + dead reckoning
 166                          * combined, 0x05 = Time only fix, 0x06..0xff: reserved */
 167         uint8_t flags;  /* Fix Status Flags (see graphic below) */
 168         int32_t ecefX;  /* cm - ECEF X coordinate */
 169         int32_t ecefY;  /* cm - ECEF Y coordinate */
 170         int32_t ecefZ;  /* cm - ECEF Z coordinate */
 171         uint32_t pAcc;  /* cm - 3D Position Accuracy Estimate */
 172         int32_t ecefVX; /* cm/s - ECEF X velocity */
 173         int32_t ecefVY; /* cm/s - ECEF Y velocity */
 174         int32_t ecefVZ; /* cm/s - ECEF Z velocity */
 175         uint32_t sAcc;  /* cm/s - Speed Accuracy Estimate */
 176         uint16_t pDOP;  /* Position DOP */
 177         uint8_t reserved1;
 178         uint8_t numSV; /*  Number of SVs used in Nav Solution */
 179         uint32_t reserved2;
 180 };
 181
 182 /********************* CFG class */
 183
 184 /* Get/Set Port Configuration for UART
 185  * Several configurations can be concatenated to one input message. In
 186  * this case the payload length can be a multiple of the normal length
 187  * (see the other versions of CFG-PRT). Output messages from the module
 188  * contain only one configuration unit. */
 189 struct gps_ubx_prt {
 190         uint8_t portID; /* Port Identifier Number (= 1 or 2 for UART ports) */
 191         uint8_t reserved0;
 192 #define GPS_UBX_PRT_TXREADY_EN_MASK    0x0001
 193 #define GPS_UBX_PRT_TXREADY_POL_MASK   0x0002
 194 #define GPS_UBX_PRT_TXREADY_PIN_MASK   0x007c
 195 #define GPS_UBX_PRT_TXREADY_THRES_MASK 0xff80
 196         uint16_t txReady; /* reserved (Alwyas set to zero) up to Firmware 7.
 197                            * TX ready PIN configuration (since Firmware 7). */
 198 #define GPS_UBX_PRT_MODE_RESERVED1_MASK 0x00000010
 199 #define GPS_UBX_PRT_MODE_CHARLEN_MASK   0x000000c0
 200 #define GPS_UBX_PRT_MODE_CHARLEN_5BITS  0x00000000
 201 #define GPS_UBX_PRT_MODE_CHARLEN_6BITS  0x00000040
 202 #define GPS_UBX_PRT_MODE_CHARLEN_7BITS  0x00000080
 203 #define GPS_UBX_PRT_MODE_CHARLEN_8BITS  0x000000c0
 204 #define GPS_UBX_PRT_MODE_PARITY_MASK    0x00000e00
 205 #define GPS_UBX_PRT_MODE_PARITY_EVEN    0x00000000
 206 #define GPS_UBX_PRT_MODE_PARITY_ODD     0x00000200
 207 #define GPS_UBX_PRT_MODE_PARITY_NONE    0x00000800
 208 #define GPS_UBX_PRT_MODE_NSTOP_MASK     0x00003000
 209 #define GPS_UBX_PRT_MODE_NSTOP_1        0x00000000
 210 #define GPS_UBX_PRT_MODE_NSTOP_1_5      0x00001000
 211 #define GPS_UBX_PRT_MODE_NSTOP_2        0x00002000
 212 #define GPS_UBX_PRT_MODE_NSTOP_0_5      0x00003000
 213         uint32_t mode; /* A bit mask describing the UART mode */
 214
 215 #define GPS_UBX_PRT_PROTOMASK_UBX  0x0001
 216 #define GPS_UBX_PRT_PROTOMASK_NMEA 0x0002
 217         /* A mask describing which input protocols are active.  Each bit
 218          * of this mask is used for a protocol.  Through that, multiple
 219          * protocols can be defined on a single port. */
 220         uint16_t inProtoMask;
 221         /* A mask describing which output protocols are active */
 222         uint16_t outProtoMask;
 223 };
 224
 225
 226 /* Set/Get message rate configuration to/from the receiver. */
 227 struct gps_ubx_msg {
 228         uint8_t class;
 229         uint8_t id;
 230         uint8_t rate; /* Send rate */
 231
 232 };
 233
 234 /* Configure the SBAS (Satellite-based augmentation systems) receiver subsystem
 235  * (i.e. WAAS, EGNOS, MSAS). */
 236 struct gps_ubx_sbas {
 237 #define GPS_UBX_SBAS_MODE_ENABLED 0x01 /* SBAS Enabled */
 238 #define GPS_UBX_SBAS_MODE_TEST    0x02 /* SBAS Testbed: Use data anyhow */
 239         uint8_t mode; /* bitfield */
 240 #define GPS_UBX_SBAS_USAGE_RANGE     0x01 /* Use SBAS GEOs as a ranging source */
 241 #define GPS_UBX_SBAS_USAGE_DIFFCORR  0x02 /* Use SBAS Differential Corrections */
 242 #define GPS_UBX_SBAS_USAGE_INTEGRITY 0x04 /* Use SBAS Integrity Information */
 243         uint8_t usage; /* bitfield */
 244         uint8_t maxSBAS;  /* Maximum Number of SBAS prioritized tracking
 245                            * channels (valid range: 0 - 3) to use */
 246         uint8_t scanmode2; /* PRN152 -> PRN156 (bit 0 to 6) */
 247         uint32_t scanmode1; /* PRN120 -> PRN151 (bit 0 to 31) */
 248 };
 249
 250 /* The u-blox positioning technology supports navigation update rates higher or
 251  * lower than 1 update per second. The calculation of the navigation solution
 252  * will always be aligned to the top of a second.
 253  *
 254  * • The update rate has a direct influence on the power consumption. The more
 255  *   fixes that are required, the more CPU power and communication resources
 256  *   are required.
 257  * • For most applications a 1 Hz update rate would be sufficient. */
 258 struct gps_ubx_rate {
 259         uint16_t measRate; /* ms - Measurement Rate */
 260         uint16_t navRate;  /* cycles - Navigation Rate, in number of measurement
 261                               cycles. On u-blox 5 and u-blox 6, this parameter
 262                               cannot be changed, and is always equals 1. */
 263         uint16_t timeRef;  /* Alignment to reference time: 0 = UTC time,
 264                             * 1 = GPS time */
 265 };
 266
 267
 268
 269 /********************* ACK class */
 270
 271 /* When messages from the Class CFG are sent to the receiver, the
 272 * receiver will send an Acknowledge (ACK-ACK) or a Not Acknowledge
 273 * (ACK-NAK) message back to the sender, depending on whether or not the
 274 * message was processed correctly.  There is no ACK/NAK mechanism for
 275 * message poll requests outside Class CFG. */
 276
 277 #define GPS_UBX_ID_NACK 0x00
 278 #define GPS_UBX_ID_ACK  0x01
 279
 280 struct gps_ubx_ack {
 281         uint8_t class; /* GPS_UBX_CLASS_ACK */
 282         uint8_t id;    /* GPS_UBX_ID_ACK or GPS_UBX_ID_NACK */
 283         uint8_t ack_class;
 284         uint8_t ack_id;
 285 };
 286
 287 /********************* RXM class */
 288
 289 struct gps_ubx_rxm_svsi {
 290         int32_t iTOW;    /* Measurement integer millisecond GPS time of week */
 291         int16_t week;    /* Measurement GPS week number. */
 292         uint8_t numVis;  /* Number of visible satellites */
 293         uint8_t numSV;   /* Number of per-SV data blocks following */
 294         struct {
 295                 uint8_t svid;    /* Satellite ID */
 296                 uint8_t svFlag;  /* Information Flags */
 297                 int16_t azim;    /* Azimuth */
 298                 int8_t elev;     /* Elevation */
 299                 uint8_t age;     /* Age of Almanach and Ephemeris */
 300         } sv[]; /* reapeated numSV times */
 301 };
 302
 303
 304
 305 static void gps_ubx_cksum_update(struct gps_ubx_cksum *cksum, uint8_t byte)
 306 {
 307         cksum->ck_a = cksum->ck_a + byte;
 308         cksum->ck_b = cksum->ck_b + cksum->ck_a;
 309 }
 310
 311 static void serial1_tx_cout(uint8_t c)
 312 {
 313         debug_printf("%.2X ", c);
 314         uart_send(GPS_UART, c);
 315 }
 316
 317
 318 void gps_ubx_send(uint8_t class, uint8_t id, void *payload, uint16_t len)
 319 {
 320         struct gps_ubx_cksum cksum = { 0, 0 };
 321         uint16_t n;
 322         uint8_t byte;
 323
 324         debug_printf("SEND ");
 325
 326         serial1_tx_cout(START1);
 327         serial1_tx_cout(START2);
 328         serial1_tx_cout(class);
 329         gps_ubx_cksum_update(&cksum, class);
 330         serial1_tx_cout(id);
 331         gps_ubx_cksum_update(&cksum, id);
 332         serial1_tx_cout(len & 0xff);
 333         gps_ubx_cksum_update(&cksum, len & 0xff);
 334         serial1_tx_cout((len >> 8) & 0xff);
 335         gps_ubx_cksum_update(&cksum, (len >> 8) & 0xff);
 336         /* payload */
 337         for (n = 0; n < len; n++) {
 338                 byte = *(uint8_t *)payload;
 339                 serial1_tx_cout(byte);
 340                 gps_ubx_cksum_update(&cksum, byte);
 341                 payload++;
 342         }
 343         /* checksum and tail */
 344         serial1_tx_cout(cksum.ck_a);
 345         serial1_tx_cout(cksum.ck_b);
 346
 347         debug_printf("\n");
 348 }
 349
 350 /* frequency of messages */
 351 void gps_ubx_set_msg_rate(uint8_t class, uint8_t id, uint8_t rate)
 352 {
 353         struct gps_ubx_msg msg_rate;
 354
 355         memset(&msg_rate, 0, sizeof(msg_rate));
 356         msg_rate.class = class;
 357         msg_rate.id = id;
 358         msg_rate.rate = rate;
 359
 360         gps_ubx_send(GPS_UBX_CLASS_CFG, GPS_UBX_ID_CFG_MSG,
 361                 &msg_rate, sizeof(msg_rate));
 362 }
 363
 364 /* update rate */
 365 void gps_ubx_set_rate(uint16_t measRate, uint16_t navRate, uint16_t timeRef)
 366 {
 367         struct gps_ubx_rate rate;
 368
 369         memset(&rate, 0, sizeof(rate));
 370         rate.measRate = htole16(measRate);
 371         rate.navRate = htole16(navRate);
 372         rate.timeRef = htole16(timeRef);
 373
 374         gps_ubx_send(GPS_UBX_CLASS_CFG, GPS_UBX_ID_CFG_RATE,
 375                 &rate, sizeof(rate));
 376 }
 377
 378 /* set satellite-based augmentation system (differential gps) */
 379 void gps_ubx_set_sbas(uint8_t mode, uint8_t usage, uint8_t maxSBAS,
 380         uint8_t scanmode2, uint32_t scanmode1)
 381 {
 382         struct gps_ubx_sbas msg_sbas;
 383
 384         memset(&msg_sbas, 0, sizeof(msg_sbas));
 385         msg_sbas.mode = mode;
 386         msg_sbas.usage = usage;
 387         msg_sbas.maxSBAS = maxSBAS;
 388         msg_sbas.scanmode2 = scanmode2;
 389         msg_sbas.scanmode1 = htole32(scanmode1);
 390
 391         gps_ubx_send(GPS_UBX_CLASS_CFG, GPS_UBX_ID_CFG_SBAS,
 392                 &msg_sbas, sizeof(msg_sbas));
 393 }
 394
 395 int8_t recv_cb(uint8_t byte)
 396 {
 397         uint16_t i;
 398
 399         /* bytes 0 and 1 are start bytes */
 400         if (rxframe.cur_len == 0) {
 401                 debug_printf("start1 %x\n", byte);
 402                 if (byte != START1) {
 403                         debug_printf("bad start1 %.2X\n", byte);
 404                         goto reset_buf;
 405                 }
 406         }
 407         else if (rxframe.cur_len == 1) {
 408                 debug_printf("start2 %x\n", byte);
 409                 if (byte != START2) {
 410                         debug_printf("bad start2 %.2X\n", byte);
 411                         goto reset_buf;
 412                 }
 413         }
 414         /* bytes 2 and 3 are the class and id */
 415         else if (rxframe.cur_len == 2) {
 416                 debug_printf("class %x\n", byte);
 417                 rxframe.class = byte;
 418         }
 419         else if (rxframe.cur_len == 3) {
 420                 debug_printf("id %x\n", byte);
 421                 rxframe.id = byte;
 422         }
 423         /* bytes 4 and 5 are the length of frame in little endian order */
 424         else if (rxframe.cur_len == 4) {
 425                 debug_printf("len_l %x\n", byte);
 426                 rxframe.len = (uint16_t)byte;
 427         }
 428         else if (rxframe.cur_len == 5) {
 429                 debug_printf("len_h %x\n", byte);
 430                 rxframe.len |= (uint16_t)byte << 8;
 431                 if (rxframe.len > MAX_LEN) {
 432                         debug_printf("bad len %d\n", rxframe.len);
 433                         goto reset_buf;
 434                 }
 435         }
 436         /* next bytes are data (the 6 below is the size of header) */
 437         else if ((rxframe.cur_len - 6) < rxframe.len) {
 438                 debug_printf("data %x\n", byte);
 439                 rxframe.data[rxframe.cur_len - 6] = byte;
 440         }
 441         /* then it's the cksum */
 442         else if ((rxframe.cur_len - 6) == rxframe.len) {
 443                 debug_printf("ck_a %x\n", byte);
 444                 rxcksum.ck_a = byte;
 445         }
 446         else if ((rxframe.cur_len - 6) == rxframe.len + 1) {
 447                 struct gps_ubx_cksum cksum = { 0, 0 };
 448
 449                 debug_printf("ck_b %x\n", byte);
 450                 rxcksum.ck_b = byte;
 451
 452                 /* check crc */
 453                 gps_ubx_cksum_update(&cksum, rxframe.class);
 454                 gps_ubx_cksum_update(&cksum, rxframe.id);
 455                 gps_ubx_cksum_update(&cksum, rxframe.len & 0xff);
 456                 gps_ubx_cksum_update(&cksum, rxframe.len >> 8);
 457                 for (i = 0; i < rxframe.len; i++)
 458                         gps_ubx_cksum_update(&cksum, rxframe.data[i]);
 459                 if (cksum.ck_a != rxcksum.ck_a ||
 460                         cksum.ck_b != rxcksum.ck_b) {
 461                         debug_printf("invalid checksum %2.2x%2.2x != %2.2x%2.2x\n",
 462                                 cksum.ck_a, cksum.ck_b,
 463                                 rxcksum.ck_a, rxcksum.ck_a);
 464                         goto reset_buf;
 465                 }
 466
 467                 debug_printf("valid frame received\n");
 468                 rxframe.cur_len = 0;
 469                 return 0;
 470         }
 471         else /* should not happen */
 472                 goto reset_buf;
 473
 474         rxframe.cur_len ++;
 475         return 1;
 476
 477  reset_buf:
 478         rxframe.cur_len = 0;
 479         return 1;
 480 }
 481
 482 int recv_msg(void)
 483 {
 484         int ret;
 485         uint8_t irq_flags;
 486         int16_t c;
 487         uint16_t t1, t2, diff;
 488
 489         IRQ_LOCK(irq_flags);
 490         t1 = global_ms;
 491         IRQ_UNLOCK(irq_flags);
 492
 493         while (1) {
 494                 IRQ_LOCK(irq_flags);
 495                 t2 = global_ms;
 496                 IRQ_UNLOCK(irq_flags);
 497                 diff = t2 - t1;
 498                 if (diff > 1000) {
 499                         printf_P(PSTR("recv_msg timeout\n"));
 500                         return -1;
 501                 }
 502
 503                 c = uart_recv_nowait(GPS_UART);
 504                 if (c < 0)
 505                         continue;
 506
 507                 debug_printf("%2.2x\n", c);
 508
 509                 ret = recv_cb(c);
 510                 if (ret == 0)
 511                         return 0;
 512         }
 513 }
 514
 515 int wait_ack(int msg_type)
 516 {
 517         uint8_t irq_flags;
 518         int ret;
 519         uint16_t t1, t2, diff;
 520
 521         IRQ_LOCK(irq_flags);
 522         t1 = global_ms;
 523         IRQ_UNLOCK(irq_flags);
 524
 525
 526         while (1) {
 527                 ret = recv_msg();
 528                 if (ret < 0)
 529                         return -1;
 530
 531                 IRQ_LOCK(irq_flags);
 532                 t2 = global_ms;
 533                 IRQ_UNLOCK(irq_flags);
 534                 diff = t2 - t1;
 535                 if (diff > 1000) {
 536                         printf_P(PSTR("wait_ack timeout\n"));
 537                         return -1;
 538                 }
 539
 540                 /* retry if it's not the expected message */
 541                 if (rxframe.data[0] != 0x83 && rxframe.data[0] != 0x84)
 542                         continue;
 543                 if (rxframe.data[1] != msg_type)
 544                         continue;
 545
 546                 if (rxframe.data[0] == 0x83)
 547                         printf_P(PSTR("ACK\n"));
 548                 else if (rxframe.data[0] == 0x84)
 549                         printf_P(PSTR("NACK\n"));
 550                 else
 551                         printf_P(PSTR("ZARB\n"));
 552                 break;
 553         }
 554
 555         return 0;
 556 }
 557
 558 static int decode_gps_posllh(const struct gps_frame *frame)
 559 {
 560         const struct gps_ubx_posllh *posllh =
 561                 (const struct gps_ubx_posllh *)&frame->data;
 562         int32_t height, heightMSL;
 563         uint8_t irq_flags;
 564
 565         //printf("%s\n", __FUNCTION__);
 566         /* pos struct is in cm, convert */
 567         height = letoh32(posllh->height) / 10;
 568         heightMSL = letoh32(posllh->heightMSL) / 10;
 569
 570         IRQ_LOCK(irq_flags);
 571         gps_pos.tow = letoh32(posllh->iTOW);
 572         gps_pos.longitude = letoh32(posllh->lon);
 573         gps_pos.latitude = letoh32(posllh->lat);
 574         gps_pos.altitude = height;
 575         gps_pos.sea_altitude = heightMSL;
 576         IRQ_UNLOCK(irq_flags);
 577
 578         return 0;
 579 }
 580
 581 static int decode_gps_posecef(const struct gps_frame *frame)
 582 {
 583         const struct gps_ubx_posecef *posecef =
 584                 (const struct gps_ubx_posecef *)&frame->data;
 585         uint8_t irq_flags;
 586
 587         //printf("%s\n", __FUNCTION__);
 588         IRQ_LOCK(irq_flags);
 589         gps_pos.tow = letoh32(posecef->iTOW);
 590         gps_pos.ecef_x = letoh32(posecef->ecefX);
 591         gps_pos.ecef_y = letoh32(posecef->ecefY);
 592         gps_pos.ecef_z = letoh32(posecef->ecefZ);
 593         IRQ_UNLOCK(irq_flags);
 594
 595         return 0;
 596 }
 597
 598 static int decode_gps_velned(const struct gps_frame *frame)
 599 {
 600         const struct gps_ubx_velned *velned =
 601                 (const struct gps_ubx_velned *)&frame->data;
 602         uint8_t irq_flags;
 603
 604         //printf("%s\n", __FUNCTION__);
 605         IRQ_LOCK(irq_flags);
 606         gps_pos.tow = letoh32(velned->iTOW);
 607         //gps_pos.speed = letoh32(velned->speed); //XXX add speed ?
 608         IRQ_UNLOCK(irq_flags);
 609
 610         return 0;
 611 }
 612
 613 static int decode_gps_nav_sol(const struct gps_frame *frame)
 614 {
 615         const struct gps_ubx_nav_sol *nav_sol =
 616                 (const struct gps_ubx_nav_sol *)&frame->data;
 617         uint8_t irq_flags;
 618
 619         //printf("%s\n", __FUNCTION__);
 620         IRQ_LOCK(irq_flags);
 621         gps_pos.tow = letoh32(nav_sol->iTOW);
 622         gps_pos.gps_week = letoh16(nav_sol->week);
 623         gps_pos.mode = nav_sol->gpsFix;
 624         gps_pos.sv_num = nav_sol->numSV;
 625         gps_pos.ecef_x = letoh32(nav_sol->ecefX);
 626         gps_pos.ecef_y = letoh32(nav_sol->ecefY);
 627         gps_pos.ecef_z = letoh32(nav_sol->ecefZ);
 628         gps_pos.ecef_vx = letoh32(nav_sol->ecefVX);
 629         gps_pos.ecef_vy = letoh32(nav_sol->ecefVY);
 630         gps_pos.ecef_vz = letoh32(nav_sol->ecefVZ);
 631         IRQ_UNLOCK(irq_flags);
 632
 633         return 0;
 634 }
 635
 636 static int decode_gps_rxm_svsi(const struct gps_frame *frame)
 637 {
 638         const struct gps_ubx_rxm_svsi *rxm_svsi =
 639                 (const struct gps_ubx_rxm_svsi *)&frame->data;
 640         uint8_t irq_flags;
 641
 642         //printf("%s\n", __FUNCTION__);
 643         IRQ_LOCK(irq_flags);
 644         gps_pos.tow = letoh32(rxm_svsi->iTOW);
 645         gps_pos.gps_week = letoh16(rxm_svsi->week);
 646         gps_pos.sv_num = rxm_svsi->numVis;
 647         IRQ_UNLOCK(irq_flags);
 648
 649         return 0;
 650 }
 651
 652 static int decode_gps(const struct gps_frame *frame)
 653 {
 654         int ret = -1;
 655
 656         switch (frame->class) {
 657         case GPS_UBX_CLASS_NAV:
 658                 switch (frame->id) {
 659                 case GPS_UBX_ID_NAV_POSLLH:
 660                         ret = decode_gps_posllh(frame);
 661                         break;
 662                 case GPS_UBX_ID_NAV_POSECEF:
 663                         ret = decode_gps_posecef(frame);
 664                         break;
 665                 case GPS_UBX_ID_NAV_VELNED:
 666                         ret = decode_gps_velned(frame);
 667                         break;
 668                 case GPS_UBX_ID_NAV_SOL:
 669                         ret = decode_gps_nav_sol(frame);
 670                         break;
 671                 default:
 672                         break;
 673                 }
 674                 break;
 675         case GPS_UBX_CLASS_RXM:
 676                 switch (frame->id) {
 677                 case GPS_UBX_ID_RXM_SVSI:
 678                         ret = decode_gps_rxm_svsi(frame);
 679                         break;
 680                 default:
 681                         break;
 682                 }
 683         default:
 684                 break;
 685         }
 686
 687         if (ret < 0)
 688                 debug_printf("error processing message class=%x id=%x\n",
 689                         frame->class, frame->id);
 690
 691         return ret;
 692 #if 0
 693         /* fix endianness inside the frame */
 694         pos->gps_week = ntohs(pos->gps_week);
 695         pos->tow = ntohl(pos->tow);
 696
 697         pos->latitude = ntohl(pos->latitude);
 698         pos->longitude = ntohl(pos->longitude);
 699         pos->altitude = ntohl(pos->altitude);
 700
 701         pos->sea_altitude = ntohl(pos->sea_altitude);
 702
 703         pos->gdop = ntohs(pos->gdop);
 704         pos->pdop = ntohs(pos->pdop);
 705         pos->hdop = ntohs(pos->hdop);
 706         pos->vdop = ntohs(pos->vdop);
 707         pos->tdop = ntohs(pos->tdop);
 708
 709         pos->ecef_vx = ntohl(pos->ecef_vx);
 710         pos->ecef_vy = ntohl(pos->ecef_vy);
 711         pos->ecef_vz = ntohl(pos->ecef_vz);
 712
 713         /* update global structure */
 714         IRQ_LOCK(irq_flags);
 715         memcpy(&gps_pos, pos, sizeof(gps_pos));
 716         IRQ_UNLOCK(irq_flags);
 717 #endif
 718
 719         return 0;
 720 }
 721
 722 /* display current GPS position stored in the global variable */
 723 static void display_gps(void)
 724 {
 725         /* no need to call ntohs/ntohl because the frame is already fixed by
 726          * decode_gps_pos() */
 727         printf_P(PSTR("id %.2X mode %.2X svnum %.2X gpsw %.4X "
 728                         "tow %.10"PRIu32"\t"),
 729                 gps_pos.msg_id,
 730                 gps_pos.mode,
 731                 gps_pos.sv_num,
 732                 gps_pos.gps_week,
 733                 gps_pos.tow);
 734
 735         printf_P(PSTR("lat %.8"PRIx32" long %.8"PRIx32" alt %.8"PRIx32"\n"),
 736                 gps_pos.latitude,
 737                 gps_pos.longitude,
 738                 gps_pos.altitude);
 739
 740         printf_P(PSTR("gdop %3.3f pdop %3.3f hdop %3.3f vdop %3.3f "
 741                         "tdop %3.3f\n"),
 742                 (double)gps_pos.gdop/100.,
 743                 (double)gps_pos.pdop/100.,
 744                 (double)gps_pos.hdop/100.,
 745                 (double)gps_pos.vdop/100.,
 746                 (double)gps_pos.tdop/100.);
 747
 748         printf_P(PSTR("lat %3.5f long %3.5f alt %3.5f sea_alt %3.5f\n"),
 749                 (double)gps_pos.latitude/10000000.,
 750                 (double)gps_pos.longitude/10000000.,
 751                 (double)gps_pos.altitude/100.,
 752                 (double)gps_pos.sea_altitude/100.);
 753
 754         printf_P(PSTR("vx %3.3f vy %3.3f vz %3.3f\n"),
 755                 (double)gps_pos.ecef_vx/100.,
 756                 (double)gps_pos.ecef_vy/100.,
 757                 (double)gps_pos.ecef_vz/100.);
 758 }
 759
 760 /* called on timer event */
 761 static void gps_ubx_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
 762 {
 763         static int16_t last_valid = 0;
 764         static uint8_t is_valid = 0;
 765         int16_t ms, diff;
 766         int16_t c;
 767         int8_t ret;
 768         uint8_t irq_flags;
 769
 770         (void)cm;
 771         (void)tim;
 772         (void)arg;
 773
 774         IRQ_LOCK(irq_flags);
 775         ms = global_ms;
 776         IRQ_UNLOCK(irq_flags);
 777
 778         while (1) {
 779                 c = uart_recv_nowait(GPS_UART);
 780                 if (c < 0) /* no more char */
 781                         break;
 782
 783                 ret = recv_cb(c);
 784                 if (ret == 0) {
 785                         decode_gps(&rxframe);
 786                         if (0)
 787                                 display_gps();
 788
 789                         last_valid = ms;
 790                         is_valid = 1;
 791                 }
 792         }
 793
 794         diff = ms - last_valid;
 795         if (is_valid == 1 && diff > 500) {
 796
 797                 is_valid = 0;
 798                 /* update global structure */
 799                 IRQ_LOCK(irq_flags);
 800                 memset(&gps_pos, 0, sizeof(gps_pos));
 801                 gps_pos.latitude = 0xffffffff;
 802                 IRQ_UNLOCK(irq_flags);
 803         }
 804
 805         callout_schedule(cm, tim, GPS_PERIOD_MS);
 806 }
 807
 808 int gps_ubx_init(void)
 809 {
 810         printf_P(PSTR("gps_ubx_init\n"));
 811
 812         gps_ubx_set_msg_rate(GPS_UBX_CLASS_NMEA_STD, GPS_UBX_ID_NMEA_GGA, 0);
 813         gps_ubx_set_msg_rate(GPS_UBX_CLASS_NMEA_STD, GPS_UBX_ID_NMEA_GLL, 0);
 814         gps_ubx_set_msg_rate(GPS_UBX_CLASS_NMEA_STD, GPS_UBX_ID_NMEA_GSA, 0);
 815         gps_ubx_set_msg_rate(GPS_UBX_CLASS_NMEA_STD, GPS_UBX_ID_NMEA_GSV, 0);
 816         gps_ubx_set_msg_rate(GPS_UBX_CLASS_NMEA_STD, GPS_UBX_ID_NMEA_RMC, 0);
 817         gps_ubx_set_msg_rate(GPS_UBX_CLASS_NMEA_STD, GPS_UBX_ID_NMEA_VTG, 0);
 818
 819         gps_ubx_set_msg_rate(GPS_UBX_CLASS_NAV, GPS_UBX_ID_NAV_POSLLH, 1);
 820         gps_ubx_set_msg_rate(GPS_UBX_CLASS_NAV, GPS_UBX_ID_NAV_STATUS, 1);
 821         gps_ubx_set_msg_rate(GPS_UBX_CLASS_NAV, GPS_UBX_ID_NAV_SOL, 1);
 822         gps_ubx_set_msg_rate(GPS_UBX_CLASS_NAV, GPS_UBX_ID_NAV_VELNED, 1);
 823         //gps_ubx_set_msg_rate(GPS_UBX_CLASS_RXM, GPS_UBX_ID_RXM_SVSI, 1);
 824
 825         /* use EGNOS */
 826         gps_ubx_set_sbas(GPS_UBX_SBAS_MODE_ENABLED,
 827                 GPS_UBX_SBAS_USAGE_RANGE | GPS_UBX_SBAS_USAGE_DIFFCORR |
 828                 GPS_UBX_SBAS_USAGE_INTEGRITY, 3, 0x0, 0x851);
 829
 830         gps_ubx_set_rate(500, 1, 1); /* 2hz */
 831
 832         printf_P(PSTR("GPS configuration done !\n"));
 833
 834         callout_init(&gps_timer, gps_ubx_cb, NULL, GPS_PRIO);
 835         callout_schedule(&imuboard.intr_cm, &gps_timer, GPS_PERIOD_MS); /* every 2ms */
 836
 837         return 0;
 838 }