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