src/main/io/gps.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <ctype.h>
  24 #include <string.h>
  25 #include <math.h>
  26
  27 #include "platform.h"
  28
  29 #ifdef USE_GPS
  30
  31 #include "build/build_config.h"
  32 #include "build/debug.h"
  33
  34 #include "common/axis.h"
  35 #include "common/gps_conversion.h"
  36 #include "common/maths.h"
  37 #include "common/utils.h"
  38
  39 #include "config/feature.h"
  40 #include "pg/pg.h"
  41 #include "pg/pg_ids.h"
  42
  43 #include "drivers/light_led.h"
  44 #include "drivers/time.h"
  45
  46 #include "io/dashboard.h"
  47 #include "io/gps.h"
  48 #include "io/serial.h"
  49
  50 #include "config/config.h"
  51 #include "fc/runtime_config.h"
  52
  53 #include "flight/imu.h"
  54 #include "flight/pid.h"
  55 #include "flight/gps_rescue.h"
  56
  57 #include "scheduler/scheduler.h"
  58
  59 #include "sensors/sensors.h"
  60
  61 #define LOG_ERROR        '?'
  62 #define LOG_IGNORED      '!'
  63 #define LOG_SKIPPED      '>'
  64 #define LOG_NMEA_GGA     'g'
  65 #define LOG_NMEA_RMC     'r'
  66 #define LOG_UBLOX_SOL    'O'
  67 #define LOG_UBLOX_STATUS 'S'
  68 #define LOG_UBLOX_SVINFO 'I'
  69 #define LOG_UBLOX_POSLLH 'P'
  70 #define LOG_UBLOX_VELNED 'V'
  71
  72 #define DEBUG_SERIAL_BAUD  0 // set to 1 to debug serial port baud config (/100)
  73 #define DEBUG_UBLOX_INIT   0 // set to 1 to debug ublox initialization
  74 #define DEBUG_UBLOX_FRAMES 0 // set to 1 to debug ublox received frames
  75
  76 char gpsPacketLog[GPS_PACKET_LOG_ENTRY_COUNT];
  77 static char *gpsPacketLogChar = gpsPacketLog;
  78 // **********************
  79 // GPS
  80 // **********************
  81 int32_t GPS_home[2];
  82 uint16_t GPS_distanceToHome;        // distance to home point in meters
  83 int16_t GPS_directionToHome;        // direction to home or hol point in degrees
  84 uint32_t GPS_distanceFlownInCm;     // distance flown since armed in centimeters
  85 int16_t GPS_verticalSpeedInCmS;     // vertical speed in cm/s
  86 float dTnav;             // Delta Time in milliseconds for navigation computations, updated with every good GPS read
  87 int16_t nav_takeoff_bearing;
  88
  89 #define GPS_DISTANCE_FLOWN_MIN_SPEED_THRESHOLD_CM_S 15 // 5.4Km/h 3.35mph
  90
  91 gpsSolutionData_t gpsSol;
  92 uint32_t GPS_packetCount = 0;
  93 uint32_t GPS_svInfoReceivedCount = 0; // SV = Space Vehicle, counter increments each time SV info is received.
  94 uint8_t GPS_update = 0;             // toogle to distinct a GPS position update (directly or via MSP)
  95
  96 uint8_t GPS_numCh;                              // Details on numCh/svinfo in gps.h
  97 uint8_t GPS_svinfo_chn[GPS_SV_MAXSATS_M8N];
  98 uint8_t GPS_svinfo_svid[GPS_SV_MAXSATS_M8N];
  99 uint8_t GPS_svinfo_quality[GPS_SV_MAXSATS_M8N];
 100 uint8_t GPS_svinfo_cno[GPS_SV_MAXSATS_M8N];
 101
 102 // GPS timeout for wrong baud rate/disconnection/etc in milliseconds (default 2.5second)
 103 #define GPS_TIMEOUT (2500)
 104 // How many entries in gpsInitData array below
 105 #define GPS_INIT_ENTRIES (GPS_BAUDRATE_MAX + 1)
 106 #define GPS_BAUDRATE_CHANGE_DELAY (200)
 107 // Timeout for waiting ACK/NAK in GPS task cycles (0.25s at 100Hz)
 108 #define UBLOX_ACK_TIMEOUT_MAX_COUNT (25)
 109
 110 static serialPort_t *gpsPort;
 111
 112 typedef struct gpsInitData_s {
 113     uint8_t index;
 114     uint8_t baudrateIndex; // see baudRate_e
 115     const char *ubx;
 116     const char *mtk;
 117 } gpsInitData_t;
 118
 119 // NMEA will cycle through these until valid data is received
 120 static const gpsInitData_t gpsInitData[] = {
 121     { GPS_BAUDRATE_115200,  BAUD_115200, "$PUBX,41,1,0003,0001,115200,0*1E\r\n", "$PMTK251,115200*1F\r\n" },
 122     { GPS_BAUDRATE_57600,    BAUD_57600, "$PUBX,41,1,0003,0001,57600,0*2D\r\n", "$PMTK251,57600*2C\r\n" },
 123     { GPS_BAUDRATE_38400,    BAUD_38400, "$PUBX,41,1,0003,0001,38400,0*26\r\n", "$PMTK251,38400*27\r\n" },
 124     { GPS_BAUDRATE_19200,    BAUD_19200, "$PUBX,41,1,0003,0001,19200,0*23\r\n", "$PMTK251,19200*22\r\n" },
 125     // 9600 is not enough for 5Hz updates - leave for compatibility to dumb NMEA that only runs at this speed
 126     { GPS_BAUDRATE_9600,      BAUD_9600, "$PUBX,41,1,0003,0001,9600,0*16\r\n", "" }
 127 };
 128
 129 #define GPS_INIT_DATA_ENTRY_COUNT (sizeof(gpsInitData) / sizeof(gpsInitData[0]))
 130
 131 #define DEFAULT_BAUD_RATE_INDEX 0
 132
 133 #ifdef USE_GPS_UBLOX
 134 enum {
 135     PREAMBLE1 = 0xB5,
 136     PREAMBLE2 = 0x62,
 137     CLASS_NAV = 0x01,
 138     CLASS_ACK = 0x05,
 139     CLASS_CFG = 0x06,
 140     MSG_ACK_NACK = 0x00,
 141     MSG_ACK_ACK = 0x01,
 142     MSG_POSLLH = 0x2,
 143     MSG_STATUS = 0x3,
 144     MSG_SOL = 0x6,
 145     MSG_PVT = 0x7,
 146     MSG_VELNED = 0x12,
 147     MSG_SVINFO = 0x30,
 148     MSG_SAT = 0x35,
 149     MSG_CFG_MSG = 0x1,
 150     MSG_CFG_PRT = 0x00,
 151     MSG_CFG_RATE = 0x08,
 152     MSG_CFG_SET_RATE = 0x01,
 153     MSG_CFG_SBAS = 0x16,
 154     MSG_CFG_NAV_SETTINGS = 0x24,
 155     MSG_CFG_GNSS = 0x3E
 156 } ubx_protocol_bytes;
 157
 158 #define UBLOX_MODE_ENABLED    0x1
 159 #define UBLOX_MODE_TEST       0x2
 160
 161 #define UBLOX_USAGE_RANGE     0x1
 162 #define UBLOX_USAGE_DIFFCORR  0x2
 163 #define UBLOX_USAGE_INTEGRITY 0x4
 164
 165 #define UBLOX_GNSS_ENABLE     0x1
 166 #define UBLOX_GNSS_DEFAULT_SIGCFGMASK 0x10000
 167
 168 #define UBLOX_DYNMODE_PEDESTRIAN  3
 169 #define UBLOX_DYNMODE_AIRBORNE_1G 6
 170 #define UBLOX_DYNMODE_AIRBORNE_4G 8
 171
 172 typedef struct {
 173     uint8_t preamble1;
 174     uint8_t preamble2;
 175     uint8_t msg_class;
 176     uint8_t msg_id;
 177     uint16_t length;
 178 } ubx_header;
 179
 180 typedef struct {
 181     uint8_t gnssId;
 182     uint8_t resTrkCh;
 183     uint8_t maxTrkCh;
 184     uint8_t reserved1;
 185     uint32_t flags;
 186 } ubx_configblock;
 187
 188 typedef struct {
 189     uint8_t msgClass;
 190     uint8_t msgID;
 191 } ubx_poll_msg;
 192
 193 typedef struct {
 194     uint8_t msgClass;
 195     uint8_t msgID;
 196     uint8_t rate;
 197 } ubx_cfg_msg;
 198
 199 typedef struct {
 200     uint16_t measRate;
 201     uint16_t navRate;
 202     uint16_t timeRef;
 203 } ubx_cfg_rate;
 204
 205 typedef struct {
 206     uint8_t mode;
 207     uint8_t usage;
 208     uint8_t maxSBAS;
 209     uint8_t scanmode2;
 210     uint32_t scanmode1;
 211 } ubx_cfg_sbas;
 212
 213 typedef struct {
 214     uint8_t msgVer;
 215     uint8_t numTrkChHw;
 216     uint8_t numTrkChUse;
 217     uint8_t numConfigBlocks;
 218     ubx_configblock configblocks[7];
 219 } ubx_cfg_gnss;
 220
 221 typedef struct {
 222     uint16_t mask;
 223     uint8_t dynModel;
 224     uint8_t fixMode;
 225     int32_t fixedAlt;
 226     uint32_t fixedAltVar;
 227     int8_t minElev;
 228     uint8_t drLimit;
 229     uint16_t pDOP;
 230     uint16_t tDOP;
 231     uint16_t pAcc;
 232     uint16_t tAcc;
 233     uint8_t staticHoldThresh;
 234     uint8_t dgnssTimeout;
 235     uint8_t cnoThreshNumSVs;
 236     uint8_t cnoThresh;
 237     uint8_t reserved0[2];
 238     uint16_t staticHoldMaxDist;
 239     uint8_t utcStandard;
 240     uint8_t reserved1[5];
 241 } ubx_cfg_nav5;
 242
 243 typedef union {
 244     ubx_poll_msg poll_msg;
 245     ubx_cfg_msg cfg_msg;
 246     ubx_cfg_rate cfg_rate;
 247     ubx_cfg_nav5 cfg_nav5;
 248     ubx_cfg_sbas cfg_sbas;
 249     ubx_cfg_gnss cfg_gnss;
 250 } ubx_payload;
 251
 252 typedef struct {
 253     ubx_header header;
 254     ubx_payload payload;
 255 } __attribute__((packed)) ubx_message;
 256
 257 #endif // USE_GPS_UBLOX
 258
 259 typedef enum {
 260     GPS_STATE_UNKNOWN,
 261     GPS_STATE_INITIALIZING,
 262     GPS_STATE_INITIALIZED,
 263     GPS_STATE_CHANGE_BAUD,
 264     GPS_STATE_CONFIGURE,
 265     GPS_STATE_RECEIVING_DATA,
 266     GPS_STATE_LOST_COMMUNICATION,
 267     GPS_STATE_COUNT
 268 } gpsState_e;
 269
 270 // Max time to wait for received data
 271 #define GPS_MAX_WAIT_DATA_RX 30
 272
 273 gpsData_t gpsData;
 274
 275 PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0);
 276
 277 PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig,
 278     .provider = GPS_NMEA,
 279     .sbasMode = SBAS_NONE,
 280     .autoConfig = GPS_AUTOCONFIG_ON,
 281     .autoBaud = GPS_AUTOBAUD_OFF,
 282     .gps_ublox_use_galileo = false,
 283     .gps_ublox_mode = UBLOX_AIRBORNE,
 284     .gps_set_home_point_once = false,
 285     .gps_use_3d_speed = false,
 286     .sbas_integrity = false
 287 );
 288
 289 static void shiftPacketLog(void)
 290 {
 291     uint32_t i;
 292
 293     for (i = ARRAYLEN(gpsPacketLog) - 1; i > 0 ; i--) {
 294         gpsPacketLog[i] = gpsPacketLog[i-1];
 295     }
 296 }
 297
 298 static bool isConfiguratorConnected() {
 299     return (getArmingDisableFlags() & ARMING_DISABLED_MSP);
 300 }
 301
 302 static void gpsNewData(uint16_t c);
 303 #ifdef USE_GPS_NMEA
 304 static bool gpsNewFrameNMEA(char c);
 305 #endif
 306 #ifdef USE_GPS_UBLOX
 307 static bool gpsNewFrameUBLOX(uint8_t data);
 308 #endif
 309
 310 static void gpsSetState(gpsState_e state)
 311 {
 312     gpsData.lastMessage = millis();
 313     sensorsClear(SENSOR_GPS);
 314
 315     gpsData.state = state;
 316     gpsData.state_position = 0;
 317     gpsData.state_ts = millis();
 318     gpsData.ackState = UBLOX_ACK_IDLE;
 319 }
 320
 321 void gpsInit(void)
 322 {
 323     gpsData.baudrateIndex = 0;
 324     gpsData.errors = 0;
 325     gpsData.timeouts = 0;
 326
 327     memset(gpsPacketLog, 0x00, sizeof(gpsPacketLog));
 328
 329     // init gpsData structure. if we're not actually enabled, don't bother doing anything else
 330     gpsSetState(GPS_STATE_UNKNOWN);
 331
 332     gpsData.lastMessage = millis();
 333
 334     if (gpsConfig()->provider == GPS_MSP) { // no serial ports used when GPS_MSP is configured
 335         gpsSetState(GPS_STATE_INITIALIZED);
 336         return;
 337     }
 338
 339     const serialPortConfig_t *gpsPortConfig = findSerialPortConfig(FUNCTION_GPS);
 340     if (!gpsPortConfig) {
 341         return;
 342     }
 343
 344     while (gpsInitData[gpsData.baudrateIndex].baudrateIndex != gpsPortConfig->gps_baudrateIndex) {
 345         gpsData.baudrateIndex++;
 346         if (gpsData.baudrateIndex >= GPS_INIT_DATA_ENTRY_COUNT) {
 347             gpsData.baudrateIndex = DEFAULT_BAUD_RATE_INDEX;
 348             break;
 349         }
 350     }
 351
 352     portMode_e mode = MODE_RXTX;
 353 #if defined(GPS_NMEA_TX_ONLY)
 354     if (gpsConfig()->provider == GPS_NMEA) {
 355         mode &= ~MODE_TX;
 356     }
 357 #endif
 358
 359     // no callback - buffer will be consumed in gpsUpdate()
 360     gpsPort = openSerialPort(gpsPortConfig->identifier, FUNCTION_GPS, NULL, NULL, baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex], mode, SERIAL_NOT_INVERTED);
 361     if (!gpsPort) {
 362         return;
 363     }
 364
 365     // signal GPS "thread" to initialize when it gets to it
 366     gpsSetState(GPS_STATE_INITIALIZING);
 367 }
 368
 369 #ifdef USE_GPS_NMEA
 370 void gpsInitNmea(void)
 371 {
 372 #if !defined(GPS_NMEA_TX_ONLY)
 373     uint32_t now;
 374 #endif
 375     switch (gpsData.state) {
 376         case GPS_STATE_INITIALIZING:
 377 #if !defined(GPS_NMEA_TX_ONLY)
 378            now = millis();
 379            if (now - gpsData.state_ts < 1000) {
 380                return;
 381            }
 382            gpsData.state_ts = now;
 383            if (gpsData.state_position < 1) {
 384                serialSetBaudRate(gpsPort, 4800);
 385                gpsData.state_position++;
 386            } else if (gpsData.state_position < 2) {
 387                // print our FIXED init string for the baudrate we want to be at
 388                serialPrint(gpsPort, "$PSRF100,1,115200,8,1,0*05\r\n");
 389                gpsData.state_position++;
 390            } else {
 391                // we're now (hopefully) at the correct rate, next state will switch to it
 392                gpsSetState(GPS_STATE_CHANGE_BAUD);
 393            }
 394            break;
 395 #endif
 396         case GPS_STATE_CHANGE_BAUD:
 397 #if !defined(GPS_NMEA_TX_ONLY)
 398            now = millis();
 399            if (now - gpsData.state_ts < 1000) {
 400                return;
 401            }
 402            gpsData.state_ts = now;
 403            if (gpsData.state_position < 1) {
 404                serialSetBaudRate(gpsPort, baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex]);
 405                gpsData.state_position++;
 406            } else if (gpsData.state_position < 2) {
 407                serialPrint(gpsPort, "$PSRF103,00,6,00,0*23\r\n");
 408                gpsData.state_position++;
 409            } else
 410 #else
 411            {
 412                serialSetBaudRate(gpsPort, baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex]);
 413            }
 414 #endif
 415                gpsSetState(GPS_STATE_RECEIVING_DATA);
 416             break;
 417     }
 418 }
 419 #endif // USE_GPS_NMEA
 420
 421 #ifdef USE_GPS_UBLOX
 422 static void ubloxSendByteUpdateChecksum(const uint8_t data, uint8_t *checksumA, uint8_t *checksumB)
 423 {
 424     *checksumA += data;
 425     *checksumB += *checksumA;
 426     serialWrite(gpsPort, data);
 427 }
 428
 429 static void ubloxSendDataUpdateChecksum(const uint8_t *data, uint8_t len, uint8_t *checksumA, uint8_t *checksumB)
 430 {
 431     while (len--) {
 432         ubloxSendByteUpdateChecksum(*data, checksumA, checksumB);
 433         data++;
 434     }
 435 }
 436
 437 static void ubloxSendMessage(const uint8_t *data, uint8_t len)
 438 {
 439     uint8_t checksumA = 0, checksumB = 0;
 440     serialWrite(gpsPort, data[0]);
 441     serialWrite(gpsPort, data[1]);
 442     ubloxSendDataUpdateChecksum(&data[2], len - 2, &checksumA, &checksumB);
 443     serialWrite(gpsPort, checksumA);
 444     serialWrite(gpsPort, checksumB);
 445
 446     // Save state for ACK waiting
 447     gpsData.ackWaitingMsgId = data[3]; //save message id for ACK
 448     gpsData.ackTimeoutCounter = 0;
 449     gpsData.ackState = UBLOX_ACK_WAITING;
 450 }
 451
 452 static void ubloxSendConfigMessage(ubx_message *message, uint8_t msg_id, uint8_t length)
 453 {
 454     message->header.preamble1 = PREAMBLE1;
 455     message->header.preamble2 = PREAMBLE2;
 456     message->header.msg_class = CLASS_CFG;
 457     message->header.msg_id = msg_id;
 458     message->header.length = length;
 459     ubloxSendMessage((const uint8_t *) message, length + 6);
 460 }
 461
 462 static void ubloxSendPollMessage(uint8_t msg_id)
 463 {
 464     ubx_message tx_buffer;
 465     tx_buffer.header.preamble1 = PREAMBLE1;
 466     tx_buffer.header.preamble2 = PREAMBLE2;
 467     tx_buffer.header.msg_class = CLASS_CFG;
 468     tx_buffer.header.msg_id = msg_id;
 469     tx_buffer.header.length = 0;
 470     ubloxSendMessage((const uint8_t *) &tx_buffer, 6);
 471 }
 472
 473 static void ubloxSendNAV5Message(bool airborne) {
 474     ubx_message tx_buffer;
 475     tx_buffer.payload.cfg_nav5.mask = 0xFFFF;
 476     if (airborne) {
 477 #if defined(GPS_UBLOX_MODE_AIRBORNE_1G)
 478         tx_buffer.payload.cfg_nav5.dynModel = UBLOX_DYNMODE_AIRBORNE_1G;
 479 #else
 480         tx_buffer.payload.cfg_nav5.dynModel = UBLOX_DYNMODE_AIRBORNE_4G;
 481 #endif
 482     } else {
 483         tx_buffer.payload.cfg_nav5.dynModel = UBLOX_DYNMODE_PEDESTRIAN;
 484     }
 485     tx_buffer.payload.cfg_nav5.fixMode = 3;
 486     tx_buffer.payload.cfg_nav5.fixedAlt = 0;
 487     tx_buffer.payload.cfg_nav5.fixedAltVar = 10000;
 488     tx_buffer.payload.cfg_nav5.minElev = 5;
 489     tx_buffer.payload.cfg_nav5.drLimit = 0;
 490     tx_buffer.payload.cfg_nav5.pDOP = 250;
 491     tx_buffer.payload.cfg_nav5.tDOP = 250;
 492     tx_buffer.payload.cfg_nav5.pAcc = 100;
 493     tx_buffer.payload.cfg_nav5.tAcc = 300;
 494     tx_buffer.payload.cfg_nav5.staticHoldThresh = 0;
 495     tx_buffer.payload.cfg_nav5.dgnssTimeout = 60;
 496     tx_buffer.payload.cfg_nav5.cnoThreshNumSVs = 0;
 497     tx_buffer.payload.cfg_nav5.cnoThresh = 0;
 498     tx_buffer.payload.cfg_nav5.reserved0[0] = 0;
 499     tx_buffer.payload.cfg_nav5.reserved0[1] = 0;
 500     tx_buffer.payload.cfg_nav5.staticHoldMaxDist = 200;
 501     tx_buffer.payload.cfg_nav5.utcStandard = 0;
 502     tx_buffer.payload.cfg_nav5.reserved1[0] = 0;
 503     tx_buffer.payload.cfg_nav5.reserved1[1] = 0;
 504     tx_buffer.payload.cfg_nav5.reserved1[2] = 0;
 505     tx_buffer.payload.cfg_nav5.reserved1[3] = 0;
 506     tx_buffer.payload.cfg_nav5.reserved1[4] = 0;
 507
 508     ubloxSendConfigMessage(&tx_buffer, MSG_CFG_NAV_SETTINGS, sizeof(ubx_cfg_nav5));
 509 }
 510
 511 static void ubloxSetMessageRate(uint8_t messageClass, uint8_t messageID, uint8_t rate) {
 512     ubx_message tx_buffer;
 513     tx_buffer.payload.cfg_msg.msgClass = messageClass;
 514     tx_buffer.payload.cfg_msg.msgID = messageID;
 515     tx_buffer.payload.cfg_msg.rate = rate;
 516     ubloxSendConfigMessage(&tx_buffer, MSG_CFG_MSG, sizeof(ubx_cfg_msg));
 517 }
 518
 519 static void ubloxSetNavRate(uint16_t measRate, uint16_t navRate, uint16_t timeRef) {
 520     ubx_message tx_buffer;
 521     tx_buffer.payload.cfg_rate.measRate = measRate;
 522     tx_buffer.payload.cfg_rate.navRate = navRate;
 523     tx_buffer.payload.cfg_rate.timeRef = timeRef;
 524     ubloxSendConfigMessage(&tx_buffer, MSG_CFG_RATE, sizeof(ubx_cfg_rate));
 525 }
 526
 527 static void ubloxSetSbas() {
 528     ubx_message tx_buffer;
 529
 530     //NOTE: default ublox config for sbas mode is: UBLOX_MODE_ENABLED, test is disabled
 531     tx_buffer.payload.cfg_sbas.mode = UBLOX_MODE_TEST;
 532     if (gpsConfig()->sbasMode != SBAS_NONE) {
 533         tx_buffer.payload.cfg_sbas.mode |= UBLOX_MODE_ENABLED;
 534     }
 535
 536     //NOTE: default ublox config for sbas mode is: UBLOX_USAGE_RANGE | UBLOX_USAGE_DIFFCORR, integrity is disabled
 537     tx_buffer.payload.cfg_sbas.usage = UBLOX_USAGE_RANGE | UBLOX_USAGE_DIFFCORR;
 538     if (gpsConfig()->sbas_integrity) {
 539         tx_buffer.payload.cfg_sbas.usage |= UBLOX_USAGE_INTEGRITY;
 540     }
 541
 542     tx_buffer.payload.cfg_sbas.maxSBAS = 3;
 543     tx_buffer.payload.cfg_sbas.scanmode2 = 0;
 544     switch (gpsConfig()->sbasMode) {
 545         case SBAS_AUTO:
 546             tx_buffer.payload.cfg_sbas.scanmode1 = 0;
 547             break;
 548         case SBAS_EGNOS:
 549             tx_buffer.payload.cfg_sbas.scanmode1 = 0x00010048; //PRN123, PRN126, PRN136
 550             break;
 551         case SBAS_WAAS:
 552             tx_buffer.payload.cfg_sbas.scanmode1 = 0x0004A800; //PRN131, PRN133, PRN135, PRN138
 553             break;
 554         case SBAS_MSAS:
 555             tx_buffer.payload.cfg_sbas.scanmode1 = 0x00020200; //PRN129, PRN137
 556             break;
 557         case SBAS_GAGAN:
 558             tx_buffer.payload.cfg_sbas.scanmode1 = 0x00001180; //PRN127, PRN128, PRN132
 559             break;
 560         default:
 561             tx_buffer.payload.cfg_sbas.scanmode1 = 0;
 562             break;
 563     }
 564     ubloxSendConfigMessage(&tx_buffer, MSG_CFG_SBAS, sizeof(ubx_cfg_sbas));
 565 }
 566
 567 void gpsInitUblox(void)
 568 {
 569     uint32_t now;
 570     // UBX will run at the serial port's baudrate, it shouldn't be "autodetected". So here we force it to that rate
 571
 572     // Wait until GPS transmit buffer is empty
 573     if (!isSerialTransmitBufferEmpty(gpsPort))
 574         return;
 575
 576     switch (gpsData.state) {
 577         case GPS_STATE_INITIALIZING:
 578             now = millis();
 579             if (now - gpsData.state_ts < GPS_BAUDRATE_CHANGE_DELAY)
 580                 return;
 581
 582             if (gpsData.state_position < GPS_INIT_ENTRIES) {
 583                 // try different speed to INIT
 584                 baudRate_e newBaudRateIndex = gpsInitData[gpsData.state_position].baudrateIndex;
 585
 586                 gpsData.state_ts = now;
 587
 588                 if (lookupBaudRateIndex(serialGetBaudRate(gpsPort)) != newBaudRateIndex) {
 589                     // change the rate if needed and wait a little
 590                     serialSetBaudRate(gpsPort, baudRates[newBaudRateIndex]);
 591 #if DEBUG_SERIAL_BAUD
 592                     debug[0] = baudRates[newBaudRateIndex] / 100;
 593 #endif
 594                     return;
 595                 }
 596
 597                 // print our FIXED init string for the baudrate we want to be at
 598                 serialPrint(gpsPort, gpsInitData[gpsData.baudrateIndex].ubx);
 599
 600                 gpsData.state_position++;
 601             } else {
 602                 // we're now (hopefully) at the correct rate, next state will switch to it
 603                 gpsSetState(GPS_STATE_CHANGE_BAUD);
 604             }
 605             break;
 606
 607         case GPS_STATE_CHANGE_BAUD:
 608             serialSetBaudRate(gpsPort, baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex]);
 609 #if DEBUG_SERIAL_BAUD
 610             debug[0] = baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex] / 100;
 611 #endif
 612             gpsSetState(GPS_STATE_CONFIGURE);
 613             break;
 614
 615         case GPS_STATE_CONFIGURE:
 616             // Either use specific config file for GPS or let dynamically upload config
 617             if (gpsConfig()->autoConfig == GPS_AUTOCONFIG_OFF) {
 618                 gpsSetState(GPS_STATE_RECEIVING_DATA);
 619                 break;
 620             }
 621
 622             if (gpsData.ackState == UBLOX_ACK_IDLE) {
 623                 switch (gpsData.state_position) {
 624                     case 0:
 625                         gpsData.ubloxUsePVT = true;
 626                         gpsData.ubloxUseSAT = true;
 627                         ubloxSendNAV5Message(gpsConfig()->gps_ublox_mode == UBLOX_AIRBORNE);
 628                         break;
 629                     case 1: //Disable NMEA Messages
 630                         ubloxSetMessageRate(0xF0, 0x05, 0); // VGS: Course over ground and Ground speed
 631                         break;
 632                     case 2:
 633                         ubloxSetMessageRate(0xF0, 0x03, 0); // GSV: GNSS Satellites in View
 634                         break;
 635                     case 3:
 636                         ubloxSetMessageRate(0xF0, 0x01, 0); // GLL: Latitude and longitude, with time of position fix and status
 637                         break;
 638                     case 4:
 639                         ubloxSetMessageRate(0xF0, 0x00, 0); // GGA: Global positioning system fix data
 640                         break;
 641                     case 5:
 642                         ubloxSetMessageRate(0xF0, 0x02, 0); // GSA: GNSS DOP and Active Satellites
 643                         break;
 644                     case 6:
 645                         ubloxSetMessageRate(0xF0, 0x04, 0); // RMC: Recommended Minimum data
 646                         break;
 647                     case 7: //Enable UBLOX Messages
 648                         if (gpsData.ubloxUsePVT) {
 649                             ubloxSetMessageRate(CLASS_NAV, MSG_PVT, 1); // set PVT MSG rate
 650                         } else {
 651                             ubloxSetMessageRate(CLASS_NAV, MSG_SOL, 1); // set SOL MSG rate
 652                         }
 653                         break;
 654                     case 8:
 655                         if (gpsData.ubloxUsePVT) {
 656                             gpsData.state_position++;
 657                         } else {
 658                            ubloxSetMessageRate(CLASS_NAV, MSG_POSLLH, 1); // set POSLLH MSG rate
 659                         }
 660                         break;
 661                     case 9:
 662                         if (gpsData.ubloxUsePVT) {
 663                             gpsData.state_position++;
 664                         } else {
 665                             ubloxSetMessageRate(CLASS_NAV, MSG_STATUS, 1); // set STATUS MSG rate
 666                         }
 667                         break;
 668                     case 10:
 669                         if (gpsData.ubloxUsePVT) {
 670                             gpsData.state_position++;
 671                         } else {
 672                             ubloxSetMessageRate(CLASS_NAV, MSG_VELNED, 1); // set VELNED MSG rate
 673                         }
 674                         break;
 675                     case 11:
 676                         if (gpsData.ubloxUseSAT) {
 677                             ubloxSetMessageRate(CLASS_NAV, MSG_SAT, 5); // set SAT MSG rate (every 5 cycles)
 678                         } else {
 679                             ubloxSetMessageRate(CLASS_NAV, MSG_SVINFO, 5); // set SVINFO MSG rate (every 5 cycles)
 680                         }
 681                         break;
 682                     case 12:
 683                         ubloxSetNavRate(0xC8, 1, 1); // set rate to 5Hz (measurement period: 200ms, navigation rate: 1 cycle)
 684                         break;
 685                     case 13:
 686                         ubloxSetSbas();
 687                         break;
 688                     case 14:
 689                         if ((gpsConfig()->sbasMode == SBAS_NONE) || (gpsConfig()->gps_ublox_use_galileo)) {
 690                             ubloxSendPollMessage(MSG_CFG_GNSS);
 691                         } else {
 692                             gpsSetState(GPS_STATE_RECEIVING_DATA);
 693                         }
 694                         break;
 695                     default:
 696                         break;
 697                 }
 698             }
 699
 700             switch (gpsData.ackState) {
 701                 case UBLOX_ACK_IDLE:
 702                     break;
 703                 case UBLOX_ACK_WAITING:
 704                     if ((++gpsData.ackTimeoutCounter) == UBLOX_ACK_TIMEOUT_MAX_COUNT) {
 705                         gpsSetState(GPS_STATE_LOST_COMMUNICATION);
 706                     }
 707                     break;
 708                 case UBLOX_ACK_GOT_ACK:
 709                     if (gpsData.state_position == 14) {
 710                         // ublox should be initialised, try receiving
 711                         gpsSetState(GPS_STATE_RECEIVING_DATA);
 712                     } else {
 713                         gpsData.state_position++;
 714                         gpsData.ackState = UBLOX_ACK_IDLE;
 715                     }
 716                     break;
 717                 case UBLOX_ACK_GOT_NACK:
 718                     if (gpsData.state_position == 7) { // If we were asking for NAV-PVT...
 719                         gpsData.ubloxUsePVT = false;   // ...retry asking for NAV-SOL
 720                         gpsData.ackState = UBLOX_ACK_IDLE;
 721                     } else {
 722                         if (gpsData.state_position == 11) { // If we were asking for NAV-SAT...
 723                             gpsData.ubloxUseSAT = false;   // ...retry asking for NAV-SVINFO
 724                             gpsData.ackState = UBLOX_ACK_IDLE;
 725                         } else {
 726                             gpsSetState(GPS_STATE_CONFIGURE);
 727                         }
 728                     }
 729                     break;
 730             }
 731
 732             break;
 733     }
 734 }
 735 #endif // USE_GPS_UBLOX
 736
 737 void gpsInitHardware(void)
 738 {
 739     switch (gpsConfig()->provider) {
 740     case GPS_NMEA:
 741 #ifdef USE_GPS_NMEA
 742         gpsInitNmea();
 743 #endif
 744         break;
 745
 746     case GPS_UBLOX:
 747 #ifdef USE_GPS_UBLOX
 748         gpsInitUblox();
 749 #endif
 750         break;
 751     default:
 752         break;
 753     }
 754 }
 755
 756 static void updateGpsIndicator(timeUs_t currentTimeUs)
 757 {
 758     static uint32_t GPSLEDTime;
 759     if ((int32_t)(currentTimeUs - GPSLEDTime) >= 0 && (gpsSol.numSat >= 5)) {
 760         GPSLEDTime = currentTimeUs + 150000;
 761         LED1_TOGGLE;
 762     }
 763 }
 764
 765 void gpsUpdate(timeUs_t currentTimeUs)
 766 {
 767     static gpsState_e gpsStateDurationUs[GPS_STATE_COUNT];
 768     timeUs_t executeTimeUs;
 769     gpsState_e gpsCurState = gpsData.state;
 770
 771     // read out available GPS bytes
 772     if (gpsPort) {
 773         while (serialRxBytesWaiting(gpsPort)) {
 774             if (cmpTimeUs(micros(), currentTimeUs) > GPS_MAX_WAIT_DATA_RX) {
 775                 // Wait 1ms and come back
 776                 rescheduleTask(TASK_SELF, TASK_PERIOD_HZ(TASK_GPS_RATE_FAST));
 777                 return;
 778             }
 779             gpsNewData(serialRead(gpsPort));
 780         }
 781         // Restore default task rate
 782         rescheduleTask(TASK_SELF, TASK_PERIOD_HZ(TASK_GPS_RATE));
 783    } else if (GPS_update & GPS_MSP_UPDATE) { // GPS data received via MSP
 784         gpsSetState(GPS_STATE_RECEIVING_DATA);
 785         onGpsNewData();
 786         GPS_update &= ~GPS_MSP_UPDATE;
 787     }
 788
 789 #if DEBUG_UBLOX_INIT
 790     debug[0] = gpsData.state;
 791     debug[1] = gpsData.state_position;
 792     debug[2] = gpsData.ackState;
 793 #endif
 794
 795     switch (gpsData.state) {
 796         case GPS_STATE_UNKNOWN:
 797         case GPS_STATE_INITIALIZED:
 798             break;
 799
 800         case GPS_STATE_INITIALIZING:
 801         case GPS_STATE_CHANGE_BAUD:
 802         case GPS_STATE_CONFIGURE:
 803             gpsInitHardware();
 804             break;
 805
 806         case GPS_STATE_LOST_COMMUNICATION:
 807             gpsData.timeouts++;
 808             if (gpsConfig()->autoBaud) {
 809                 // try another rate
 810                 gpsData.baudrateIndex++;
 811                 gpsData.baudrateIndex %= GPS_INIT_ENTRIES;
 812             }
 813             gpsSol.numSat = 0;
 814             DISABLE_STATE(GPS_FIX);
 815             gpsSetState(GPS_STATE_INITIALIZING);
 816             break;
 817
 818         case GPS_STATE_RECEIVING_DATA:
 819             // check for no data/gps timeout/cable disconnection etc
 820             if (millis() - gpsData.lastMessage > GPS_TIMEOUT) {
 821                 gpsSetState(GPS_STATE_LOST_COMMUNICATION);
 822 #ifdef USE_GPS_UBLOX
 823             } else {
 824                 if (gpsConfig()->autoConfig == GPS_AUTOCONFIG_ON) { // Only if autoconfig is enabled
 825                     switch (gpsData.state_position) {
 826                         case 0:
 827                             if (!isConfiguratorConnected()) {
 828                                 if (gpsData.ubloxUseSAT) {
 829                                     ubloxSetMessageRate(CLASS_NAV, MSG_SAT, 0); // disable SAT MSG
 830                                 } else {
 831                                     ubloxSetMessageRate(CLASS_NAV, MSG_SVINFO, 0); // disable SVINFO MSG
 832                                 }
 833                                 gpsData.state_position = 1;
 834                             }
 835                             break;
 836                         case 1:
 837                             if (STATE(GPS_FIX) && (gpsConfig()->gps_ublox_mode == UBLOX_DYNAMIC)) {
 838                                 ubloxSendNAV5Message(true);
 839                                 gpsData.state_position = 2;
 840                             }
 841                             if (isConfiguratorConnected()) {
 842                                 gpsData.state_position = 2;
 843                             }
 844                             break;
 845                         case 2:
 846                             if (isConfiguratorConnected()) {
 847                                 if (gpsData.ubloxUseSAT) {
 848                                     ubloxSetMessageRate(CLASS_NAV, MSG_SAT, 5); // set SAT MSG rate (every 5 cycles)
 849                                 } else {
 850                                     ubloxSetMessageRate(CLASS_NAV, MSG_SVINFO, 5); // set SVINFO MSG rate (every 5 cycles)
 851                                 }
 852                                 gpsData.state_position = 0;
 853                             }
 854                             break;
 855                     }
 856                 }
 857 #endif //USE_GPS_UBLOX
 858             }
 859             break;
 860     }
 861
 862     executeTimeUs = micros() - currentTimeUs;
 863
 864     if (executeTimeUs > gpsStateDurationUs[gpsCurState]) {
 865         gpsStateDurationUs[gpsCurState] = executeTimeUs;
 866     }
 867     schedulerSetNextStateTime(gpsStateDurationUs[gpsData.state]);
 868
 869     if (sensors(SENSOR_GPS)) {
 870         updateGpsIndicator(currentTimeUs);
 871     }
 872     if (!ARMING_FLAG(ARMED) && !gpsConfig()->gps_set_home_point_once) {
 873         DISABLE_STATE(GPS_FIX_HOME);
 874     }
 875 #if defined(USE_GPS_RESCUE)
 876     if (gpsRescueIsConfigured()) {
 877         updateGPSRescueState();
 878     }
 879 #endif
 880 }
 881
 882 static void gpsNewData(uint16_t c)
 883 {
 884     if (!gpsNewFrame(c)) {
 885         return;
 886     }
 887
 888     if (gpsData.state == GPS_STATE_RECEIVING_DATA) {
 889         // new data received and parsed, we're in business
 890         gpsData.lastLastMessage = gpsData.lastMessage;
 891         gpsData.lastMessage = millis();
 892         sensorsSet(SENSOR_GPS);
 893     }
 894
 895     GPS_update ^= GPS_DIRECT_TICK;
 896
 897 #if DEBUG_UBLOX_INIT
 898     debug[3] = GPS_update;
 899 #endif
 900
 901     onGpsNewData();
 902 }
 903
 904 bool gpsNewFrame(uint8_t c)
 905 {
 906     switch (gpsConfig()->provider) {
 907     case GPS_NMEA:          // NMEA
 908 #ifdef USE_GPS_NMEA
 909         return gpsNewFrameNMEA(c);
 910 #endif
 911         break;
 912     case GPS_UBLOX:         // UBX binary
 913 #ifdef USE_GPS_UBLOX
 914         return gpsNewFrameUBLOX(c);
 915 #endif
 916         break;
 917     default:
 918         break;
 919     }
 920     return false;
 921 }
 922
 923 // Check for healthy communications
 924 bool gpsIsHealthy()
 925 {
 926     return (gpsData.state == GPS_STATE_RECEIVING_DATA);
 927 }
 928
 929 /* This is a light implementation of a GPS frame decoding
 930    This should work with most of modern GPS devices configured to output 5 frames.
 931    It assumes there are some NMEA GGA frames to decode on the serial bus
 932    Now verifies checksum correctly before applying data
 933
 934    Here we use only the following data :
 935      - latitude
 936      - longitude
 937      - GPS fix is/is not ok
 938      - GPS num sat (4 is enough to be +/- reliable)
 939      // added by Mis
 940      - GPS altitude (for OSD displaying)
 941      - GPS speed (for OSD displaying)
 942 */
 943
 944 #define NO_FRAME   0
 945 #define FRAME_GGA  1
 946 #define FRAME_RMC  2
 947 #define FRAME_GSV  3
 948
 949
 950 // This code is used for parsing NMEA data
 951
 952 /* Alex optimization
 953   The latitude or longitude is coded this way in NMEA frames
 954   dm.f   coded as degrees + minutes + minute decimal
 955   Where:
 956     - d can be 1 or more char long. generally: 2 char long for latitude, 3 char long for longitude
 957     - m is always 2 char long
 958     - f can be 1 or more char long
 959   This function converts this format in a unique unsigned long where 1 degree = 10 000 000
 960
 961   EOS increased the precision here, even if we think that the gps is not precise enough, with 10e5 precision it has 76cm resolution
 962   with 10e7 it's around 1 cm now. Increasing it further is irrelevant, since even 1cm resolution is unrealistic, however increased
 963   resolution also increased precision of nav calculations
 964 static uint32_t GPS_coord_to_degrees(char *coordinateString)
 965 {
 966     char *p = s, *d = s;
 967     uint8_t min, deg = 0;
 968     uint16_t frac = 0, mult = 10000;
 969
 970     while (*p) {                // parse the string until its end
 971         if (d != s) {
 972             frac += (*p - '0') * mult;  // calculate only fractional part on up to 5 digits  (d != s condition is true when the . is located)
 973             mult /= 10;
 974         }
 975         if (*p == '.')
 976             d = p;              // locate '.' char in the string
 977         p++;
 978     }
 979     if (p == s)
 980         return 0;
 981     while (s < d - 2) {
 982         deg *= 10;              // convert degrees : all chars before minutes ; for the first iteration, deg = 0
 983         deg += *(s++) - '0';
 984     }
 985     min = *(d - 1) - '0' + (*(d - 2) - '0') * 10;       // convert minutes : 2 previous char before '.'
 986     return deg * 10000000UL + (min * 100000UL + frac) * 10UL / 6;
 987 }
 988 */
 989
 990 // helper functions
 991 #ifdef USE_GPS_NMEA
 992 static uint32_t grab_fields(char *src, uint8_t mult)
 993 {                               // convert string to uint32
 994     uint32_t i;
 995     uint32_t tmp = 0;
 996     int isneg = 0;
 997     for (i = 0; src[i] != 0; i++) {
 998         if ((i == 0) && (src[0] == '-')) { // detect negative sign
 999             isneg = 1;
1000             continue; // jump to next character if the first one was a negative sign
1001         }
1002         if (src[i] == '.') {
1003             i++;
1004             if (mult == 0) {
1005                 break;
1006             } else {
1007                 src[i + mult] = 0;
1008             }
1009         }
1010         tmp *= 10;
1011         if (src[i] >= '0' && src[i] <= '9') {
1012             tmp += src[i] - '0';
1013         }
1014         if (i >= 15) {
1015             return 0; // out of bounds
1016         }
1017     }
1018     return isneg ? -tmp : tmp;    // handle negative altitudes
1019 }
1020
1021 typedef struct gpsDataNmea_s {
1022     int32_t latitude;
1023     int32_t longitude;
1024     uint8_t numSat;
1025     int32_t altitudeCm;
1026     uint16_t speed;
1027     uint16_t hdop;
1028     uint16_t ground_course;
1029     uint32_t time;
1030     uint32_t date;
1031 } gpsDataNmea_t;
1032
1033 static bool gpsNewFrameNMEA(char c)
1034 {
1035     static gpsDataNmea_t gps_Msg;
1036
1037     uint8_t frameOK = 0;
1038     static uint8_t param = 0, offset = 0, parity = 0;
1039     static char string[15];
1040     static uint8_t checksum_param, gps_frame = NO_FRAME;
1041     static uint8_t svMessageNum = 0;
1042     uint8_t svSatNum = 0, svPacketIdx = 0, svSatParam = 0;
1043
1044     switch (c) {
1045         case '$':
1046             param = 0;
1047             offset = 0;
1048             parity = 0;
1049             break;
1050         case ',':
1051         case '*':
1052             string[offset] = 0;
1053             if (param == 0) {       //frame identification
1054                 gps_frame = NO_FRAME;
1055                 if (0 == strcmp(string, "GPGGA") || 0 == strcmp(string, "GNGGA")) {
1056                     gps_frame = FRAME_GGA;
1057                 } else if (0 == strcmp(string, "GPRMC") || 0 == strcmp(string, "GNRMC")) {
1058                     gps_frame = FRAME_RMC;
1059                 } else if (0 == strcmp(string, "GPGSV")) {
1060                     gps_frame = FRAME_GSV;
1061                 }
1062             }
1063
1064             switch (gps_frame) {
1065                 case FRAME_GGA:        //************* GPGGA FRAME parsing
1066                     switch (param) {
1067             //          case 1:             // Time information
1068             //              break;
1069                         case 2:
1070                             gps_Msg.latitude = GPS_coord_to_degrees(string);
1071                             break;
1072                         case 3:
1073                             if (string[0] == 'S')
1074                                 gps_Msg.latitude *= -1;
1075                             break;
1076                         case 4:
1077                             gps_Msg.longitude = GPS_coord_to_degrees(string);
1078                             break;
1079                         case 5:
1080                             if (string[0] == 'W')
1081                                 gps_Msg.longitude *= -1;
1082                             break;
1083                         case 6:
1084                             gpsSetFixState(string[0] > '0');
1085                             break;
1086                         case 7:
1087                             gps_Msg.numSat = grab_fields(string, 0);
1088                             break;
1089                         case 8:
1090                             gps_Msg.hdop = grab_fields(string, 1) * 100;          // hdop
1091                             break;
1092                         case 9:
1093                             gps_Msg.altitudeCm = grab_fields(string, 1) * 10;     // altitude in centimeters. Note: NMEA delivers altitude with 1 or 3 decimals. It's safer to cut at 0.1m and multiply by 10
1094                             break;
1095                     }
1096                     break;
1097                 case FRAME_RMC:        //************* GPRMC FRAME parsing
1098                     switch (param) {
1099                         case 1:
1100                             gps_Msg.time = grab_fields(string, 2); // UTC time hhmmss.ss
1101                             break;
1102                         case 7:
1103                             gps_Msg.speed = ((grab_fields(string, 1) * 5144L) / 1000L);    // speed in cm/s added by Mis
1104                             break;
1105                         case 8:
1106                             gps_Msg.ground_course = (grab_fields(string, 1));      // ground course deg * 10
1107                             break;
1108                         case 9:
1109                             gps_Msg.date = grab_fields(string, 0); // date dd/mm/yy
1110                             break;
1111                     }
1112                     break;
1113                 case FRAME_GSV:
1114                     switch (param) {
1115                       /*case 1:
1116                             // Total number of messages of this type in this cycle
1117                             break; */
1118                         case 2:
1119                             // Message number
1120                             svMessageNum = grab_fields(string, 0);
1121                             break;
1122                         case 3:
1123                             // Total number of SVs visible
1124                             GPS_numCh = grab_fields(string, 0);
1125                             if (GPS_numCh > GPS_SV_MAXSATS_LEGACY) {
1126                                 GPS_numCh = GPS_SV_MAXSATS_LEGACY;
1127                             }
1128                             break;
1129                     }
1130                     if (param < 4)
1131                         break;
1132
1133                     svPacketIdx = (param - 4) / 4 + 1; // satellite number in packet, 1-4
1134                     svSatNum    = svPacketIdx + (4 * (svMessageNum - 1)); // global satellite number
1135                     svSatParam  = param - 3 - (4 * (svPacketIdx - 1)); // parameter number for satellite
1136
1137                     if (svSatNum > GPS_SV_MAXSATS_LEGACY)
1138                         break;
1139
1140                     switch (svSatParam) {
1141                         case 1:
1142                             // SV PRN number
1143                             GPS_svinfo_chn[svSatNum - 1]  = svSatNum;
1144                             GPS_svinfo_svid[svSatNum - 1] = grab_fields(string, 0);
1145                             break;
1146                       /*case 2:
1147                             // Elevation, in degrees, 90 maximum
1148                             break;
1149                         case 3:
1150                             // Azimuth, degrees from True North, 000 through 359
1151                             break; */
1152                         case 4:
1153                             // SNR, 00 through 99 dB (null when not tracking)
1154                             GPS_svinfo_cno[svSatNum - 1] = grab_fields(string, 0);
1155                             GPS_svinfo_quality[svSatNum - 1] = 0; // only used by ublox
1156                             break;
1157                     }
1158
1159                     GPS_svInfoReceivedCount++;
1160
1161                     break;
1162             }
1163
1164             param++;
1165             offset = 0;
1166             if (c == '*')
1167                 checksum_param = 1;
1168             else
1169                 parity ^= c;
1170             break;
1171         case '\r':
1172         case '\n':
1173             if (checksum_param) {   //parity checksum
1174                 shiftPacketLog();
1175                 uint8_t checksum = 16 * ((string[0] >= 'A') ? string[0] - 'A' + 10 : string[0] - '0') + ((string[1] >= 'A') ? string[1] - 'A' + 10 : string[1] - '0');
1176                 if (checksum == parity) {
1177                     *gpsPacketLogChar = LOG_IGNORED;
1178                     GPS_packetCount++;
1179                     switch (gps_frame) {
1180                     case FRAME_GGA:
1181                       *gpsPacketLogChar = LOG_NMEA_GGA;
1182                       frameOK = 1;
1183                       if (STATE(GPS_FIX)) {
1184                             gpsSol.llh.lat = gps_Msg.latitude;
1185                             gpsSol.llh.lon = gps_Msg.longitude;
1186                             gpsSol.numSat = gps_Msg.numSat;
1187                             gpsSol.llh.altCm = gps_Msg.altitudeCm;
1188                             gpsSol.hdop = gps_Msg.hdop;
1189                         }
1190                         break;
1191                     case FRAME_RMC:
1192                         *gpsPacketLogChar = LOG_NMEA_RMC;
1193                         gpsSol.groundSpeed = gps_Msg.speed;
1194                         gpsSol.groundCourse = gps_Msg.ground_course;
1195 #ifdef USE_RTC_TIME
1196                         // This check will miss 00:00:00.00, but we shouldn't care - next report will be valid
1197                         if(!rtcHasTime() && gps_Msg.date != 0 && gps_Msg.time != 0) {
1198                             dateTime_t temp_time;
1199                             temp_time.year = (gps_Msg.date % 100) + 2000;
1200                             temp_time.month = (gps_Msg.date / 100) % 100;
1201                             temp_time.day = (gps_Msg.date / 10000) % 100;
1202                             temp_time.hours = (gps_Msg.time / 1000000) % 100;
1203                             temp_time.minutes = (gps_Msg.time / 10000) % 100;
1204                             temp_time.seconds = (gps_Msg.time / 100) % 100;
1205                             temp_time.millis = (gps_Msg.time & 100) * 10;
1206                             rtcSetDateTime(&temp_time);
1207                         }
1208 #endif
1209                         break;
1210                     } // end switch
1211                 } else {
1212                     *gpsPacketLogChar = LOG_ERROR;
1213                 }
1214             }
1215             checksum_param = 0;
1216             break;
1217         default:
1218             if (offset < 15)
1219                 string[offset++] = c;
1220             if (!checksum_param)
1221                 parity ^= c;
1222     }
1223     return frameOK;
1224 }
1225 #endif // USE_GPS_NMEA
1226
1227 #ifdef USE_GPS_UBLOX
1228 // UBX support
1229 typedef struct {
1230     uint32_t time;              // GPS msToW
1231     int32_t longitude;
1232     int32_t latitude;
1233     int32_t altitude_ellipsoid;
1234     int32_t altitudeMslMm;
1235     uint32_t horizontal_accuracy;
1236     uint32_t vertical_accuracy;
1237 } ubx_nav_posllh;
1238
1239 typedef struct {
1240     uint32_t time;              // GPS msToW
1241     uint8_t fix_type;
1242     uint8_t fix_status;
1243     uint8_t differential_status;
1244     uint8_t res;
1245     uint32_t time_to_first_fix;
1246     uint32_t uptime;            // milliseconds
1247 } ubx_nav_status;
1248
1249 typedef struct {
1250     uint32_t time;
1251     int32_t time_nsec;
1252     int16_t week;
1253     uint8_t fix_type;
1254     uint8_t fix_status;
1255     int32_t ecef_x;
1256     int32_t ecef_y;
1257     int32_t ecef_z;
1258     uint32_t position_accuracy_3d;
1259     int32_t ecef_x_velocity;
1260     int32_t ecef_y_velocity;
1261     int32_t ecef_z_velocity;
1262     uint32_t speed_accuracy;
1263     uint16_t position_DOP;
1264     uint8_t res;
1265     uint8_t satellites;
1266     uint32_t res2;
1267 } ubx_nav_solution;
1268
1269 typedef struct {
1270     uint32_t time;
1271     uint16_t year;
1272     uint8_t month;
1273     uint8_t day;
1274     uint8_t hour;
1275     uint8_t min;
1276     uint8_t sec;
1277     uint8_t valid;
1278     uint32_t tAcc;
1279     int32_t nano;
1280     uint8_t fixType;
1281     uint8_t flags;
1282     uint8_t flags2;
1283     uint8_t numSV;
1284     int32_t lon;
1285     int32_t lat;
1286     int32_t height;
1287     int32_t hMSL;
1288     uint32_t hAcc;
1289     uint32_t vAcc;
1290     int32_t velN;
1291     int32_t velE;
1292     int32_t velD;
1293     int32_t gSpeed;
1294     int32_t headMot;
1295     uint32_t sAcc;
1296     uint32_t headAcc;
1297     uint16_t pDOP;
1298     uint8_t flags3;
1299     uint8_t reserved0[5];
1300     int32_t headVeh;
1301     int16_t magDec;
1302     uint16_t magAcc;
1303 } ubx_nav_pvt;
1304
1305 typedef struct {
1306     uint32_t time;              // GPS msToW
1307     int32_t ned_north;
1308     int32_t ned_east;
1309     int32_t ned_down;
1310     uint32_t speed_3d;
1311     uint32_t speed_2d;
1312     int32_t heading_2d;
1313     uint32_t speed_accuracy;
1314     uint32_t heading_accuracy;
1315 } ubx_nav_velned;
1316
1317 typedef struct {
1318     uint8_t chn;                // Channel number, 255 for SVx not assigned to channel
1319     uint8_t svid;               // Satellite ID
1320     uint8_t flags;              // Bitmask
1321     uint8_t quality;            // Bitfield
1322     uint8_t cno;                // Carrier to Noise Ratio (Signal Strength) // dbHz, 0-55.
1323     uint8_t elev;               // Elevation in integer degrees
1324     int16_t azim;               // Azimuth in integer degrees
1325     int32_t prRes;              // Pseudo range residual in centimetres
1326 } ubx_nav_svinfo_channel;
1327
1328 typedef struct {
1329     uint8_t gnssId;
1330     uint8_t svId;               // Satellite ID
1331     uint8_t cno;                // Carrier to Noise Ratio (Signal Strength) // dbHz, 0-55.
1332     int8_t elev;                // Elevation in integer degrees
1333     int16_t azim;               // Azimuth in integer degrees
1334     int16_t prRes;              // Pseudo range residual in decimetres
1335     uint32_t flags;             // Bitmask
1336 } ubx_nav_sat_sv;
1337
1338 typedef struct {
1339     uint32_t time;              // GPS Millisecond time of week
1340     uint8_t numCh;              // Number of channels
1341     uint8_t globalFlags;        // Bitmask, Chip hardware generation 0:Antaris, 1:u-blox 5, 2:u-blox 6
1342     uint16_t reserved2;         // Reserved
1343     ubx_nav_svinfo_channel channel[GPS_SV_MAXSATS_M8N];         // 32 satellites * 12 byte
1344 } ubx_nav_svinfo;
1345
1346 typedef struct {
1347     uint32_t time;              // GPS Millisecond time of week
1348     uint8_t version;
1349     uint8_t numSvs;
1350     uint8_t reserved0[2];
1351     ubx_nav_sat_sv svs[GPS_SV_MAXSATS_M9N];
1352 } ubx_nav_sat;
1353
1354 typedef struct {
1355     uint8_t clsId;               // Class ID of the acknowledged message
1356     uint8_t msgId;               // Message ID of the acknowledged message
1357 } ubx_ack;
1358
1359 enum {
1360     FIX_NONE = 0,
1361     FIX_DEAD_RECKONING = 1,
1362     FIX_2D = 2,
1363     FIX_3D = 3,
1364     FIX_GPS_DEAD_RECKONING = 4,
1365     FIX_TIME = 5
1366 } ubs_nav_fix_type;
1367
1368 enum {
1369     NAV_STATUS_FIX_VALID = 1,
1370     NAV_STATUS_TIME_WEEK_VALID = 4,
1371     NAV_STATUS_TIME_SECOND_VALID = 8
1372 } ubx_nav_status_bits;
1373
1374 enum {
1375     NAV_VALID_DATE = 1,
1376     NAV_VALID_TIME = 2
1377 } ubx_nav_pvt_valid;
1378
1379 // Packet checksum accumulators
1380 static uint8_t _ck_a;
1381 static uint8_t _ck_b;
1382
1383 // State machine state
1384 static bool _skip_packet;
1385 static uint8_t _step;
1386 static uint8_t _msg_id;
1387 static uint16_t _payload_length;
1388 static uint16_t _payload_counter;
1389
1390 static bool next_fix;
1391 static uint8_t _class;
1392
1393 // do we have new position information?
1394 static bool _new_position;
1395
1396 // do we have new speed information?
1397 static bool _new_speed;
1398
1399 // Example packet sizes from UBlox u-center from a Glonass capable GPS receiver.
1400 //15:17:55  R -> UBX NAV-STATUS,  Size  24,  'Navigation Status'
1401 //15:17:55  R -> UBX NAV-POSLLH,  Size  36,  'Geodetic Position'
1402 //15:17:55  R -> UBX NAV-VELNED,  Size  44,  'Velocity in WGS 84'
1403 //15:17:55  R -> UBX NAV-CLOCK,  Size  28,  'Clock Status'
1404 //15:17:55  R -> UBX NAV-AOPSTATUS,  Size  24,  'AOP Status'
1405 //15:17:55  R -> UBX 03-09,  Size 208,  'Unknown'
1406 //15:17:55  R -> UBX 03-10,  Size 336,  'Unknown'
1407 //15:17:55  R -> UBX NAV-SOL,  Size  60,  'Navigation Solution'
1408 //15:17:55  R -> UBX NAV,  Size 100,  'Navigation'
1409 //15:17:55  R -> UBX NAV-SVINFO,  Size 328,  'Satellite Status and Information'
1410
1411 // from the UBlox9 document, the largest payout we receive is the NAV-SAT and the payload size
1412 // is calculated as 8 + 12*numCh.  numCh in the case of a M9N is 42.
1413 #define UBLOX_PAYLOAD_SIZE (8 + 12 * 42)
1414
1415
1416 // Receive buffer
1417 static union {
1418     ubx_nav_posllh posllh;
1419     ubx_nav_status status;
1420     ubx_nav_solution solution;
1421     ubx_nav_velned velned;
1422     ubx_nav_pvt pvt;
1423     ubx_nav_svinfo svinfo;
1424     ubx_nav_sat sat;
1425     ubx_cfg_gnss gnss;
1426     ubx_ack ack;
1427     uint8_t bytes[UBLOX_PAYLOAD_SIZE];
1428 } _buffer;
1429
1430 void _update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b)
1431 {
1432     while (len--) {
1433         *ck_a += *data;
1434         *ck_b += *ck_a;
1435         data++;
1436     }
1437 }
1438
1439 static bool UBLOX_parse_gps(void)
1440 {
1441     uint32_t i;
1442
1443     *gpsPacketLogChar = LOG_IGNORED;
1444
1445     switch (_msg_id) {
1446     case MSG_POSLLH:
1447         *gpsPacketLogChar = LOG_UBLOX_POSLLH;
1448         //i2c_dataset.time                = _buffer.posllh.time;
1449         gpsSol.llh.lon = _buffer.posllh.longitude;
1450         gpsSol.llh.lat = _buffer.posllh.latitude;
1451         gpsSol.llh.altCm = _buffer.posllh.altitudeMslMm / 10;  //alt in cm
1452         gpsSetFixState(next_fix);
1453         _new_position = true;
1454         break;
1455     case MSG_STATUS:
1456         *gpsPacketLogChar = LOG_UBLOX_STATUS;
1457         next_fix = (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.status.fix_type == FIX_3D);
1458         if (!next_fix)
1459             DISABLE_STATE(GPS_FIX);
1460         break;
1461     case MSG_SOL:
1462         *gpsPacketLogChar = LOG_UBLOX_SOL;
1463         next_fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D);
1464         if (!next_fix)
1465             DISABLE_STATE(GPS_FIX);
1466         gpsSol.numSat = _buffer.solution.satellites;
1467         gpsSol.hdop = _buffer.solution.position_DOP;
1468 #ifdef USE_RTC_TIME
1469         //set clock, when gps time is available
1470         if(!rtcHasTime() && (_buffer.solution.fix_status & NAV_STATUS_TIME_SECOND_VALID) && (_buffer.solution.fix_status & NAV_STATUS_TIME_WEEK_VALID)) {
1471             //calculate rtctime: week number * ms in a week + ms of week + fractions of second + offset to UNIX reference year - 18 leap seconds
1472             rtcTime_t temp_time = (((int64_t) _buffer.solution.week)*7*24*60*60*1000) + _buffer.solution.time + (_buffer.solution.time_nsec/1000000) + 315964800000LL - 18000;
1473             rtcSet(&temp_time);
1474         }
1475 #endif
1476         break;
1477     case MSG_VELNED:
1478         *gpsPacketLogChar = LOG_UBLOX_VELNED;
1479         gpsSol.speed3d = _buffer.velned.speed_3d;       // cm/s
1480         gpsSol.groundSpeed = _buffer.velned.speed_2d;    // cm/s
1481         gpsSol.groundCourse = (uint16_t) (_buffer.velned.heading_2d / 10000);     // Heading 2D deg * 100000 rescaled to deg * 10
1482         _new_speed = true;
1483         break;
1484     case MSG_PVT:
1485         *gpsPacketLogChar = LOG_UBLOX_SOL;
1486         next_fix = (_buffer.pvt.flags & NAV_STATUS_FIX_VALID) && (_buffer.pvt.fixType == FIX_3D);
1487         gpsSol.llh.lon = _buffer.pvt.lon;
1488         gpsSol.llh.lat = _buffer.pvt.lat;
1489         gpsSol.llh.altCm = _buffer.pvt.hMSL / 10;  //alt in cm
1490         gpsSetFixState(next_fix);
1491         _new_position = true;
1492         gpsSol.numSat = _buffer.pvt.numSV;
1493         gpsSol.hdop = _buffer.pvt.pDOP;
1494         gpsSol.speed3d = (uint16_t) sqrtf(powf(_buffer.pvt.gSpeed / 10, 2.0f) + powf(_buffer.pvt.velD / 10, 2.0f));
1495         gpsSol.groundSpeed = _buffer.pvt.gSpeed / 10;    // cm/s
1496         gpsSol.groundCourse = (uint16_t) (_buffer.pvt.headMot / 10000);     // Heading 2D deg * 100000 rescaled to deg * 10
1497         _new_speed = true;
1498 #ifdef USE_RTC_TIME
1499         //set clock, when gps time is available
1500         if (!rtcHasTime() && (_buffer.pvt.valid & NAV_VALID_DATE) && (_buffer.pvt.valid & NAV_VALID_TIME)) {
1501             dateTime_t dt;
1502             dt.year = _buffer.pvt.year;
1503             dt.month = _buffer.pvt.month;
1504             dt.day = _buffer.pvt.day;
1505             dt.hours = _buffer.pvt.hour;
1506             dt.minutes = _buffer.pvt.min;
1507             dt.seconds = _buffer.pvt.sec;
1508             dt.millis = (_buffer.pvt.nano > 0) ? _buffer.pvt.nano / 1000 : 0; //up to 5ms of error
1509             rtcSetDateTime(&dt);
1510         }
1511 #endif
1512         break;
1513     case MSG_SVINFO:
1514         *gpsPacketLogChar = LOG_UBLOX_SVINFO;
1515         GPS_numCh = _buffer.svinfo.numCh;
1516         // If we're getting NAV-SVINFO is because we're dealing with an old receiver that does not support NAV-SAT, so we'll only
1517         // save up to GPS_SV_MAXSATS_LEGACY channels so the BF Configurator knows it's receiving the old sat list info format.
1518         if (GPS_numCh > GPS_SV_MAXSATS_LEGACY)
1519             GPS_numCh = GPS_SV_MAXSATS_LEGACY;
1520         for (i = 0; i < GPS_numCh; i++) {
1521             GPS_svinfo_chn[i] = _buffer.svinfo.channel[i].chn;
1522             GPS_svinfo_svid[i] = _buffer.svinfo.channel[i].svid;
1523             GPS_svinfo_quality[i] =_buffer.svinfo.channel[i].quality;
1524             GPS_svinfo_cno[i] = _buffer.svinfo.channel[i].cno;
1525         }
1526         for (i = GPS_numCh; i < GPS_SV_MAXSATS_LEGACY; i++) {
1527             GPS_svinfo_chn[i] = 0;
1528             GPS_svinfo_svid[i] = 0;
1529             GPS_svinfo_quality[i] = 0;
1530             GPS_svinfo_cno[i] = 0;
1531         }
1532         GPS_svInfoReceivedCount++;
1533         break;
1534     case MSG_SAT:
1535         *gpsPacketLogChar = LOG_UBLOX_SVINFO; // The logger won't show this is NAV-SAT instead of NAV-SVINFO
1536         GPS_numCh = _buffer.sat.numSvs;
1537         // We can receive here upto GPS_SV_MAXSATS_M9N channels, but since the majority of receivers currently in use are M8N or older,
1538         // it would be a waste of RAM to size the arrays that big. For now, they're sized GPS_SV_MAXSATS_M8N which means M9N won't show
1539         // all their channel information on BF Configurator. When M9N's are more widespread it would be a good time to increase those arrays.
1540         if (GPS_numCh > GPS_SV_MAXSATS_M8N)
1541             GPS_numCh = GPS_SV_MAXSATS_M8N;
1542         for (i = 0; i < GPS_numCh; i++) {
1543             GPS_svinfo_chn[i] = _buffer.sat.svs[i].gnssId;
1544             GPS_svinfo_svid[i] = _buffer.sat.svs[i].svId;
1545             GPS_svinfo_cno[i] = _buffer.sat.svs[i].cno;
1546             GPS_svinfo_quality[i] =_buffer.sat.svs[i].flags;
1547         }
1548         for (i = GPS_numCh; i < GPS_SV_MAXSATS_M8N; i++) {
1549             GPS_svinfo_chn[i] = 255;
1550             GPS_svinfo_svid[i] = 0;
1551             GPS_svinfo_quality[i] = 0;
1552             GPS_svinfo_cno[i] = 0;
1553         }
1554
1555         // Setting the number of channels higher than GPS_SV_MAXSATS_LEGACY is the only way to tell BF Configurator we're sending the
1556         // enhanced sat list info without changing the MSP protocol. Also, we're sending the complete list each time even if it's empty, so
1557         // BF Conf can erase old entries shown on screen when channels are removed from the list.
1558         GPS_numCh = GPS_SV_MAXSATS_M8N;
1559         GPS_svInfoReceivedCount++;
1560         break;
1561     case MSG_CFG_GNSS:
1562         {
1563             bool isSBASenabled = false;
1564             bool isM8NwithDefaultConfig = false;
1565
1566             if ((_buffer.gnss.numConfigBlocks >= 2) &&
1567                 (_buffer.gnss.configblocks[1].gnssId == 1) && //SBAS
1568                 (_buffer.gnss.configblocks[1].flags & UBLOX_GNSS_ENABLE)) { //enabled
1569
1570                 isSBASenabled = true;
1571             }
1572
1573             if ((_buffer.gnss.numTrkChHw == 32) &&  //M8N
1574                 (_buffer.gnss.numTrkChUse == 32) &&
1575                 (_buffer.gnss.numConfigBlocks == 7) &&
1576                 (_buffer.gnss.configblocks[2].gnssId == 2) && //Galileo
1577                 (_buffer.gnss.configblocks[2].resTrkCh == 4) && //min channels
1578                 (_buffer.gnss.configblocks[2].maxTrkCh == 8) && //max channels
1579                 !(_buffer.gnss.configblocks[2].flags & UBLOX_GNSS_ENABLE)) { //disabled
1580
1581                 isM8NwithDefaultConfig = true;
1582             }
1583
1584             const uint16_t messageSize = 4 + (_buffer.gnss.numConfigBlocks * sizeof(ubx_configblock));
1585
1586             ubx_message tx_buffer;
1587             memcpy(&tx_buffer.payload, &_buffer, messageSize);
1588
1589             if (isSBASenabled && (gpsConfig()->sbasMode == SBAS_NONE)) {
1590                 tx_buffer.payload.cfg_gnss.configblocks[1].flags &= ~UBLOX_GNSS_ENABLE; //Disable SBAS
1591             }
1592
1593             if (isM8NwithDefaultConfig && gpsConfig()->gps_ublox_use_galileo) {
1594                 tx_buffer.payload.cfg_gnss.configblocks[2].flags |= UBLOX_GNSS_ENABLE; //Enable Galileo
1595             }
1596
1597             ubloxSendConfigMessage(&tx_buffer, MSG_CFG_GNSS, messageSize);
1598         }
1599         break;
1600     case MSG_ACK_ACK:
1601         if ((gpsData.ackState == UBLOX_ACK_WAITING) && (_buffer.ack.msgId == gpsData.ackWaitingMsgId)) {
1602             gpsData.ackState = UBLOX_ACK_GOT_ACK;
1603         }
1604         break;
1605     case MSG_ACK_NACK:
1606         if ((gpsData.ackState == UBLOX_ACK_WAITING) && (_buffer.ack.msgId == gpsData.ackWaitingMsgId)) {
1607             gpsData.ackState = UBLOX_ACK_GOT_NACK;
1608         }
1609         break;
1610     default:
1611         return false;
1612     }
1613
1614     // we only return true when we get new position and speed data
1615     // this ensures we don't use stale data
1616     if (_new_position && _new_speed) {
1617         _new_speed = _new_position = false;
1618         return true;
1619     }
1620     return false;
1621 }
1622
1623 static bool gpsNewFrameUBLOX(uint8_t data)
1624 {
1625     bool parsed = false;
1626
1627     switch (_step) {
1628         case 0: // Sync char 1 (0xB5)
1629             if (PREAMBLE1 == data) {
1630                 _skip_packet = false;
1631                 _step++;
1632             }
1633             break;
1634         case 1: // Sync char 2 (0x62)
1635             if (PREAMBLE2 != data) {
1636                 _step = 0;
1637                 break;
1638             }
1639             _step++;
1640             break;
1641         case 2: // Class
1642             _step++;
1643             _class = data;
1644             _ck_b = _ck_a = data;   // reset the checksum accumulators
1645             break;
1646         case 3: // Id
1647             _step++;
1648             _ck_b += (_ck_a += data);       // checksum byte
1649             _msg_id = data;
1650 #if DEBUG_UBLOX_FRAMES
1651     debug[2] = _msg_id;
1652 #endif
1653             break;
1654         case 4: // Payload length (part 1)
1655             _step++;
1656             _ck_b += (_ck_a += data);       // checksum byte
1657             _payload_length = data; // payload length low byte
1658             break;
1659         case 5: // Payload length (part 2)
1660             _step++;
1661             _ck_b += (_ck_a += data);       // checksum byte
1662             _payload_length += (uint16_t)(data << 8);
1663 #if DEBUG_UBLOX_FRAMES
1664     debug[3] = _payload_length;
1665 #endif
1666             if (_payload_length > UBLOX_PAYLOAD_SIZE) {
1667                 _skip_packet = true;
1668             }
1669             _payload_counter = 0;   // prepare to receive payload
1670             if (_payload_length == 0) {
1671                 _step = 7;
1672             }
1673             break;
1674         case 6:
1675             _ck_b += (_ck_a += data);       // checksum byte
1676             if (_payload_counter < UBLOX_PAYLOAD_SIZE) {
1677                 _buffer.bytes[_payload_counter] = data;
1678             }
1679             if (++_payload_counter >= _payload_length) {
1680                 _step++;
1681             }
1682             break;
1683         case 7:
1684             _step++;
1685             if (_ck_a != data) {
1686                 _skip_packet = true;          // bad checksum
1687                 gpsData.errors++;
1688             }
1689             break;
1690         case 8:
1691             _step = 0;
1692
1693             shiftPacketLog();
1694
1695             if (_ck_b != data) {
1696                 *gpsPacketLogChar = LOG_ERROR;
1697                 gpsData.errors++;
1698                 break;              // bad checksum
1699             }
1700
1701             GPS_packetCount++;
1702
1703             if (_skip_packet) {
1704                 *gpsPacketLogChar = LOG_SKIPPED;
1705                 break;
1706             }
1707
1708             if (UBLOX_parse_gps()) {
1709                 parsed = true;
1710             }
1711     }
1712     return parsed;
1713 }
1714 #endif // USE_GPS_UBLOX
1715
1716 static void gpsHandlePassthrough(uint8_t data)
1717 {
1718      gpsNewData(data);
1719  #ifdef USE_DASHBOARD
1720      if (featureIsEnabled(FEATURE_DASHBOARD)) {
1721          dashboardUpdate(micros());
1722      }
1723  #endif
1724
1725  }
1726
1727 void gpsEnablePassthrough(serialPort_t *gpsPassthroughPort)
1728 {
1729     waitForSerialPortToFinishTransmitting(gpsPort);
1730     waitForSerialPortToFinishTransmitting(gpsPassthroughPort);
1731
1732     if (!(gpsPort->mode & MODE_TX))
1733         serialSetMode(gpsPort, gpsPort->mode | MODE_TX);
1734
1735 #ifdef USE_DASHBOARD
1736     if (featureIsEnabled(FEATURE_DASHBOARD)) {
1737         dashboardShowFixedPage(PAGE_GPS);
1738     }
1739 #endif
1740
1741     serialPassthrough(gpsPort, gpsPassthroughPort, &gpsHandlePassthrough, NULL);
1742 }
1743
1744 float GPS_scaleLonDown = 1.0f;  // this is used to offset the shrinking longitude as we go towards the poles
1745
1746 void GPS_calc_longitude_scaling(int32_t lat)
1747 {
1748     float rads = (fabsf((float)lat) / 10000000.0f) * 0.0174532925f;
1749     GPS_scaleLonDown = cos_approx(rads);
1750 }
1751
1752 ////////////////////////////////////////////////////////////////////////////////////
1753 // Calculate the distance flown and vertical speed from gps position data
1754 //
1755 static void GPS_calculateDistanceFlownVerticalSpeed(bool initialize)
1756 {
1757     static int32_t lastCoord[2] = { 0, 0 };
1758     static int32_t lastAlt;
1759     static int32_t lastMillis;
1760
1761     int currentMillis = millis();
1762
1763     if (initialize) {
1764         GPS_distanceFlownInCm = 0;
1765         GPS_verticalSpeedInCmS = 0;
1766     } else {
1767         if (STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED)) {
1768             uint16_t speed = gpsConfig()->gps_use_3d_speed ? gpsSol.speed3d : gpsSol.groundSpeed;
1769             // Only add up movement when speed is faster than minimum threshold
1770             if (speed > GPS_DISTANCE_FLOWN_MIN_SPEED_THRESHOLD_CM_S) {
1771                 uint32_t dist;
1772                 int32_t dir;
1773                 GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &lastCoord[GPS_LATITUDE], &lastCoord[GPS_LONGITUDE], &dist, &dir);
1774                 if (gpsConfig()->gps_use_3d_speed) {
1775                     dist = sqrtf(powf(gpsSol.llh.altCm - lastAlt, 2.0f) + powf(dist, 2.0f));
1776                 }
1777                 GPS_distanceFlownInCm += dist;
1778             }
1779         }
1780         GPS_verticalSpeedInCmS = (gpsSol.llh.altCm - lastAlt) * 1000 / (currentMillis - lastMillis);
1781         GPS_verticalSpeedInCmS = constrain(GPS_verticalSpeedInCmS, -1500, 1500);
1782     }
1783     lastCoord[GPS_LONGITUDE] = gpsSol.llh.lon;
1784     lastCoord[GPS_LATITUDE] = gpsSol.llh.lat;
1785     lastAlt = gpsSol.llh.altCm;
1786     lastMillis = currentMillis;
1787 }
1788
1789 void GPS_reset_home_position(void)
1790 {
1791     if (!STATE(GPS_FIX_HOME) || !gpsConfig()->gps_set_home_point_once) {
1792         if (STATE(GPS_FIX) && gpsSol.numSat >= 5) {
1793             GPS_home[GPS_LATITUDE] = gpsSol.llh.lat;
1794             GPS_home[GPS_LONGITUDE] = gpsSol.llh.lon;
1795             GPS_calc_longitude_scaling(gpsSol.llh.lat); // need an initial value for distance and bearing calc
1796             // Set ground altitude
1797             ENABLE_STATE(GPS_FIX_HOME);
1798         }
1799     }
1800     GPS_calculateDistanceFlownVerticalSpeed(true); //Initialize
1801 }
1802
1803 ////////////////////////////////////////////////////////////////////////////////////
1804 #define DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS 1.113195f
1805 #define TAN_89_99_DEGREES 5729.57795f
1806 // Get distance between two points in cm
1807 // Get bearing from pos1 to pos2, returns an 1deg = 100 precision
1808 void GPS_distance_cm_bearing(int32_t *currentLat1, int32_t *currentLon1, int32_t *destinationLat2, int32_t *destinationLon2, uint32_t *dist, int32_t *bearing)
1809 {
1810     float dLat = *destinationLat2 - *currentLat1; // difference of latitude in 1/10 000 000 degrees
1811     float dLon = (float)(*destinationLon2 - *currentLon1) * GPS_scaleLonDown;
1812     *dist = sqrtf(sq(dLat) + sq(dLon)) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS;
1813
1814     *bearing = 9000.0f + atan2_approx(-dLat, dLon) * TAN_89_99_DEGREES;      // Convert the output radians to 100xdeg
1815     if (*bearing < 0)
1816         *bearing += 36000;
1817 }
1818
1819 void GPS_calculateDistanceAndDirectionToHome(void)
1820 {
1821     if (STATE(GPS_FIX_HOME)) {      // If we don't have home set, do not display anything
1822         uint32_t dist;
1823         int32_t dir;
1824         GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_home[GPS_LATITUDE], &GPS_home[GPS_LONGITUDE], &dist, &dir);
1825         GPS_distanceToHome = dist / 100;
1826         GPS_directionToHome = dir / 100;
1827     } else {
1828         GPS_distanceToHome = 0;
1829         GPS_directionToHome = 0;
1830     }
1831 }
1832
1833 void onGpsNewData(void)
1834 {
1835     if (!(STATE(GPS_FIX) && gpsSol.numSat >= 5)) {
1836         return;
1837     }
1838
1839     //
1840     // Calculate time delta for navigation loop, range 0-1.0f, in seconds
1841     //
1842     // Time for calculating x,y speed and navigation pids
1843     static uint32_t nav_loopTimer;
1844     dTnav = (float)(millis() - nav_loopTimer) / 1000.0f;
1845     nav_loopTimer = millis();
1846     // prevent runup from bad GPS
1847     dTnav = MIN(dTnav, 1.0f);
1848
1849     GPS_calculateDistanceAndDirectionToHome();
1850     if (ARMING_FLAG(ARMED)) {
1851         GPS_calculateDistanceFlownVerticalSpeed(false);
1852     }
1853
1854 #ifdef USE_GPS_RESCUE
1855     rescueNewGpsData();
1856 #endif
1857 }
1858
1859 void gpsSetFixState(bool state)
1860 {
1861     if (state) {
1862         ENABLE_STATE(GPS_FIX);
1863         ENABLE_STATE(GPS_FIX_EVER);
1864     } else {
1865         DISABLE_STATE(GPS_FIX);
1866     }
1867 }
1868 #endif