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