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