src/main/io/gps.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <ctype.h>
  24 #include <string.h>
  25 #include <math.h>
  26
  27 #include "platform.h"
  28
  29 #ifdef USE_GPS
  30
  31 #include "build/build_config.h"
  32 #include "build/debug.h"
  33
  34 #include "common/axis.h"
  35 #include "common/gps_conversion.h"
  36 #include "common/maths.h"
  37 #include "common/utils.h"
  38
  39 #include "config/feature.h"
  40 #include "pg/pg.h"
  41 #include "pg/pg_ids.h"
  42
  43 #include "drivers/light_led.h"
  44 #include "drivers/time.h"
  45
  46 #include "io/dashboard.h"
  47 #include "io/gps.h"
  48 #include "io/serial.h"
  49
  50 #include "config/config.h"
  51 #include "fc/runtime_config.h"
  52
  53 #include "flight/imu.h"
  54 #include "flight/pid.h"
  55 #include "flight/gps_rescue.h"
  56
  57 #include "sensors/sensors.h"
  58
  59 #define LOG_ERROR        '?'
  60 #define LOG_IGNORED      '!'
  61 #define LOG_SKIPPED      '>'
  62 #define LOG_NMEA_GGA     'g'
  63 #define LOG_NMEA_RMC     'r'
  64 #define LOG_UBLOX_SOL    'O'
  65 #define LOG_UBLOX_STATUS 'S'
  66 #define LOG_UBLOX_SVINFO 'I'
  67 #define LOG_UBLOX_POSLLH 'P'
  68 #define LOG_UBLOX_VELNED 'V'
  69
  70 #define GPS_SV_MAXSATS   16
  71
  72 char gpsPacketLog[GPS_PACKET_LOG_ENTRY_COUNT];
  73 static char *gpsPacketLogChar = gpsPacketLog;
  74 // **********************
  75 // GPS
  76 // **********************
  77 int32_t GPS_home[2];
  78 uint16_t GPS_distanceToHome;        // distance to home point in meters
  79 int16_t GPS_directionToHome;        // direction to home or hol point in degrees
  80 uint32_t GPS_distanceFlownInCm;     // distance flown since armed in centimeters
  81 int16_t GPS_verticalSpeedInCmS;     // vertical speed in cm/s
  82 float dTnav;             // Delta Time in milliseconds for navigation computations, updated with every good GPS read
  83 int16_t nav_takeoff_bearing;
  84
  85 #define GPS_DISTANCE_FLOWN_MIN_SPEED_THRESHOLD_CM_S 15 // 5.4Km/h 3.35mph
  86
  87 gpsSolutionData_t gpsSol;
  88 uint32_t GPS_packetCount = 0;
  89 uint32_t GPS_svInfoReceivedCount = 0; // SV = Space Vehicle, counter increments each time SV info is received.
  90 uint8_t GPS_update = 0;             // toogle to distinct a GPS position update (directly or via MSP)
  91
  92 uint8_t GPS_numCh;                          // Number of channels
  93 uint8_t GPS_svinfo_chn[GPS_SV_MAXSATS];     // Channel number
  94 uint8_t GPS_svinfo_svid[GPS_SV_MAXSATS];    // Satellite ID
  95 uint8_t GPS_svinfo_quality[GPS_SV_MAXSATS]; // Bitfield Qualtity
  96 uint8_t GPS_svinfo_cno[GPS_SV_MAXSATS];     // Carrier to Noise Ratio (Signal Strength)
  97
  98 // GPS timeout for wrong baud rate/disconnection/etc in milliseconds (default 2.5second)
  99 #define GPS_TIMEOUT (2500)
 100 // How many entries in gpsInitData array below
 101 #define GPS_INIT_ENTRIES (GPS_BAUDRATE_MAX + 1)
 102 #define GPS_BAUDRATE_CHANGE_DELAY (200)
 103
 104 static serialPort_t *gpsPort;
 105
 106 typedef struct gpsInitData_s {
 107     uint8_t index;
 108     uint8_t baudrateIndex; // see baudRate_e
 109     const char *ubx;
 110     const char *mtk;
 111 } gpsInitData_t;
 112
 113 // NMEA will cycle through these until valid data is received
 114 static const gpsInitData_t gpsInitData[] = {
 115     { GPS_BAUDRATE_115200,  BAUD_115200, "$PUBX,41,1,0003,0001,115200,0*1E\r\n", "$PMTK251,115200*1F\r\n" },
 116     { GPS_BAUDRATE_57600,    BAUD_57600, "$PUBX,41,1,0003,0001,57600,0*2D\r\n", "$PMTK251,57600*2C\r\n" },
 117     { GPS_BAUDRATE_38400,    BAUD_38400, "$PUBX,41,1,0003,0001,38400,0*26\r\n", "$PMTK251,38400*27\r\n" },
 118     { GPS_BAUDRATE_19200,    BAUD_19200, "$PUBX,41,1,0003,0001,19200,0*23\r\n", "$PMTK251,19200*22\r\n" },
 119     // 9600 is not enough for 5Hz updates - leave for compatibility to dumb NMEA that only runs at this speed
 120     { GPS_BAUDRATE_9600,      BAUD_9600, "$PUBX,41,1,0003,0001,9600,0*16\r\n", "" }
 121 };
 122
 123 #define GPS_INIT_DATA_ENTRY_COUNT (sizeof(gpsInitData) / sizeof(gpsInitData[0]))
 124
 125 #define DEFAULT_BAUD_RATE_INDEX 0
 126
 127 #ifdef USE_GPS_UBLOX
 128 static const uint8_t ubloxInit[] = {
 129     //Preprocessor Pedestrian Dynamic Platform Model Option
 130     0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x03, 0x03, 0x00,           // CFG-NAV5 - Set engine settings
 131     0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00,           // Collected by resetting a GPS unit to defaults. Changing mode to Pedistrian and
 132     0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00,           // capturing the data from the U-Center binary console.
 133     0x00, 0xC8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0xC2,
 134
 135     // DISABLE NMEA messages
 136     0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x05, 0x00, 0xFF, 0x19,           // VGS: Course over ground and Ground speed
 137     0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x03, 0x00, 0xFD, 0x15,           // GSV: GNSS Satellites in View
 138     0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x01, 0x00, 0xFB, 0x11,           // GLL: Latitude and longitude, with time of position fix and status
 139     0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x00, 0x00, 0xFA, 0x0F,           // GGA: Global positioning system fix data
 140     0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x02, 0x00, 0xFC, 0x13,           // GSA: GNSS DOP and Active Satellites
 141     0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x04, 0x00, 0xFE, 0x17,           // RMC: Recommended Minimum data
 142
 143     // Enable UBLOX messages
 144     0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x02, 0x01, 0x0E, 0x47,           // set POSLLH MSG rate
 145     0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x03, 0x01, 0x0F, 0x49,           // set STATUS MSG rate
 146     0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x06, 0x01, 0x12, 0x4F,           // set SOL MSG rate
 147     //0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x30, 0x01, 0x3C, 0xA3,           // set SVINFO MSG rate (every cycle - high bandwidth)
 148     0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x30, 0x05, 0x40, 0xA7,           // set SVINFO MSG rate (evey 5 cycles - low bandwidth)
 149     0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x12, 0x01, 0x1E, 0x67,           // set VELNED MSG rate
 150
 151     0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0xC8, 0x00, 0x01, 0x00, 0x01, 0x00, 0xDE, 0x6A,             // set rate to 5Hz (measurement period: 200ms, navigation rate: 1 cycle)
 152 };
 153
 154 static const uint8_t ubloxAirborne[] = {
 155     //Preprocessor Airborne_1g Dynamic Platform Model Option
 156     #if defined(GPS_UBLOX_MODE_AIRBORNE_1G)
 157     0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x06, 0x03, 0x00,           // CFG-NAV5 - Set engine settings
 158     0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00,           // Collected by resetting a GPS unit to defaults. Changing mode to Airborne with
 159     0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00,           // <1g acceleration and capturing the data from the U-Center binary console.
 160     0x00, 0xC8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1A, 0x28,
 161
 162     //Default Airborne_4g Dynamic Platform Model
 163     #else
 164     0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x08, 0x03, 0x00,           // CFG-NAV5 - Set engine settings
 165     0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00,           // Collected by resetting a GPS unit to defaults. Changing mode to Airborne with
 166     0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00,           // <4g acceleration and capturing the data from the U-Center binary console.
 167     0x00, 0xC8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1C, 0x6C,
 168     #endif
 169 };
 170
 171 // UBlox 6 Protocol documentation - GPS.G6-SW-10018-F
 172 // SBAS Configuration Settings Desciption, Page 4/210
 173 // 31.21 CFG-SBAS (0x06 0x16), Page 142/210
 174 // A.10 SBAS Configuration (UBX-CFG-SBAS), Page 198/210 - GPS.G6-SW-10018-F
 175
 176 #define UBLOX_SBAS_PREFIX_LENGTH 10
 177
 178 static const uint8_t ubloxSbasPrefix[UBLOX_SBAS_PREFIX_LENGTH] = { 0xB5, 0x62, 0x06, 0x16, 0x08, 0x00, 0x03, 0x07, 0x03, 0x00 };
 179
 180 #define UBLOX_SBAS_MESSAGE_LENGTH 6
 181 typedef struct ubloxSbas_s {
 182     sbasMode_e mode;
 183     uint8_t message[UBLOX_SBAS_MESSAGE_LENGTH];
 184 } ubloxSbas_t;
 185
 186
 187
 188 // Note: these must be defined in the same order is sbasMode_e since no lookup table is used.
 189 static const ubloxSbas_t ubloxSbas[] = {
 190     // NOTE this could be optimized to save a few bytes of flash space since the same prefix is used for each command.
 191     { SBAS_AUTO,  { 0x00, 0x00, 0x00, 0x00, 0x31, 0xE5}},
 192     { SBAS_EGNOS, { 0x51, 0x08, 0x00, 0x00, 0x8A, 0x41}},
 193     { SBAS_WAAS,  { 0x04, 0xE0, 0x04, 0x00, 0x19, 0x9D}},
 194     { SBAS_MSAS,  { 0x00, 0x02, 0x02, 0x00, 0x35, 0xEF}},
 195     { SBAS_GAGAN, { 0x80, 0x01, 0x00, 0x00, 0xB2, 0xE8}}
 196 };
 197
 198 // Remove QZSS and add Galileo (only 3 GNSS systems supported simultaneously)
 199 // Frame captured from uCenter
 200 static const uint8_t ubloxGalileoInit[] = {
 201         0xB5, 0x62, 0x06, 0x3E, 0x3C,                       // UBX-CGF-GNSS
 202         0x00, 0x00, 0x20, 0x20, 0x07,                       // GNSS
 203         0x00, 0x08, 0x10, 0x00, 0x01, 0x00, 0x01, 0x01,     // GPS
 204         0x01, 0x01, 0x03, 0x00, 0x01, 0x00, 0x01, 0x01,     // SBAS
 205         0x02, 0x04, 0x08, 0x00, 0x01, 0x00, 0x01, 0x01,     // Galileo
 206         0x03, 0x08, 0x10, 0x00, 0x00, 0x00, 0x01, 0x01,     // BeiDou
 207         0x04, 0x00, 0x08, 0x00, 0x00, 0x00, 0x01, 0x03,     // IMES
 208         0x05, 0x00, 0x03, 0x00, 0x00, 0x00, 0x01, 0x05,     // QZSS
 209         0x06, 0x08, 0x0E, 0x00, 0x01, 0x00, 0x01, 0x01,     // GLONASS
 210         0x55, 0x47
 211 };
 212 #endif // USE_GPS_UBLOX
 213
 214 typedef enum {
 215     GPS_UNKNOWN,
 216     GPS_INITIALIZING,
 217     GPS_INITIALIZED,
 218     GPS_CHANGE_BAUD,
 219     GPS_CONFIGURE,
 220     GPS_RECEIVING_DATA,
 221     GPS_LOST_COMMUNICATION
 222 } gpsState_e;
 223
 224 gpsData_t gpsData;
 225
 226
 227 PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0);
 228
 229 PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig,
 230     .provider = GPS_NMEA,
 231     .sbasMode = SBAS_AUTO,
 232     .autoConfig = GPS_AUTOCONFIG_ON,
 233     .autoBaud = GPS_AUTOBAUD_OFF,
 234     .gps_ublox_use_galileo = false,
 235     .gps_set_home_point_once = false,
 236     .gps_use_3d_speed = false
 237 );
 238
 239 static void shiftPacketLog(void)
 240 {
 241     uint32_t i;
 242
 243     for (i = ARRAYLEN(gpsPacketLog) - 1; i > 0 ; i--) {
 244         gpsPacketLog[i] = gpsPacketLog[i-1];
 245     }
 246 }
 247
 248 static void gpsNewData(uint16_t c);
 249 #ifdef USE_GPS_NMEA
 250 static bool gpsNewFrameNMEA(char c);
 251 #endif
 252 #ifdef USE_GPS_UBLOX
 253 static bool gpsNewFrameUBLOX(uint8_t data);
 254 #endif
 255
 256 static void gpsSetState(gpsState_e state)
 257 {
 258     gpsData.state = state;
 259     gpsData.state_position = 0;
 260     gpsData.state_ts = millis();
 261     gpsData.messageState = GPS_MESSAGE_STATE_IDLE;
 262 }
 263
 264 void gpsInit(void)
 265 {
 266     gpsData.baudrateIndex = 0;
 267     gpsData.errors = 0;
 268     gpsData.timeouts = 0;
 269
 270     memset(gpsPacketLog, 0x00, sizeof(gpsPacketLog));
 271
 272     // init gpsData structure. if we're not actually enabled, don't bother doing anything else
 273     gpsSetState(GPS_UNKNOWN);
 274
 275     gpsData.lastMessage = millis();
 276
 277     if (gpsConfig()->provider == GPS_MSP) { // no serial ports used when GPS_MSP is configured
 278         gpsSetState(GPS_INITIALIZED);
 279         return;
 280     }
 281
 282     serialPortConfig_t *gpsPortConfig = findSerialPortConfig(FUNCTION_GPS);
 283     if (!gpsPortConfig) {
 284         return;
 285     }
 286
 287     while (gpsInitData[gpsData.baudrateIndex].baudrateIndex != gpsPortConfig->gps_baudrateIndex) {
 288         gpsData.baudrateIndex++;
 289         if (gpsData.baudrateIndex >= GPS_INIT_DATA_ENTRY_COUNT) {
 290             gpsData.baudrateIndex = DEFAULT_BAUD_RATE_INDEX;
 291             break;
 292         }
 293     }
 294
 295     portMode_e mode = MODE_RXTX;
 296 #if defined(GPS_NMEA_TX_ONLY)
 297     if (gpsConfig()->provider == GPS_NMEA) {
 298         mode &= ~MODE_TX;
 299     }
 300 #endif
 301
 302     // no callback - buffer will be consumed in gpsUpdate()
 303     gpsPort = openSerialPort(gpsPortConfig->identifier, FUNCTION_GPS, NULL, NULL, baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex], mode, SERIAL_NOT_INVERTED);
 304     if (!gpsPort) {
 305         return;
 306     }
 307
 308     // signal GPS "thread" to initialize when it gets to it
 309     gpsSetState(GPS_INITIALIZING);
 310 }
 311
 312 #ifdef USE_GPS_NMEA
 313 void gpsInitNmea(void)
 314 {
 315 #if !defined(GPS_NMEA_TX_ONLY)
 316     uint32_t now;
 317 #endif
 318     switch (gpsData.state) {
 319         case GPS_INITIALIZING:
 320 #if !defined(GPS_NMEA_TX_ONLY)
 321            now = millis();
 322            if (now - gpsData.state_ts < 1000) {
 323                return;
 324            }
 325            gpsData.state_ts = now;
 326            if (gpsData.state_position < 1) {
 327                serialSetBaudRate(gpsPort, 4800);
 328                gpsData.state_position++;
 329            } else if (gpsData.state_position < 2) {
 330                // print our FIXED init string for the baudrate we want to be at
 331                serialPrint(gpsPort, "$PSRF100,1,115200,8,1,0*05\r\n");
 332                gpsData.state_position++;
 333            } else {
 334                // we're now (hopefully) at the correct rate, next state will switch to it
 335                gpsSetState(GPS_CHANGE_BAUD);
 336            }
 337            break;
 338 #endif
 339         case GPS_CHANGE_BAUD:
 340 #if !defined(GPS_NMEA_TX_ONLY)
 341            now = millis();
 342            if (now - gpsData.state_ts < 1000) {
 343                return;
 344            }
 345            gpsData.state_ts = now;
 346            if (gpsData.state_position < 1) {
 347                serialSetBaudRate(gpsPort, baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex]);
 348                gpsData.state_position++;
 349            } else if (gpsData.state_position < 2) {
 350                serialPrint(gpsPort, "$PSRF103,00,6,00,0*23\r\n");
 351                gpsData.state_position++;
 352            } else
 353 #else
 354            {
 355                serialSetBaudRate(gpsPort, baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex]);
 356            }
 357 #endif
 358                gpsSetState(GPS_RECEIVING_DATA);
 359             break;
 360     }
 361 }
 362 #endif // USE_GPS_NMEA
 363
 364 #ifdef USE_GPS_UBLOX
 365 void gpsInitUblox(void)
 366 {
 367     uint32_t now;
 368     // UBX will run at the serial port's baudrate, it shouldn't be "autodetected". So here we force it to that rate
 369
 370     // Wait until GPS transmit buffer is empty
 371     if (!isSerialTransmitBufferEmpty(gpsPort))
 372         return;
 373
 374
 375     switch (gpsData.state) {
 376         case GPS_INITIALIZING:
 377             now = millis();
 378             if (now - gpsData.state_ts < GPS_BAUDRATE_CHANGE_DELAY)
 379                 return;
 380
 381             if (gpsData.state_position < GPS_INIT_ENTRIES) {
 382                 // try different speed to INIT
 383                 baudRate_e newBaudRateIndex = gpsInitData[gpsData.state_position].baudrateIndex;
 384
 385                 gpsData.state_ts = now;
 386
 387                 if (lookupBaudRateIndex(serialGetBaudRate(gpsPort)) != newBaudRateIndex) {
 388                     // change the rate if needed and wait a little
 389                     serialSetBaudRate(gpsPort, baudRates[newBaudRateIndex]);
 390                     return;
 391                 }
 392
 393                 // print our FIXED init string for the baudrate we want to be at
 394                 serialPrint(gpsPort, gpsInitData[gpsData.baudrateIndex].ubx);
 395
 396                 gpsData.state_position++;
 397             } else {
 398                 // we're now (hopefully) at the correct rate, next state will switch to it
 399                 gpsSetState(GPS_CHANGE_BAUD);
 400             }
 401             break;
 402         case GPS_CHANGE_BAUD:
 403             serialSetBaudRate(gpsPort, baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex]);
 404             gpsSetState(GPS_CONFIGURE);
 405             break;
 406         case GPS_CONFIGURE:
 407
 408             // Either use specific config file for GPS or let dynamically upload config
 409             if ( gpsConfig()->autoConfig == GPS_AUTOCONFIG_OFF ) {
 410                 gpsSetState(GPS_RECEIVING_DATA);
 411                 break;
 412             }
 413
 414             if (gpsData.messageState == GPS_MESSAGE_STATE_IDLE) {
 415                 gpsData.messageState++;
 416             }
 417
 418             if (gpsData.messageState == GPS_MESSAGE_STATE_INIT) {
 419
 420                 if (gpsData.state_position < sizeof(ubloxInit)) {
 421                     serialWrite(gpsPort, ubloxInit[gpsData.state_position]);
 422                     gpsData.state_position++;
 423                 } else {
 424                     gpsData.state_position = 0;
 425                     gpsData.messageState++;
 426                 }
 427             }
 428
 429             if (gpsData.messageState == GPS_MESSAGE_STATE_SBAS) {
 430                 if (gpsData.state_position < UBLOX_SBAS_PREFIX_LENGTH) {
 431                     serialWrite(gpsPort, ubloxSbasPrefix[gpsData.state_position]);
 432                     gpsData.state_position++;
 433                 } else if (gpsData.state_position < UBLOX_SBAS_PREFIX_LENGTH + UBLOX_SBAS_MESSAGE_LENGTH) {
 434                     serialWrite(gpsPort, ubloxSbas[gpsConfig()->sbasMode].message[gpsData.state_position - UBLOX_SBAS_PREFIX_LENGTH]);
 435                     gpsData.state_position++;
 436                 } else {
 437                     gpsData.state_position = 0;
 438                     gpsData.messageState++;
 439                 }
 440             }
 441
 442             if (gpsData.messageState == GPS_MESSAGE_STATE_GALILEO) {
 443                 if ((gpsConfig()->gps_ublox_use_galileo) && (gpsData.state_position < sizeof(ubloxGalileoInit))) {
 444                     serialWrite(gpsPort, ubloxGalileoInit[gpsData.state_position]);
 445                     gpsData.state_position++;
 446                 } else {
 447                     gpsData.state_position = 0;
 448                     gpsData.messageState++;
 449                 }
 450             }
 451
 452             if (gpsData.messageState >= GPS_MESSAGE_STATE_INITIALIZED) {
 453                 // ublox should be initialised, try receiving
 454                 gpsSetState(GPS_RECEIVING_DATA);
 455             }
 456             break;
 457     }
 458 }
 459 #endif // USE_GPS_UBLOX
 460
 461 void gpsInitHardware(void)
 462 {
 463     switch (gpsConfig()->provider) {
 464     case GPS_NMEA:
 465 #ifdef USE_GPS_NMEA
 466         gpsInitNmea();
 467 #endif
 468         break;
 469
 470     case GPS_UBLOX:
 471 #ifdef USE_GPS_UBLOX
 472         gpsInitUblox();
 473 #endif
 474         break;
 475     default:
 476         break;
 477     }
 478 }
 479
 480 static void updateGpsIndicator(timeUs_t currentTimeUs)
 481 {
 482     static uint32_t GPSLEDTime;
 483     if ((int32_t)(currentTimeUs - GPSLEDTime) >= 0 && (gpsSol.numSat >= 5)) {
 484         GPSLEDTime = currentTimeUs + 150000;
 485         LED1_TOGGLE;
 486     }
 487 }
 488
 489 void gpsUpdate(timeUs_t currentTimeUs)
 490 {
 491     // read out available GPS bytes
 492     if (gpsPort) {
 493         while (serialRxBytesWaiting(gpsPort))
 494             gpsNewData(serialRead(gpsPort));
 495     } else if (GPS_update & GPS_MSP_UPDATE) { // GPS data received via MSP
 496         gpsSetState(GPS_RECEIVING_DATA);
 497         gpsData.lastMessage = millis();
 498         sensorsSet(SENSOR_GPS);
 499         onGpsNewData();
 500         GPS_update &= ~GPS_MSP_UPDATE;
 501     }
 502
 503     switch (gpsData.state) {
 504         case GPS_UNKNOWN:
 505         case GPS_INITIALIZED:
 506             break;
 507
 508         case GPS_INITIALIZING:
 509         case GPS_CHANGE_BAUD:
 510         case GPS_CONFIGURE:
 511             gpsInitHardware();
 512             break;
 513
 514         case GPS_LOST_COMMUNICATION:
 515             gpsData.timeouts++;
 516             if (gpsConfig()->autoBaud) {
 517                 // try another rate
 518                 gpsData.baudrateIndex++;
 519                 gpsData.baudrateIndex %= GPS_INIT_ENTRIES;
 520             }
 521             gpsData.lastMessage = millis();
 522             gpsSol.numSat = 0;
 523             DISABLE_STATE(GPS_FIX);
 524             gpsSetState(GPS_INITIALIZING);
 525             break;
 526
 527         case GPS_RECEIVING_DATA:
 528             // check for no data/gps timeout/cable disconnection etc
 529             if (millis() - gpsData.lastMessage > GPS_TIMEOUT) {
 530                 // remove GPS from capability
 531                 sensorsClear(SENSOR_GPS);
 532                 gpsSetState(GPS_LOST_COMMUNICATION);
 533 #if !defined(GPS_UBLOX_MODE_PEDESTRIAN)
 534             } else {
 535                 if ((gpsData.messageState == GPS_MESSAGE_STATE_IDLE) && STATE(GPS_FIX)) {
 536                     gpsData.messageState = GPS_MESSAGE_STATE_AIRBORNE;
 537                     gpsData.state_position = 0;
 538                 }
 539                 if (gpsData.messageState == GPS_MESSAGE_STATE_AIRBORNE) {
 540                     if (gpsData.state_position < sizeof(ubloxAirborne)) {
 541                         serialWrite(gpsPort, ubloxAirborne[gpsData.state_position]);
 542                         gpsData.state_position++;
 543                     } else {
 544                         gpsData.messageState = GPS_MESSAGE_STATE_ENTRY_COUNT;
 545                     }
 546                 }
 547 #endif
 548             }
 549             break;
 550     }
 551     if (sensors(SENSOR_GPS)) {
 552         updateGpsIndicator(currentTimeUs);
 553     }
 554     if (!ARMING_FLAG(ARMED) && !gpsConfig()->gps_set_home_point_once) {
 555         DISABLE_STATE(GPS_FIX_HOME);
 556     }
 557 #if defined(USE_GPS_RESCUE)
 558     if (gpsRescueIsConfigured()) {
 559         updateGPSRescueState();
 560     }
 561 #endif
 562 }
 563
 564 static void gpsNewData(uint16_t c)
 565 {
 566     if (!gpsNewFrame(c)) {
 567         return;
 568     }
 569
 570     // new data received and parsed, we're in business
 571     gpsData.lastLastMessage = gpsData.lastMessage;
 572     gpsData.lastMessage = millis();
 573     sensorsSet(SENSOR_GPS);
 574
 575     GPS_update ^= GPS_DIRECT_TICK;
 576
 577 #if 0
 578     debug[3] = GPS_update;
 579 #endif
 580
 581     onGpsNewData();
 582 }
 583
 584 bool gpsNewFrame(uint8_t c)
 585 {
 586     switch (gpsConfig()->provider) {
 587     case GPS_NMEA:          // NMEA
 588 #ifdef USE_GPS_NMEA
 589         return gpsNewFrameNMEA(c);
 590 #endif
 591         break;
 592     case GPS_UBLOX:         // UBX binary
 593 #ifdef USE_GPS_UBLOX
 594         return gpsNewFrameUBLOX(c);
 595 #endif
 596         break;
 597     default:
 598         break;
 599     }
 600     return false;
 601 }
 602
 603 // Check for healthy communications
 604 bool gpsIsHealthy()
 605 {
 606     return (gpsData.state == GPS_RECEIVING_DATA);
 607 }
 608
 609 /* This is a light implementation of a GPS frame decoding
 610    This should work with most of modern GPS devices configured to output 5 frames.
 611    It assumes there are some NMEA GGA frames to decode on the serial bus
 612    Now verifies checksum correctly before applying data
 613
 614    Here we use only the following data :
 615      - latitude
 616      - longitude
 617      - GPS fix is/is not ok
 618      - GPS num sat (4 is enough to be +/- reliable)
 619      // added by Mis
 620      - GPS altitude (for OSD displaying)
 621      - GPS speed (for OSD displaying)
 622 */
 623
 624 #define NO_FRAME   0
 625 #define FRAME_GGA  1
 626 #define FRAME_RMC  2
 627 #define FRAME_GSV  3
 628
 629
 630 // This code is used for parsing NMEA data
 631
 632 /* Alex optimization
 633   The latitude or longitude is coded this way in NMEA frames
 634   dm.f   coded as degrees + minutes + minute decimal
 635   Where:
 636     - d can be 1 or more char long. generally: 2 char long for latitude, 3 char long for longitude
 637     - m is always 2 char long
 638     - f can be 1 or more char long
 639   This function converts this format in a unique unsigned long where 1 degree = 10 000 000
 640
 641   EOS increased the precision here, even if we think that the gps is not precise enough, with 10e5 precision it has 76cm resolution
 642   with 10e7 it's around 1 cm now. Increasing it further is irrelevant, since even 1cm resolution is unrealistic, however increased
 643   resolution also increased precision of nav calculations
 644 static uint32_t GPS_coord_to_degrees(char *coordinateString)
 645 {
 646     char *p = s, *d = s;
 647     uint8_t min, deg = 0;
 648     uint16_t frac = 0, mult = 10000;
 649
 650     while (*p) {                // parse the string until its end
 651         if (d != s) {
 652             frac += (*p - '0') * mult;  // calculate only fractional part on up to 5 digits  (d != s condition is true when the . is located)
 653             mult /= 10;
 654         }
 655         if (*p == '.')
 656             d = p;              // locate '.' char in the string
 657         p++;
 658     }
 659     if (p == s)
 660         return 0;
 661     while (s < d - 2) {
 662         deg *= 10;              // convert degrees : all chars before minutes ; for the first iteration, deg = 0
 663         deg += *(s++) - '0';
 664     }
 665     min = *(d - 1) - '0' + (*(d - 2) - '0') * 10;       // convert minutes : 2 previous char before '.'
 666     return deg * 10000000UL + (min * 100000UL + frac) * 10UL / 6;
 667 }
 668 */
 669
 670 // helper functions
 671 #ifdef USE_GPS_NMEA
 672 static uint32_t grab_fields(char *src, uint8_t mult)
 673 {                               // convert string to uint32
 674     uint32_t i;
 675     uint32_t tmp = 0;
 676     int isneg = 0;
 677     for (i = 0; src[i] != 0; i++) {
 678         if ((i == 0) && (src[0] == '-')) { // detect negative sign
 679             isneg = 1;
 680             continue; // jump to next character if the first one was a negative sign
 681         }
 682         if (src[i] == '.') {
 683             i++;
 684             if (mult == 0) {
 685                 break;
 686             } else {
 687                 src[i + mult] = 0;
 688             }
 689         }
 690         tmp *= 10;
 691         if (src[i] >= '0' && src[i] <= '9') {
 692             tmp += src[i] - '0';
 693         }
 694         if (i >= 15) {
 695             return 0; // out of bounds
 696         }
 697     }
 698     return isneg ? -tmp : tmp;    // handle negative altitudes
 699 }
 700
 701 typedef struct gpsDataNmea_s {
 702     int32_t latitude;
 703     int32_t longitude;
 704     uint8_t numSat;
 705     int32_t altitudeCm;
 706     uint16_t speed;
 707     uint16_t hdop;
 708     uint16_t ground_course;
 709     uint32_t time;
 710     uint32_t date;
 711 } gpsDataNmea_t;
 712
 713 static bool gpsNewFrameNMEA(char c)
 714 {
 715     static gpsDataNmea_t gps_Msg;
 716
 717     uint8_t frameOK = 0;
 718     static uint8_t param = 0, offset = 0, parity = 0;
 719     static char string[15];
 720     static uint8_t checksum_param, gps_frame = NO_FRAME;
 721     static uint8_t svMessageNum = 0;
 722     uint8_t svSatNum = 0, svPacketIdx = 0, svSatParam = 0;
 723
 724     switch (c) {
 725         case '$':
 726             param = 0;
 727             offset = 0;
 728             parity = 0;
 729             break;
 730         case ',':
 731         case '*':
 732             string[offset] = 0;
 733             if (param == 0) {       //frame identification
 734                 gps_frame = NO_FRAME;
 735                 if (0 == strcmp(string, "GPGGA") || 0 == strcmp(string, "GNGGA")) {
 736                     gps_frame = FRAME_GGA;
 737                 } else if (0 == strcmp(string, "GPRMC") || 0 == strcmp(string, "GNRMC")) {
 738                     gps_frame = FRAME_RMC;
 739                 } else if (0 == strcmp(string, "GPGSV")) {
 740                     gps_frame = FRAME_GSV;
 741                 }
 742             }
 743
 744             switch (gps_frame) {
 745                 case FRAME_GGA:        //************* GPGGA FRAME parsing
 746                     switch (param) {
 747             //          case 1:             // Time information
 748             //              break;
 749                         case 2:
 750                             gps_Msg.latitude = GPS_coord_to_degrees(string);
 751                             break;
 752                         case 3:
 753                             if (string[0] == 'S')
 754                                 gps_Msg.latitude *= -1;
 755                             break;
 756                         case 4:
 757                             gps_Msg.longitude = GPS_coord_to_degrees(string);
 758                             break;
 759                         case 5:
 760                             if (string[0] == 'W')
 761                                 gps_Msg.longitude *= -1;
 762                             break;
 763                         case 6:
 764                             if (string[0] > '0') {
 765                                 ENABLE_STATE(GPS_FIX);
 766                             } else {
 767                                 DISABLE_STATE(GPS_FIX);
 768                             }
 769                             break;
 770                         case 7:
 771                             gps_Msg.numSat = grab_fields(string, 0);
 772                             break;
 773                         case 8:
 774                             gps_Msg.hdop = grab_fields(string, 1) * 100;          // hdop
 775                             break;
 776                         case 9:
 777                             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
 778                             break;
 779                     }
 780                     break;
 781                 case FRAME_RMC:        //************* GPRMC FRAME parsing
 782                     switch (param) {
 783                         case 1:
 784                             gps_Msg.time = grab_fields(string, 2); // UTC time hhmmss.ss
 785                             break;
 786                         case 7:
 787                             gps_Msg.speed = ((grab_fields(string, 1) * 5144L) / 1000L);    // speed in cm/s added by Mis
 788                             break;
 789                         case 8:
 790                             gps_Msg.ground_course = (grab_fields(string, 1));      // ground course deg * 10
 791                             break;
 792                         case 9:
 793                             gps_Msg.date = grab_fields(string, 0); // date dd/mm/yy
 794                             break;
 795                     }
 796                     break;
 797                 case FRAME_GSV:
 798                     switch (param) {
 799                       /*case 1:
 800                             // Total number of messages of this type in this cycle
 801                             break; */
 802                         case 2:
 803                             // Message number
 804                             svMessageNum = grab_fields(string, 0);
 805                             break;
 806                         case 3:
 807                             // Total number of SVs visible
 808                             GPS_numCh = grab_fields(string, 0);
 809                             break;
 810                     }
 811                     if (param < 4)
 812                         break;
 813
 814                     svPacketIdx = (param - 4) / 4 + 1; // satellite number in packet, 1-4
 815                     svSatNum    = svPacketIdx + (4 * (svMessageNum - 1)); // global satellite number
 816                     svSatParam  = param - 3 - (4 * (svPacketIdx - 1)); // parameter number for satellite
 817
 818                     if (svSatNum > GPS_SV_MAXSATS)
 819                         break;
 820
 821                     switch (svSatParam) {
 822                         case 1:
 823                             // SV PRN number
 824                             GPS_svinfo_chn[svSatNum - 1]  = svSatNum;
 825                             GPS_svinfo_svid[svSatNum - 1] = grab_fields(string, 0);
 826                             break;
 827                       /*case 2:
 828                             // Elevation, in degrees, 90 maximum
 829                             break;
 830                         case 3:
 831                             // Azimuth, degrees from True North, 000 through 359
 832                             break; */
 833                         case 4:
 834                             // SNR, 00 through 99 dB (null when not tracking)
 835                             GPS_svinfo_cno[svSatNum - 1] = grab_fields(string, 0);
 836                             GPS_svinfo_quality[svSatNum - 1] = 0; // only used by ublox
 837                             break;
 838                     }
 839
 840                     GPS_svInfoReceivedCount++;
 841
 842                     break;
 843             }
 844
 845             param++;
 846             offset = 0;
 847             if (c == '*')
 848                 checksum_param = 1;
 849             else
 850                 parity ^= c;
 851             break;
 852         case '\r':
 853         case '\n':
 854             if (checksum_param) {   //parity checksum
 855                 shiftPacketLog();
 856                 uint8_t checksum = 16 * ((string[0] >= 'A') ? string[0] - 'A' + 10 : string[0] - '0') + ((string[1] >= 'A') ? string[1] - 'A' + 10 : string[1] - '0');
 857                 if (checksum == parity) {
 858                     *gpsPacketLogChar = LOG_IGNORED;
 859                     GPS_packetCount++;
 860                     switch (gps_frame) {
 861                     case FRAME_GGA:
 862                       *gpsPacketLogChar = LOG_NMEA_GGA;
 863                       frameOK = 1;
 864                       if (STATE(GPS_FIX)) {
 865                             gpsSol.llh.lat = gps_Msg.latitude;
 866                             gpsSol.llh.lon = gps_Msg.longitude;
 867                             gpsSol.numSat = gps_Msg.numSat;
 868                             gpsSol.llh.altCm = gps_Msg.altitudeCm;
 869                             gpsSol.hdop = gps_Msg.hdop;
 870                         }
 871                         break;
 872                     case FRAME_RMC:
 873                         *gpsPacketLogChar = LOG_NMEA_RMC;
 874                         gpsSol.groundSpeed = gps_Msg.speed;
 875                         gpsSol.groundCourse = gps_Msg.ground_course;
 876 #ifdef USE_RTC_TIME
 877                         // This check will miss 00:00:00.00, but we shouldn't care - next report will be valid
 878                         if(!rtcHasTime() && gps_Msg.date != 0 && gps_Msg.time != 0) {
 879                             dateTime_t temp_time;
 880                             temp_time.year = (gps_Msg.date % 100) + 2000;
 881                             temp_time.month = (gps_Msg.date / 100) % 100;
 882                             temp_time.day = (gps_Msg.date / 10000) % 100;
 883                             temp_time.hours = (gps_Msg.time / 1000000) % 100;
 884                             temp_time.minutes = (gps_Msg.time / 10000) % 100;
 885                             temp_time.seconds = (gps_Msg.time / 100) % 100;
 886                             temp_time.millis = (gps_Msg.time & 100) * 10;
 887                             rtcSetDateTime(&temp_time);
 888                         }
 889 #endif
 890                         break;
 891                     } // end switch
 892                 } else {
 893                     *gpsPacketLogChar = LOG_ERROR;
 894                 }
 895             }
 896             checksum_param = 0;
 897             break;
 898         default:
 899             if (offset < 15)
 900                 string[offset++] = c;
 901             if (!checksum_param)
 902                 parity ^= c;
 903     }
 904     return frameOK;
 905 }
 906 #endif // USE_GPS_NMEA
 907
 908 #ifdef USE_GPS_UBLOX
 909 // UBX support
 910 typedef struct {
 911     uint8_t preamble1;
 912     uint8_t preamble2;
 913     uint8_t msg_class;
 914     uint8_t msg_id;
 915     uint16_t length;
 916 } ubx_header;
 917
 918 typedef struct {
 919     uint32_t time;              // GPS msToW
 920     int32_t longitude;
 921     int32_t latitude;
 922     int32_t altitude_ellipsoid;
 923     int32_t altitudeMslMm;
 924     uint32_t horizontal_accuracy;
 925     uint32_t vertical_accuracy;
 926 } ubx_nav_posllh;
 927
 928 typedef struct {
 929     uint32_t time;              // GPS msToW
 930     uint8_t fix_type;
 931     uint8_t fix_status;
 932     uint8_t differential_status;
 933     uint8_t res;
 934     uint32_t time_to_first_fix;
 935     uint32_t uptime;            // milliseconds
 936 } ubx_nav_status;
 937
 938 typedef struct {
 939     uint32_t time;
 940     int32_t time_nsec;
 941     int16_t week;
 942     uint8_t fix_type;
 943     uint8_t fix_status;
 944     int32_t ecef_x;
 945     int32_t ecef_y;
 946     int32_t ecef_z;
 947     uint32_t position_accuracy_3d;
 948     int32_t ecef_x_velocity;
 949     int32_t ecef_y_velocity;
 950     int32_t ecef_z_velocity;
 951     uint32_t speed_accuracy;
 952     uint16_t position_DOP;
 953     uint8_t res;
 954     uint8_t satellites;
 955     uint32_t res2;
 956 } ubx_nav_solution;
 957
 958 typedef struct {
 959     uint32_t time;              // GPS msToW
 960     int32_t ned_north;
 961     int32_t ned_east;
 962     int32_t ned_down;
 963     uint32_t speed_3d;
 964     uint32_t speed_2d;
 965     int32_t heading_2d;
 966     uint32_t speed_accuracy;
 967     uint32_t heading_accuracy;
 968 } ubx_nav_velned;
 969
 970 typedef struct {
 971     uint8_t chn;                // Channel number, 255 for SVx not assigned to channel
 972     uint8_t svid;               // Satellite ID
 973     uint8_t flags;              // Bitmask
 974     uint8_t quality;            // Bitfield
 975     uint8_t cno;                // Carrier to Noise Ratio (Signal Strength) // dbHz, 0-55.
 976     uint8_t elev;               // Elevation in integer degrees
 977     int16_t azim;               // Azimuth in integer degrees
 978     int32_t prRes;              // Pseudo range residual in centimetres
 979 } ubx_nav_svinfo_channel;
 980
 981 typedef struct {
 982     uint32_t time;              // GPS Millisecond time of week
 983     uint8_t numCh;              // Number of channels
 984     uint8_t globalFlags;        // Bitmask, Chip hardware generation 0:Antaris, 1:u-blox 5, 2:u-blox 6
 985     uint16_t reserved2;         // Reserved
 986     ubx_nav_svinfo_channel channel[16];         // 16 satellites * 12 byte
 987 } ubx_nav_svinfo;
 988
 989 enum {
 990     PREAMBLE1 = 0xb5,
 991     PREAMBLE2 = 0x62,
 992     CLASS_NAV = 0x01,
 993     CLASS_ACK = 0x05,
 994     CLASS_CFG = 0x06,
 995     MSG_ACK_NACK = 0x00,
 996     MSG_ACK_ACK = 0x01,
 997     MSG_POSLLH = 0x2,
 998     MSG_STATUS = 0x3,
 999     MSG_SOL = 0x6,
1000     MSG_VELNED = 0x12,
1001     MSG_SVINFO = 0x30,
1002     MSG_CFG_PRT = 0x00,
1003     MSG_CFG_RATE = 0x08,
1004     MSG_CFG_SET_RATE = 0x01,
1005     MSG_CFG_NAV_SETTINGS = 0x24
1006 } ubx_protocol_bytes;
1007
1008 enum {
1009     FIX_NONE = 0,
1010     FIX_DEAD_RECKONING = 1,
1011     FIX_2D = 2,
1012     FIX_3D = 3,
1013     FIX_GPS_DEAD_RECKONING = 4,
1014     FIX_TIME = 5
1015 } ubs_nav_fix_type;
1016
1017 enum {
1018     NAV_STATUS_FIX_VALID = 1,
1019     NAV_STATUS_TIME_WEEK_VALID = 4,
1020     NAV_STATUS_TIME_SECOND_VALID = 8
1021 } ubx_nav_status_bits;
1022
1023 // Packet checksum accumulators
1024 static uint8_t _ck_a;
1025 static uint8_t _ck_b;
1026
1027 // State machine state
1028 static bool _skip_packet;
1029 static uint8_t _step;
1030 static uint8_t _msg_id;
1031 static uint16_t _payload_length;
1032 static uint16_t _payload_counter;
1033
1034 static bool next_fix;
1035 static uint8_t _class;
1036
1037 // do we have new position information?
1038 static bool _new_position;
1039
1040 // do we have new speed information?
1041 static bool _new_speed;
1042
1043 // Example packet sizes from UBlox u-center from a Glonass capable GPS receiver.
1044 //15:17:55  R -> UBX NAV-STATUS,  Size  24,  'Navigation Status'
1045 //15:17:55  R -> UBX NAV-POSLLH,  Size  36,  'Geodetic Position'
1046 //15:17:55  R -> UBX NAV-VELNED,  Size  44,  'Velocity in WGS 84'
1047 //15:17:55  R -> UBX NAV-CLOCK,  Size  28,  'Clock Status'
1048 //15:17:55  R -> UBX NAV-AOPSTATUS,  Size  24,  'AOP Status'
1049 //15:17:55  R -> UBX 03-09,  Size 208,  'Unknown'
1050 //15:17:55  R -> UBX 03-10,  Size 336,  'Unknown'
1051 //15:17:55  R -> UBX NAV-SOL,  Size  60,  'Navigation Solution'
1052 //15:17:55  R -> UBX NAV,  Size 100,  'Navigation'
1053 //15:17:55  R -> UBX NAV-SVINFO,  Size 328,  'Satellite Status and Information'
1054
1055 // from the UBlox6 document, the largest payout we receive i the NAV-SVINFO and the payload size
1056 // is calculated as 8 + 12*numCh.  numCh in the case of a Glonass receiver is 28.
1057 #define UBLOX_PAYLOAD_SIZE 344
1058
1059
1060 // Receive buffer
1061 static union {
1062     ubx_nav_posllh posllh;
1063     ubx_nav_status status;
1064     ubx_nav_solution solution;
1065     ubx_nav_velned velned;
1066     ubx_nav_svinfo svinfo;
1067     uint8_t bytes[UBLOX_PAYLOAD_SIZE];
1068 } _buffer;
1069
1070 void _update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b)
1071 {
1072     while (len--) {
1073         *ck_a += *data;
1074         *ck_b += *ck_a;
1075         data++;
1076     }
1077 }
1078
1079
1080 static bool UBLOX_parse_gps(void)
1081 {
1082     uint32_t i;
1083
1084     *gpsPacketLogChar = LOG_IGNORED;
1085
1086     switch (_msg_id) {
1087     case MSG_POSLLH:
1088         *gpsPacketLogChar = LOG_UBLOX_POSLLH;
1089         //i2c_dataset.time                = _buffer.posllh.time;
1090         gpsSol.llh.lon = _buffer.posllh.longitude;
1091         gpsSol.llh.lat = _buffer.posllh.latitude;
1092         gpsSol.llh.altCm = _buffer.posllh.altitudeMslMm / 10;  //alt in cm
1093         if (next_fix) {
1094             ENABLE_STATE(GPS_FIX);
1095         } else {
1096             DISABLE_STATE(GPS_FIX);
1097         }
1098         _new_position = true;
1099         break;
1100     case MSG_STATUS:
1101         *gpsPacketLogChar = LOG_UBLOX_STATUS;
1102         next_fix = (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.status.fix_type == FIX_3D);
1103         if (!next_fix)
1104             DISABLE_STATE(GPS_FIX);
1105         break;
1106     case MSG_SOL:
1107         *gpsPacketLogChar = LOG_UBLOX_SOL;
1108         next_fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D);
1109         if (!next_fix)
1110             DISABLE_STATE(GPS_FIX);
1111         gpsSol.numSat = _buffer.solution.satellites;
1112         gpsSol.hdop = _buffer.solution.position_DOP;
1113 #ifdef USE_RTC_TIME
1114         //set clock, when gps time is available
1115         if(!rtcHasTime() && (_buffer.solution.fix_status & NAV_STATUS_TIME_SECOND_VALID) && (_buffer.solution.fix_status & NAV_STATUS_TIME_WEEK_VALID)) {
1116             //calculate rtctime: week number * ms in a week + ms of week + fractions of second + offset to UNIX reference year - 18 leap seconds
1117             rtcTime_t temp_time = (((int64_t) _buffer.solution.week)*7*24*60*60*1000) + _buffer.solution.time + (_buffer.solution.time_nsec/1000000) + 315964800000LL - 18000;
1118             rtcSet(&temp_time);
1119         }
1120 #endif
1121         break;
1122     case MSG_VELNED:
1123         *gpsPacketLogChar = LOG_UBLOX_VELNED;
1124         gpsSol.speed3d = _buffer.velned.speed_3d;       // cm/s
1125         gpsSol.groundSpeed = _buffer.velned.speed_2d;    // cm/s
1126         gpsSol.groundCourse = (uint16_t) (_buffer.velned.heading_2d / 10000);     // Heading 2D deg * 100000 rescaled to deg * 10
1127         _new_speed = true;
1128         break;
1129     case MSG_SVINFO:
1130         *gpsPacketLogChar = LOG_UBLOX_SVINFO;
1131         GPS_numCh = _buffer.svinfo.numCh;
1132         if (GPS_numCh > 16)
1133             GPS_numCh = 16;
1134         for (i = 0; i < GPS_numCh; i++) {
1135             GPS_svinfo_chn[i]= _buffer.svinfo.channel[i].chn;
1136             GPS_svinfo_svid[i]= _buffer.svinfo.channel[i].svid;
1137             GPS_svinfo_quality[i]=_buffer.svinfo.channel[i].quality;
1138             GPS_svinfo_cno[i]= _buffer.svinfo.channel[i].cno;
1139         }
1140         GPS_svInfoReceivedCount++;
1141         break;
1142     default:
1143         return false;
1144     }
1145
1146     // we only return true when we get new position and speed data
1147     // this ensures we don't use stale data
1148     if (_new_position && _new_speed) {
1149         _new_speed = _new_position = false;
1150         return true;
1151     }
1152     return false;
1153 }
1154
1155 static bool gpsNewFrameUBLOX(uint8_t data)
1156 {
1157     bool parsed = false;
1158
1159     switch (_step) {
1160         case 0: // Sync char 1 (0xB5)
1161             if (PREAMBLE1 == data) {
1162                 _skip_packet = false;
1163                 _step++;
1164             }
1165             break;
1166         case 1: // Sync char 2 (0x62)
1167             if (PREAMBLE2 != data) {
1168                 _step = 0;
1169                 break;
1170             }
1171             _step++;
1172             break;
1173         case 2: // Class
1174             _step++;
1175             _class = data;
1176             _ck_b = _ck_a = data;   // reset the checksum accumulators
1177             break;
1178         case 3: // Id
1179             _step++;
1180             _ck_b += (_ck_a += data);       // checksum byte
1181             _msg_id = data;
1182             break;
1183         case 4: // Payload length (part 1)
1184             _step++;
1185             _ck_b += (_ck_a += data);       // checksum byte
1186             _payload_length = data; // payload length low byte
1187             break;
1188         case 5: // Payload length (part 2)
1189             _step++;
1190             _ck_b += (_ck_a += data);       // checksum byte
1191             _payload_length += (uint16_t)(data << 8);
1192             if (_payload_length > UBLOX_PAYLOAD_SIZE) {
1193                 _skip_packet = true;
1194             }
1195             _payload_counter = 0;   // prepare to receive payload
1196             if (_payload_length == 0) {
1197                 _step = 7;
1198             }
1199             break;
1200         case 6:
1201             _ck_b += (_ck_a += data);       // checksum byte
1202             if (_payload_counter < UBLOX_PAYLOAD_SIZE) {
1203                 _buffer.bytes[_payload_counter] = data;
1204             }
1205             if (++_payload_counter >= _payload_length) {
1206                 _step++;
1207             }
1208             break;
1209         case 7:
1210             _step++;
1211             if (_ck_a != data) {
1212                 _skip_packet = true;          // bad checksum
1213                 gpsData.errors++;
1214             }
1215             break;
1216         case 8:
1217             _step = 0;
1218
1219             shiftPacketLog();
1220
1221             if (_ck_b != data) {
1222                 *gpsPacketLogChar = LOG_ERROR;
1223                 gpsData.errors++;
1224                 break;              // bad checksum
1225             }
1226
1227             GPS_packetCount++;
1228
1229             if (_skip_packet) {
1230                 *gpsPacketLogChar = LOG_SKIPPED;
1231                 break;
1232             }
1233
1234             if (UBLOX_parse_gps()) {
1235                 parsed = true;
1236             }
1237     }
1238     return parsed;
1239 }
1240 #endif // USE_GPS_UBLOX
1241
1242 static void gpsHandlePassthrough(uint8_t data)
1243 {
1244      gpsNewData(data);
1245  #ifdef USE_DASHBOARD
1246      if (featureIsEnabled(FEATURE_DASHBOARD)) {
1247          dashboardUpdate(micros());
1248      }
1249  #endif
1250
1251  }
1252
1253 void gpsEnablePassthrough(serialPort_t *gpsPassthroughPort)
1254 {
1255     waitForSerialPortToFinishTransmitting(gpsPort);
1256     waitForSerialPortToFinishTransmitting(gpsPassthroughPort);
1257
1258     if (!(gpsPort->mode & MODE_TX))
1259         serialSetMode(gpsPort, gpsPort->mode | MODE_TX);
1260
1261 #ifdef USE_DASHBOARD
1262     if (featureIsEnabled(FEATURE_DASHBOARD)) {
1263         dashboardShowFixedPage(PAGE_GPS);
1264     }
1265 #endif
1266
1267     serialPassthrough(gpsPort, gpsPassthroughPort, &gpsHandlePassthrough, NULL);
1268 }
1269
1270 float GPS_scaleLonDown = 1.0f;  // this is used to offset the shrinking longitude as we go towards the poles
1271
1272 void GPS_calc_longitude_scaling(int32_t lat)
1273 {
1274     float rads = (fabsf((float)lat) / 10000000.0f) * 0.0174532925f;
1275     GPS_scaleLonDown = cos_approx(rads);
1276 }
1277
1278 ////////////////////////////////////////////////////////////////////////////////////
1279 // Calculate the distance flown and vertical speed from gps position data
1280 //
1281 static void GPS_calculateDistanceFlownVerticalSpeed(bool initialize)
1282 {
1283     static int32_t lastCoord[2] = { 0, 0 };
1284     static int32_t lastAlt;
1285     static int32_t lastMillis;
1286
1287     int currentMillis = millis();
1288
1289     if (initialize) {
1290         GPS_distanceFlownInCm = 0;
1291         GPS_verticalSpeedInCmS = 0;
1292     } else {
1293         if (STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED)) {
1294             uint16_t speed = gpsConfig()->gps_use_3d_speed ? gpsSol.speed3d : gpsSol.groundSpeed;
1295             // Only add up movement when speed is faster than minimum threshold
1296             if (speed > GPS_DISTANCE_FLOWN_MIN_SPEED_THRESHOLD_CM_S) {
1297                 uint32_t dist;
1298                 int32_t dir;
1299                 GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &lastCoord[LAT], &lastCoord[LON], &dist, &dir);
1300                 if (gpsConfig()->gps_use_3d_speed) {
1301                     dist = sqrtf(powf(gpsSol.llh.altCm - lastAlt, 2.0f) + powf(dist, 2.0f));
1302                 }
1303                 GPS_distanceFlownInCm += dist;
1304             }
1305         }
1306         GPS_verticalSpeedInCmS = (gpsSol.llh.altCm - lastAlt) * 1000 / (currentMillis - lastMillis);
1307         GPS_verticalSpeedInCmS = constrain(GPS_verticalSpeedInCmS, -1500, 1500);
1308     }
1309     lastCoord[LON] = gpsSol.llh.lon;
1310     lastCoord[LAT] = gpsSol.llh.lat;
1311     lastAlt = gpsSol.llh.altCm;
1312     lastMillis = currentMillis;
1313 }
1314
1315 void GPS_reset_home_position(void)
1316 {
1317     if (!STATE(GPS_FIX_HOME) || !gpsConfig()->gps_set_home_point_once) {
1318         if (STATE(GPS_FIX) && gpsSol.numSat >= 5) {
1319             GPS_home[LAT] = gpsSol.llh.lat;
1320             GPS_home[LON] = gpsSol.llh.lon;
1321             GPS_calc_longitude_scaling(gpsSol.llh.lat); // need an initial value for distance and bearing calc
1322             // Set ground altitude
1323             ENABLE_STATE(GPS_FIX_HOME);
1324         }
1325     }
1326     GPS_calculateDistanceFlownVerticalSpeed(true); //Initialize
1327 }
1328
1329 ////////////////////////////////////////////////////////////////////////////////////
1330 #define DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS 1.113195f
1331 #define TAN_89_99_DEGREES 5729.57795f
1332 // Get distance between two points in cm
1333 // Get bearing from pos1 to pos2, returns an 1deg = 100 precision
1334 void GPS_distance_cm_bearing(int32_t *currentLat1, int32_t *currentLon1, int32_t *destinationLat2, int32_t *destinationLon2, uint32_t *dist, int32_t *bearing)
1335 {
1336     float dLat = *destinationLat2 - *currentLat1; // difference of latitude in 1/10 000 000 degrees
1337     float dLon = (float)(*destinationLon2 - *currentLon1) * GPS_scaleLonDown;
1338     *dist = sqrtf(sq(dLat) + sq(dLon)) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS;
1339
1340     *bearing = 9000.0f + atan2_approx(-dLat, dLon) * TAN_89_99_DEGREES;      // Convert the output radians to 100xdeg
1341     if (*bearing < 0)
1342         *bearing += 36000;
1343 }
1344
1345 void GPS_calculateDistanceAndDirectionToHome(void)
1346 {
1347     if (STATE(GPS_FIX_HOME)) {      // If we don't have home set, do not display anything
1348         uint32_t dist;
1349         int32_t dir;
1350         GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_home[LAT], &GPS_home[LON], &dist, &dir);
1351         GPS_distanceToHome = dist / 100;
1352         GPS_directionToHome = dir / 100;
1353     } else {
1354         GPS_distanceToHome = 0;
1355         GPS_directionToHome = 0;
1356     }
1357 }
1358
1359 void onGpsNewData(void)
1360 {
1361     if (!(STATE(GPS_FIX) && gpsSol.numSat >= 5)) {
1362         return;
1363     }
1364
1365     //
1366     // Calculate time delta for navigation loop, range 0-1.0f, in seconds
1367     //
1368     // Time for calculating x,y speed and navigation pids
1369     static uint32_t nav_loopTimer;
1370     dTnav = (float)(millis() - nav_loopTimer) / 1000.0f;
1371     nav_loopTimer = millis();
1372     // prevent runup from bad GPS
1373     dTnav = MIN(dTnav, 1.0f);
1374
1375     GPS_calculateDistanceAndDirectionToHome();
1376     if (ARMING_FLAG(ARMED)) {
1377         GPS_calculateDistanceFlownVerticalSpeed(false);
1378     }
1379
1380 #ifdef USE_GPS_RESCUE
1381     rescueNewGpsData();
1382 #endif
1383 }
1384
1385 #endif