flight/modules/GPS/UBX.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotModules OpenPilot Modules
   4  * @{
   5  * @addtogroup GPSModule GPS Module
   6  * @brief Process GPS information (UBX binary format)
   7  * @{
   8  *
   9  * @file       UBX.c
  10  * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016.
  11  *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
  12  * @brief      GPS module, handles GPS and UBX stream
  13  * @see        The GNU Public License (GPL) Version 3
  14  *
  15  *****************************************************************************/
  16 /*
  17  * This program is free software; you can redistribute it and/or modify
  18  * it under the terms of the GNU General Public License as published by
  19  * the Free Software Foundation; either version 3 of the License, or
  20  * (at your option) any later version.
  21  *
  22  * This program is distributed in the hope that it will be useful, but
  23  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  24  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  25  * for more details.
  26  *
  27  * You should have received a copy of the GNU General Public License along
  28  * with this program; if not, write to the Free Software Foundation, Inc.,
  29  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  30  */
  31
  32 #include "openpilot.h"
  33 #include "pios.h"
  34 #include "pios_math.h"
  35 #include <pios_helpers.h>
  36 #include <pios_delay.h>
  37
  38 #if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
  39
  40 #include "inc/UBX.h"
  41 #include "inc/GPS.h"
  42 #include <string.h>
  43
  44 #if !defined(PIOS_GPS_MINIMAL)
  45 #include <auxmagsupport.h>
  46 static bool useMag = false;
  47 #endif /* !defined(PIOS_GPS_MINIMAL) */
  48
  49 // this is set and used by this low level ubx code
  50 // it is also reset by the ubx configuration code (UBX6 vs. UBX7) in ubx_autoconfig.c
  51 GPSPositionSensorSensorTypeOptions ubxSensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN;
  52
  53 static bool usePvt = false;
  54 static uint32_t lastPvtTime = 0;
  55
  56 // parse table item
  57 typedef struct {
  58     uint8_t msgClass;
  59     uint8_t msgID;
  60     void (*handler)(struct UBXPacket *, GPSPositionSensorData *GpsPosition);
  61 } ubx_message_handler;
  62
  63 // parsing functions, roughly ordered by reception rate (higher rate messages on top)
  64 static void parse_ubx_nav_posllh(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
  65 static void parse_ubx_nav_velned(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
  66 static void parse_ubx_nav_sol(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
  67 static void parse_ubx_nav_dop(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
  68 #if !defined(PIOS_GPS_MINIMAL)
  69 static void parse_ubx_nav_pvt(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
  70 static void parse_ubx_nav_timeutc(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
  71 static void parse_ubx_nav_svinfo(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
  72 static void parse_ubx_op_sys(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
  73 static void parse_ubx_op_mag(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
  74 static void parse_ubx_ack_ack(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
  75 static void parse_ubx_ack_nak(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
  76 static void parse_ubx_mon_ver(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
  77 #endif /* !defined(PIOS_GPS_MINIMAL) */
  78
  79 const ubx_message_handler ubx_handler_table[] = {
  80     { .msgClass = UBX_CLASS_NAV,     .msgID = UBX_ID_NAV_POSLLH,  .handler = &parse_ubx_nav_posllh  },
  81     { .msgClass = UBX_CLASS_NAV,     .msgID = UBX_ID_NAV_VELNED,  .handler = &parse_ubx_nav_velned  },
  82     { .msgClass = UBX_CLASS_NAV,     .msgID = UBX_ID_NAV_SOL,     .handler = &parse_ubx_nav_sol     },
  83     { .msgClass = UBX_CLASS_NAV,     .msgID = UBX_ID_NAV_DOP,     .handler = &parse_ubx_nav_dop     },
  84 #if !defined(PIOS_GPS_MINIMAL)
  85     { .msgClass = UBX_CLASS_NAV,     .msgID = UBX_ID_NAV_PVT,     .handler = &parse_ubx_nav_pvt     },
  86     { .msgClass = UBX_CLASS_OP_CUST, .msgID = UBX_ID_OP_MAG,      .handler = &parse_ubx_op_mag      },
  87     { .msgClass = UBX_CLASS_NAV,     .msgID = UBX_ID_NAV_SVINFO,  .handler = &parse_ubx_nav_svinfo  },
  88     { .msgClass = UBX_CLASS_NAV,     .msgID = UBX_ID_NAV_TIMEUTC, .handler = &parse_ubx_nav_timeutc },
  89
  90     { .msgClass = UBX_CLASS_OP_CUST, .msgID = UBX_ID_OP_SYS,      .handler = &parse_ubx_op_sys      },
  91     { .msgClass = UBX_CLASS_ACK,     .msgID = UBX_ID_ACK_ACK,     .handler = &parse_ubx_ack_ack     },
  92     { .msgClass = UBX_CLASS_ACK,     .msgID = UBX_ID_ACK_NAK,     .handler = &parse_ubx_ack_nak     },
  93
  94     { .msgClass = UBX_CLASS_MON,     .msgID = UBX_ID_MON_VER,     .handler = &parse_ubx_mon_ver     },
  95 #endif /* !defined(PIOS_GPS_MINIMAL) */
  96 };
  97 #define UBX_HANDLER_TABLE_SIZE NELEMENTS(ubx_handler_table)
  98
  99 // detected hw version
 100 int32_t ubxHwVersion = -1;
 101
 102 // Last received Ack/Nak
 103 struct UBX_ACK_ACK ubxLastAck;
 104 struct UBX_ACK_NAK ubxLastNak;
 105
 106 // If a PVT sentence is received in the last UBX_PVT_TIMEOUT (ms) timeframe it disables VELNED/POSLLH/SOL/TIMEUTC
 107 #define UBX_PVT_TIMEOUT (1000)
 108
 109 // parse incoming character stream for messages in UBX binary format
 110 int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats)
 111 {
 112     enum proto_states {
 113         START,
 114         UBX_SY2,
 115         UBX_CLASS,
 116         UBX_ID,
 117         UBX_LEN1,
 118         UBX_LEN2,
 119         UBX_PAYLOAD,
 120         UBX_CHK1,
 121         UBX_CHK2,
 122         FINISHED
 123     };
 124     enum restart_states {
 125         RESTART_WITH_ERROR,
 126         RESTART_NO_ERROR
 127     };
 128     static uint16_t rx_count = 0;
 129     static enum proto_states proto_state = START;
 130     struct UBXPacket *ubx    = (struct UBXPacket *)gps_rx_buffer;
 131     int ret = PARSER_INCOMPLETE; // message not (yet) complete
 132     uint16_t i = 0;
 133     uint16_t restart_index   = 0;
 134     enum restart_states restart_state;
 135     uint8_t c;
 136
 137     // switch continue is the normal condition and comes back to here for another byte
 138     // switch break is the error state that branches to the end and restarts the scan at the byte after the first sync byte
 139     while (i < len) {
 140         c = rx[i++];
 141         switch (proto_state) {
 142         case START: // detect protocol
 143             if (c == UBX_SYNC1) { // first UBX sync char found
 144                 proto_state   = UBX_SY2;
 145                 // restart here, at byte after SYNC1, if we fail to parse
 146                 restart_index = i;
 147             }
 148             continue;
 149         case UBX_SY2:
 150             if (c == UBX_SYNC2) { // second UBX sync char found
 151                 proto_state = UBX_CLASS;
 152             } else {
 153                 restart_state = RESTART_NO_ERROR;
 154                 break;
 155             }
 156             continue;
 157         case UBX_CLASS:
 158             ubx->header.class = c;
 159             proto_state      = UBX_ID;
 160             continue;
 161         case UBX_ID:
 162             ubx->header.id   = c;
 163             proto_state      = UBX_LEN1;
 164             continue;
 165         case UBX_LEN1:
 166             ubx->header.len  = c;
 167             proto_state      = UBX_LEN2;
 168             continue;
 169         case UBX_LEN2:
 170             ubx->header.len += (c << 8);
 171             if (ubx->header.len > sizeof(UBXPayload)) {
 172                 gpsRxStats->gpsRxOverflow++;
 173 #if defined(PIOS_GPS_MINIMAL)
 174                 restart_state = RESTART_NO_ERROR;
 175 #else
 176                 restart_state = RESTART_WITH_ERROR;
 177 #endif
 178                 break;
 179             } else {
 180                 if (ubx->header.len == 0) {
 181                     proto_state = UBX_CHK1;
 182                 } else {
 183                     proto_state = UBX_PAYLOAD;
 184                     rx_count    = 0;
 185                 }
 186             }
 187             continue;
 188         case UBX_PAYLOAD:
 189             if (rx_count < ubx->header.len) {
 190                 ubx->payload.payload[rx_count] = c;
 191                 if (++rx_count == ubx->header.len) {
 192                     proto_state = UBX_CHK1;
 193                 }
 194             }
 195             continue;
 196         case UBX_CHK1:
 197             ubx->header.ck_a = c;
 198             proto_state = UBX_CHK2;
 199             continue;
 200         case UBX_CHK2:
 201             ubx->header.ck_b = c;
 202             // OP GPSV9 sends data with bad checksums this appears to happen because it drops data
 203             // this has been proven by running it without autoconfig and testing:
 204             // data coming from OPV9 "GPS+MCU" port the checksum errors happen roughly every 5 to 30 seconds
 205             // same data coming from OPV9 "GPS Only" port the checksums are always good
 206             // this also occasionally causes parse_ubx_message() to issue alarms because not all the messages were received
 207             // see OP GPSV9 comment in parse_ubx_message() for further information
 208             if (checksum_ubx_message(ubx)) {
 209                 gpsRxStats->gpsRxReceived++;
 210                 proto_state = START;
 211                 // overwrite PARSER_INCOMPLETE with PARSER_COMPLETE
 212                 // but don't overwrite PARSER_ERROR with PARSER_COMPLETE
 213                 // pass PARSER_ERROR to caller if it happens even once
 214                 // only pass PARSER_COMPLETE back to caller if we parsed a full set of GPS data
 215                 // that allows the caller to know if we are parsing GPS data
 216                 // or just other packets for some reason (mis-configuration)
 217                 if (parse_ubx_message(ubx, GpsData) == GPSPOSITIONSENSOR_OBJID
 218                     && ret == PARSER_INCOMPLETE) {
 219                     ret = PARSER_COMPLETE;
 220                 }
 221             } else {
 222                 gpsRxStats->gpsRxChkSumError++;
 223                 restart_state = RESTART_WITH_ERROR;
 224                 break;
 225             }
 226             continue;
 227         default:
 228             continue;
 229         }
 230
 231         // this simple restart doesn't work across calls
 232         // but it does work within a single call
 233         // and it does the expected thing across calls
 234         // if restarting due to error detected in 2nd call to this function (on split packet)
 235         // then we just restart at index 0, which is mid-packet, not the second byte
 236         if (restart_state == RESTART_WITH_ERROR) {
 237             ret = PARSER_ERROR; // inform caller that we found at least one error (along with 0 or more good packets)
 238         }
 239         rx  += restart_index; // restart parsing just past the most recent SYNC1
 240         len -= restart_index;
 241         i    = 0;
 242         proto_state = START;
 243     }
 244
 245     return ret;
 246 }
 247
 248 // Keep track of various GPS messages needed to make up a single UAVO update
 249 // time-of-week timestamp is used to correlate matching messages
 250 #define POSLLH_RECEIVED (1 << 0)
 251 #define STATUS_RECEIVED (1 << 1)
 252 #define DOP_RECEIVED    (1 << 2)
 253 #define VELNED_RECEIVED (1 << 3)
 254 #define SOL_RECEIVED    (1 << 4)
 255 #define ALL_RECEIVED    (SOL_RECEIVED | VELNED_RECEIVED | DOP_RECEIVED | POSLLH_RECEIVED)
 256 #define NONE_RECEIVED   0
 257
 258 static struct msgtracker {
 259     uint32_t currentTOW; // TOW of the message set currently in progress
 260     uint8_t  msg_received;   // keep track of received message types
 261 } msgtracker;
 262
 263 // Check if a message belongs to the current data set and register it as 'received'
 264 bool check_msgtracker(uint32_t tow, uint8_t msg_flag)
 265 {
 266     if (tow > msgtracker.currentTOW ? true // start of a new message set
 267         : (msgtracker.currentTOW - tow > 6 * 24 * 3600 * 1000)) { // 6 days, TOW wrap around occured
 268         msgtracker.currentTOW   = tow;
 269         msgtracker.msg_received = NONE_RECEIVED;
 270     } else if (tow < msgtracker.currentTOW) { // message outdated (don't process)
 271         return false;
 272     }
 273
 274     msgtracker.msg_received |= msg_flag; // register reception of this msg type
 275     return true;
 276 }
 277
 278 bool checksum_ubx_message(struct UBXPacket *ubx)
 279 {
 280     int i;
 281     uint8_t ck_a, ck_b;
 282
 283     ck_a  = ubx->header.class;
 284     ck_b  = ck_a;
 285
 286     ck_a += ubx->header.id;
 287     ck_b += ck_a;
 288
 289     ck_a += ubx->header.len & 0xff;
 290     ck_b += ck_a;
 291
 292     ck_a += ubx->header.len >> 8;
 293     ck_b += ck_a;
 294
 295     for (i = 0; i < ubx->header.len; i++) {
 296         ck_a += ubx->payload.payload[i];
 297         ck_b += ck_a;
 298     }
 299
 300     if (ubx->header.ck_a == ck_a &&
 301         ubx->header.ck_b == ck_b) {
 302         return true;
 303     } else {
 304         return false;
 305     }
 306 }
 307
 308 static void parse_ubx_nav_posllh(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition)
 309 {
 310     if (usePvt) {
 311         return;
 312     }
 313     struct UBX_NAV_POSLLH *posllh = &ubx->payload.nav_posllh;
 314
 315     if (check_msgtracker(posllh->iTOW, POSLLH_RECEIVED)) {
 316         if (GpsPosition->Status != GPSPOSITIONSENSOR_STATUS_NOFIX) {
 317             GpsPosition->Altitude  = (float)posllh->hMSL * 0.001f;
 318             GpsPosition->GeoidSeparation = (float)(posllh->height - posllh->hMSL) * 0.001f;
 319             GpsPosition->Latitude  = posllh->lat;
 320             GpsPosition->Longitude = posllh->lon;
 321         }
 322     }
 323 }
 324
 325 static void parse_ubx_nav_sol(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition)
 326 {
 327     if (usePvt) {
 328         return;
 329     }
 330     struct UBX_NAV_SOL *sol = &ubx->payload.nav_sol;
 331     if (check_msgtracker(sol->iTOW, SOL_RECEIVED)) {
 332         GpsPosition->Satellites = sol->numSV;
 333
 334         if (sol->flags & STATUS_FLAGS_GPSFIX_OK) {
 335             switch (sol->gpsFix) {
 336             case STATUS_GPSFIX_2DFIX:
 337                 GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_FIX2D;
 338                 break;
 339             case STATUS_GPSFIX_3DFIX:
 340                 GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_FIX3D;
 341                 break;
 342             default: GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_NOFIX;
 343             }
 344         } else { // fix is not valid so we make sure to treat is as NOFIX
 345             GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_NOFIX;
 346         }
 347     }
 348 }
 349
 350 static void parse_ubx_nav_dop(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition)
 351 {
 352     struct UBX_NAV_DOP *dop = &ubx->payload.nav_dop;
 353
 354     if (check_msgtracker(dop->iTOW, DOP_RECEIVED)) {
 355         GpsPosition->HDOP = (float)dop->hDOP * 0.01f;
 356         GpsPosition->VDOP = (float)dop->vDOP * 0.01f;
 357         GpsPosition->PDOP = (float)dop->pDOP * 0.01f;
 358     }
 359 }
 360
 361 static void parse_ubx_nav_velned(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition)
 362 {
 363     if (usePvt) {
 364         return;
 365     }
 366     GPSVelocitySensorData GpsVelocity;
 367     struct UBX_NAV_VELNED *velned = &ubx->payload.nav_velned;
 368     if (check_msgtracker(velned->iTOW, VELNED_RECEIVED)) {
 369         if (GpsPosition->Status != GPSPOSITIONSENSOR_STATUS_NOFIX) {
 370             GpsVelocity.North        = (float)velned->velN / 100.0f;
 371             GpsVelocity.East         = (float)velned->velE / 100.0f;
 372             GpsVelocity.Down         = (float)velned->velD / 100.0f;
 373             GPSVelocitySensorSet(&GpsVelocity);
 374             GpsPosition->Groundspeed = (float)velned->gSpeed * 0.01f;
 375             GpsPosition->Heading     = (float)velned->heading * 1.0e-5f;
 376         }
 377     }
 378 }
 379
 380 #if !defined(PIOS_GPS_MINIMAL)
 381 static void parse_ubx_nav_pvt(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition)
 382 {
 383     lastPvtTime = PIOS_DELAY_GetuS();
 384
 385     GPSVelocitySensorData GpsVelocity;
 386     struct UBX_NAV_PVT *pvt = &ubx->payload.nav_pvt;
 387     check_msgtracker(pvt->iTOW, (ALL_RECEIVED));
 388
 389     GpsVelocity.North = (float)pvt->velN * 0.001f;
 390     GpsVelocity.East  = (float)pvt->velE * 0.001f;
 391     GpsVelocity.Down  = (float)pvt->velD * 0.001f;
 392     GPSVelocitySensorSet(&GpsVelocity);
 393
 394     GpsPosition->Groundspeed     = (float)pvt->gSpeed * 0.001f;
 395     GpsPosition->Heading         = (float)pvt->heading * 1.0e-5f;
 396     GpsPosition->Altitude        = (float)pvt->hMSL * 0.001f;
 397     GpsPosition->GeoidSeparation = (float)(pvt->height - pvt->hMSL) * 0.001f;
 398     GpsPosition->Latitude        = pvt->lat;
 399     GpsPosition->Longitude       = pvt->lon;
 400     GpsPosition->Satellites      = pvt->numSV;
 401     GpsPosition->PDOP = pvt->pDOP * 0.01f;
 402     if (pvt->flags & PVT_FLAGS_GNSSFIX_OK) {
 403         GpsPosition->Status = pvt->fixType == PVT_FIX_TYPE_3D ?
 404                               GPSPOSITIONSENSOR_STATUS_FIX3D : GPSPOSITIONSENSOR_STATUS_FIX2D;
 405     } else {
 406         GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_NOFIX;
 407     }
 408
 409     if (pvt->valid & PVT_VALID_VALIDTIME) {
 410         // Time is valid, set GpsTime
 411         GPSTimeData GpsTime;
 412
 413         GpsTime.Year   = pvt->year;
 414         GpsTime.Month  = pvt->month;
 415         GpsTime.Day    = pvt->day;
 416         GpsTime.Hour   = pvt->hour;
 417         GpsTime.Minute = pvt->min;
 418         GpsTime.Second = pvt->sec;
 419
 420         GPSTimeSet(&GpsTime);
 421     }
 422 }
 423
 424 static void parse_ubx_nav_timeutc(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition)
 425 {
 426     if (usePvt) {
 427         return;
 428     }
 429
 430     struct UBX_NAV_TIMEUTC *timeutc = &ubx->payload.nav_timeutc;
 431     // Test if time is valid
 432     if ((timeutc->valid & TIMEUTC_VALIDTOW) && (timeutc->valid & TIMEUTC_VALIDWKN)) {
 433         // Time is valid, set GpsTime
 434         GPSTimeData GpsTime;
 435
 436         GpsTime.Year   = timeutc->year;
 437         GpsTime.Month  = timeutc->month;
 438         GpsTime.Day    = timeutc->day;
 439         GpsTime.Hour   = timeutc->hour;
 440         GpsTime.Minute = timeutc->min;
 441         GpsTime.Second = timeutc->sec;
 442         GpsTime.Millisecond = (int16_t)(timeutc->nano / 1000000);
 443         GPSTimeSet(&GpsTime);
 444     } else {
 445         // Time is not valid, nothing to do
 446         return;
 447     }
 448 }
 449
 450 static void parse_ubx_nav_svinfo(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition)
 451 {
 452     uint8_t chan;
 453     GPSSatellitesData svdata;
 454     struct UBX_NAV_SVINFO *svinfo = &ubx->payload.nav_svinfo;
 455
 456     svdata.SatsInView = 0;
 457
 458     // First, use slots for SVs actually being received
 459     for (chan = 0; chan < svinfo->numCh; chan++) {
 460         if (svdata.SatsInView < GPSSATELLITES_PRN_NUMELEM && svinfo->sv[chan].cno > 0) {
 461             svdata.Azimuth[svdata.SatsInView]   = svinfo->sv[chan].azim;
 462             svdata.Elevation[svdata.SatsInView] = svinfo->sv[chan].elev;
 463             svdata.PRN[svdata.SatsInView] = svinfo->sv[chan].svid;
 464             svdata.SNR[svdata.SatsInView] = svinfo->sv[chan].cno;
 465             svdata.SatsInView++;
 466         }
 467     }
 468
 469     // Now try to add the rest
 470     for (chan = 0; chan < svinfo->numCh; chan++) {
 471         if (svdata.SatsInView < GPSSATELLITES_PRN_NUMELEM && 0 == svinfo->sv[chan].cno) {
 472             svdata.Azimuth[svdata.SatsInView]   = svinfo->sv[chan].azim;
 473             svdata.Elevation[svdata.SatsInView] = svinfo->sv[chan].elev;
 474             svdata.PRN[svdata.SatsInView] = svinfo->sv[chan].svid;
 475             svdata.SNR[svdata.SatsInView] = svinfo->sv[chan].cno;
 476             svdata.SatsInView++;
 477         }
 478     }
 479
 480     // fill remaining slots (if any)
 481     for (chan = svdata.SatsInView; chan < GPSSATELLITES_PRN_NUMELEM; chan++) {
 482         svdata.Azimuth[chan]   = 0;
 483         svdata.Elevation[chan] = 0;
 484         svdata.PRN[chan] = 0;
 485         svdata.SNR[chan] = 0;
 486     }
 487
 488     GPSSatellitesSet(&svdata);
 489 }
 490
 491 static void parse_ubx_ack_ack(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition)
 492 {
 493     struct UBX_ACK_ACK *ack_ack = &ubx->payload.ack_ack;
 494
 495     ubxLastAck = *ack_ack;
 496 }
 497
 498 static void parse_ubx_ack_nak(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition)
 499 {
 500     struct UBX_ACK_NAK *ack_nak = &ubx->payload.ack_nak;
 501
 502     ubxLastNak = *ack_nak;
 503 }
 504
 505 static void parse_ubx_mon_ver(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition)
 506 {
 507     struct UBX_MON_VER *mon_ver = &ubx->payload.mon_ver;
 508
 509     ubxHwVersion  = atoi(mon_ver->hwVersion);
 510     ubxSensorType = (ubxHwVersion >= UBX_HW_VERSION_8) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX8 :
 511                     ((ubxHwVersion >= UBX_HW_VERSION_7) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX7 : GPSPOSITIONSENSOR_SENSORTYPE_UBX);
 512     // send sensor type right now because on UBX NEMA we don't get a full set of messages
 513     // and we want to be able to see sensor type even on UBX NEMA GPS's
 514     GPSPositionSensorSensorTypeSet((uint8_t *)&ubxSensorType);
 515 }
 516
 517 static void parse_ubx_op_sys(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition)
 518 {
 519     struct UBX_OP_SYSINFO *sysinfo = &ubx->payload.op_sysinfo;
 520     GPSExtendedStatusData data;
 521
 522     data.FlightTime   = sysinfo->flightTime;
 523     data.BoardType[0] = sysinfo->board_type;
 524     data.BoardType[1] = sysinfo->board_revision;
 525     memcpy(&data.FirmwareHash, &sysinfo->sha1sum, GPSEXTENDEDSTATUS_FIRMWAREHASH_NUMELEM);
 526     memcpy(&data.FirmwareTag, &sysinfo->commit_tag_name, GPSEXTENDEDSTATUS_FIRMWARETAG_NUMELEM);
 527     data.Options = sysinfo->options;
 528     data.Status  = GPSEXTENDEDSTATUS_STATUS_GPSV9;
 529     GPSExtendedStatusSet(&data);
 530 }
 531
 532 static void parse_ubx_op_mag(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition)
 533 {
 534     if (!useMag) {
 535         return;
 536     }
 537     struct UBX_OP_MAG *mag = &ubx->payload.op_mag;
 538     float mags[3] = { mag->x, mag->y, mag->z };
 539     auxmagsupport_publish_samples(mags, AUXMAGSENSOR_STATUS_OK);
 540 }
 541 #endif /* if !defined(PIOS_GPS_MINIMAL) */
 542
 543 // UBX message parser
 544 // returns UAVObjectID if a UAVObject structure is ready for further processing
 545 uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition)
 546 {
 547     uint32_t id = 0;
 548     static bool ubxInitialized = false;
 549
 550     if (!ubxInitialized) {
 551         // initialize dop values. If no DOP sentence is received it is safer to initialize them to a high value rather than 0.
 552         GpsPosition->HDOP = 99.99f;
 553         GpsPosition->PDOP = 99.99f;
 554         GpsPosition->VDOP = 99.99f;
 555         ubxInitialized    = true;
 556     }
 557     // is it using PVT?
 558     usePvt = (lastPvtTime) && (PIOS_DELAY_GetuSSince(lastPvtTime) < UBX_PVT_TIMEOUT * 1000);
 559     for (uint8_t i = 0; i < UBX_HANDLER_TABLE_SIZE; i++) {
 560         const ubx_message_handler *handler = &ubx_handler_table[i];
 561         if (handler->msgClass == ubx->header.class && handler->msgID == ubx->header.id) {
 562             handler->handler(ubx, GpsPosition);
 563             break;
 564         }
 565     }
 566
 567     GpsPosition->SensorType = ubxSensorType;
 568
 569     if (msgtracker.msg_received == ALL_RECEIVED) {
 570         // leave BaudRate field alone!
 571         GPSPositionSensorBaudRateGet(&GpsPosition->BaudRate);
 572         GPSPositionSensorSet(GpsPosition);
 573         msgtracker.msg_received = NONE_RECEIVED;
 574         id = GPSPOSITIONSENSOR_OBJID;
 575     } else {
 576         uint8_t status;
 577         GPSPositionSensorStatusGet(&status);
 578         if (status == GPSPOSITIONSENSOR_STATUS_NOGPS) {
 579             // Some ubx thing has been received so GPS is there
 580             //
 581             // OP GPSV9 will sometimes cause this NOFIX
 582             // because GPSV9 drops data which causes checksum errors which causes GPS.c to set the status to NOGPS
 583             // see OP GPSV9 comment in parse_ubx_stream() for further information
 584             status = GPSPOSITIONSENSOR_STATUS_NOFIX;
 585             GPSPositionSensorStatusSet(&status);
 586         }
 587     }
 588     return id;
 589 }
 590
 591 #if !defined(PIOS_GPS_MINIMAL)
 592 void op_gpsv9_load_mag_settings()
 593 {
 594     if (auxmagsupport_get_type() == AUXMAGSETTINGS_TYPE_GPSV9) {
 595         useMag = true;
 596     } else {
 597         useMag = false;
 598     }
 599 }
 600 #endif // !defined(PIOS_GPS_MINIMAL)
 601 #endif // defined(PIOS_INCLUDE_GPS_UBX_PARSER)