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