/*
 * This file is part of Cleanflight and Betaflight.
 *
 * Cleanflight and Betaflight are free software. You can redistribute
 * this software and/or modify this software under the terms of the
 * GNU General Public License as published by the Free Software
 * Foundation, either version 3 of the License, or (at your option)
 * any later version.
 *
 * Cleanflight and Betaflight are distributed in the hope that they
 * will be useful, but WITHOUT ANY WARRANTY; without even the implied
 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this software.
 *
 * If not, see <http://www.gnu.org/licenses/>.
 */

#include <stdbool.h>
#include <stdint.h>
#include <ctype.h>
#include <string.h>
#include <math.h>

#include "platform.h"

#ifdef USE_GPS

#include "build/build_config.h"
#include "build/debug.h"

#include "common/axis.h"
#include "common/gps_conversion.h"
#include "common/maths.h"
#include "common/utils.h"

#include "config/feature.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"

#include "drivers/light_led.h"
#include "drivers/time.h"

#include "io/beeper.h"
#include "io/dashboard.h"
#include "io/gps.h"
#include "io/serial.h"

#include "config/config.h"
#include "fc/runtime_config.h"

#include "flight/imu.h"
#include "flight/pid.h"
#include "flight/gps_rescue.h"

#include "scheduler/scheduler.h"

#include "sensors/sensors.h"

#define LOG_ERROR        '?'
#define LOG_IGNORED      '!'
#define LOG_SKIPPED      '>'
#define LOG_NMEA_GGA     'g'
#define LOG_NMEA_RMC     'r'
#define LOG_UBLOX_SOL    'O'
#define LOG_UBLOX_STATUS 'S'
#define LOG_UBLOX_SVINFO 'I'
#define LOG_UBLOX_POSLLH 'P'
#define LOG_UBLOX_VELNED 'V'

#define DEBUG_SERIAL_BAUD  0 // set to 1 to debug serial port baud config (/100)
#define DEBUG_UBLOX_INIT   0 // set to 1 to debug ublox initialization
#define DEBUG_UBLOX_FRAMES 0 // set to 1 to debug ublox received frames

char gpsPacketLog[GPS_PACKET_LOG_ENTRY_COUNT];
static char *gpsPacketLogChar = gpsPacketLog;
// **********************
// GPS
// **********************
int32_t GPS_home[2];
uint16_t GPS_distanceToHome;        // distance to home point in meters
int16_t GPS_directionToHome;        // direction to home or hol point in degrees
uint32_t GPS_distanceFlownInCm;     // distance flown since armed in centimeters
int16_t GPS_verticalSpeedInCmS;     // vertical speed in cm/s
float dTnav;             // Delta Time in milliseconds for navigation computations, updated with every good GPS read
int16_t nav_takeoff_bearing;

#define GPS_DISTANCE_FLOWN_MIN_SPEED_THRESHOLD_CM_S 15 // 5.4Km/h 3.35mph

gpsSolutionData_t gpsSol;
uint32_t GPS_packetCount = 0;
uint32_t GPS_svInfoReceivedCount = 0; // SV = Space Vehicle, counter increments each time SV info is received.
uint8_t GPS_update = 0;             // toogle to distinct a GPS position update (directly or via MSP)

uint8_t GPS_numCh;                              // Details on numCh/svinfo in gps.h
uint8_t GPS_svinfo_chn[GPS_SV_MAXSATS_M8N];
uint8_t GPS_svinfo_svid[GPS_SV_MAXSATS_M8N];
uint8_t GPS_svinfo_quality[GPS_SV_MAXSATS_M8N];
uint8_t GPS_svinfo_cno[GPS_SV_MAXSATS_M8N];

// GPS timeout for wrong baud rate/disconnection/etc in milliseconds (default 2.5second)
#define GPS_TIMEOUT (2500)
// How many entries in gpsInitData array below
#define GPS_INIT_ENTRIES (GPS_BAUDRATE_MAX + 1)
#define GPS_BAUDRATE_CHANGE_DELAY (200)
// Timeout for waiting ACK/NAK in GPS task cycles (0.25s at 100Hz)
#define UBLOX_ACK_TIMEOUT_MAX_COUNT (25)

static serialPort_t *gpsPort;

typedef struct gpsInitData_s {
    uint8_t index;
    uint8_t baudrateIndex; // see baudRate_e
    const char *ubx;
    const char *mtk;
} gpsInitData_t;

// NMEA will cycle through these until valid data is received
static const gpsInitData_t gpsInitData[] = {
    { GPS_BAUDRATE_115200,  BAUD_115200, "$PUBX,41,1,0003,0001,115200,0*1E\r\n", "$PMTK251,115200*1F\r\n" },
    { GPS_BAUDRATE_57600,    BAUD_57600, "$PUBX,41,1,0003,0001,57600,0*2D\r\n", "$PMTK251,57600*2C\r\n" },
    { GPS_BAUDRATE_38400,    BAUD_38400, "$PUBX,41,1,0003,0001,38400,0*26\r\n", "$PMTK251,38400*27\r\n" },
    { GPS_BAUDRATE_19200,    BAUD_19200, "$PUBX,41,1,0003,0001,19200,0*23\r\n", "$PMTK251,19200*22\r\n" },
    // 9600 is not enough for 5Hz updates - leave for compatibility to dumb NMEA that only runs at this speed
    { GPS_BAUDRATE_9600,      BAUD_9600, "$PUBX,41,1,0003,0001,9600,0*16\r\n", "" }
};

#define GPS_INIT_DATA_ENTRY_COUNT (sizeof(gpsInitData) / sizeof(gpsInitData[0]))

#define DEFAULT_BAUD_RATE_INDEX 0

#ifdef USE_GPS_UBLOX
enum {
    PREAMBLE1 = 0xB5,
    PREAMBLE2 = 0x62,
    CLASS_NAV = 0x01,
    CLASS_ACK = 0x05,
    CLASS_CFG = 0x06,
    MSG_ACK_NACK = 0x00,
    MSG_ACK_ACK = 0x01,
    MSG_POSLLH = 0x2,
    MSG_STATUS = 0x3,
    MSG_SOL = 0x6,
    MSG_PVT = 0x7,
    MSG_VELNED = 0x12,
    MSG_SVINFO = 0x30,
    MSG_SAT = 0x35,
    MSG_CFG_MSG = 0x1,
    MSG_CFG_PRT = 0x00,
    MSG_CFG_RATE = 0x08,
    MSG_CFG_SET_RATE = 0x01,
    MSG_CFG_SBAS = 0x16,
    MSG_CFG_NAV_SETTINGS = 0x24,
    MSG_CFG_GNSS = 0x3E
} ubx_protocol_bytes;

#define UBLOX_MODE_ENABLED    0x1
#define UBLOX_MODE_TEST       0x2

#define UBLOX_USAGE_RANGE     0x1
#define UBLOX_USAGE_DIFFCORR  0x2
#define UBLOX_USAGE_INTEGRITY 0x4

#define UBLOX_GNSS_ENABLE     0x1
#define UBLOX_GNSS_DEFAULT_SIGCFGMASK 0x10000

#define UBLOX_DYNMODE_PEDESTRIAN  3
#define UBLOX_DYNMODE_AIRBORNE_1G 6
#define UBLOX_DYNMODE_AIRBORNE_4G 8

typedef struct {
    uint8_t preamble1;
    uint8_t preamble2;
    uint8_t msg_class;
    uint8_t msg_id;
    uint16_t length;
} ubx_header;

typedef struct {
    uint8_t gnssId;
    uint8_t resTrkCh;
    uint8_t maxTrkCh;
    uint8_t reserved1;
    uint32_t flags;
} ubx_configblock;

typedef struct {
    uint8_t msgClass;
    uint8_t msgID;
} ubx_poll_msg;

typedef struct {
    uint8_t msgClass;
    uint8_t msgID;
    uint8_t rate;
} ubx_cfg_msg;

typedef struct {
    uint16_t measRate;
    uint16_t navRate;
    uint16_t timeRef;
} ubx_cfg_rate;

typedef struct {
    uint8_t mode;
    uint8_t usage;
    uint8_t maxSBAS;
    uint8_t scanmode2;
    uint32_t scanmode1;
} ubx_cfg_sbas;

typedef struct {
    uint8_t msgVer;
    uint8_t numTrkChHw;
    uint8_t numTrkChUse;
    uint8_t numConfigBlocks;
    ubx_configblock configblocks[7];
} ubx_cfg_gnss;

typedef struct {
    uint16_t mask;
    uint8_t dynModel;
    uint8_t fixMode;
    int32_t fixedAlt;
    uint32_t fixedAltVar;
    int8_t minElev;
    uint8_t drLimit;
    uint16_t pDOP;
    uint16_t tDOP;
    uint16_t pAcc;
    uint16_t tAcc;
    uint8_t staticHoldThresh;
    uint8_t dgnssTimeout;
    uint8_t cnoThreshNumSVs;
    uint8_t cnoThresh;
    uint8_t reserved0[2];
    uint16_t staticHoldMaxDist;
    uint8_t utcStandard;
    uint8_t reserved1[5];
} ubx_cfg_nav5;

typedef union {
    ubx_poll_msg poll_msg;
    ubx_cfg_msg cfg_msg;
    ubx_cfg_rate cfg_rate;
    ubx_cfg_nav5 cfg_nav5;
    ubx_cfg_sbas cfg_sbas;
    ubx_cfg_gnss cfg_gnss;
} ubx_payload;

typedef struct {
    ubx_header header;
    ubx_payload payload;
} __attribute__((packed)) ubx_message;

#endif // USE_GPS_UBLOX

typedef enum {
    GPS_STATE_UNKNOWN,
    GPS_STATE_INITIALIZING,
    GPS_STATE_INITIALIZED,
    GPS_STATE_CHANGE_BAUD,
    GPS_STATE_CONFIGURE,
    GPS_STATE_RECEIVING_DATA,
    GPS_STATE_LOST_COMMUNICATION,
    GPS_STATE_COUNT
} gpsState_e;

// Max time to wait for received data
#define GPS_MAX_WAIT_DATA_RX 30

gpsData_t gpsData;

PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0);

PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig,
    .provider = GPS_NMEA,
    .sbasMode = SBAS_NONE,
    .autoConfig = GPS_AUTOCONFIG_ON,
    .autoBaud = GPS_AUTOBAUD_OFF,
    .gps_ublox_use_galileo = false,
    .gps_ublox_mode = UBLOX_AIRBORNE,
    .gps_set_home_point_once = false,
    .gps_use_3d_speed = false,
    .sbas_integrity = false
);

static void shiftPacketLog(void)
{
    uint32_t i;

    for (i = ARRAYLEN(gpsPacketLog) - 1; i > 0 ; i--) {
        gpsPacketLog[i] = gpsPacketLog[i-1];
    }
}

static bool isConfiguratorConnected() {
    return (getArmingDisableFlags() & ARMING_DISABLED_MSP);
}

static void gpsNewData(uint16_t c);
#ifdef USE_GPS_NMEA
static bool gpsNewFrameNMEA(char c);
#endif
#ifdef USE_GPS_UBLOX
static bool gpsNewFrameUBLOX(uint8_t data);
#endif

static void gpsSetState(gpsState_e state)
{
    gpsData.lastMessage = millis();
    sensorsClear(SENSOR_GPS);

    gpsData.state = state;
    gpsData.state_position = 0;
    gpsData.state_ts = millis();
    gpsData.ackState = UBLOX_ACK_IDLE;
}

void gpsInit(void)
{
    gpsData.baudrateIndex = 0;
    gpsData.errors = 0;
    gpsData.timeouts = 0;

    memset(gpsPacketLog, 0x00, sizeof(gpsPacketLog));

    // init gpsData structure. if we're not actually enabled, don't bother doing anything else
    gpsSetState(GPS_STATE_UNKNOWN);

    gpsData.lastMessage = millis();

    if (gpsConfig()->provider == GPS_MSP) { // no serial ports used when GPS_MSP is configured
        gpsSetState(GPS_STATE_INITIALIZED);
        return;
    }

    const serialPortConfig_t *gpsPortConfig = findSerialPortConfig(FUNCTION_GPS);
    if (!gpsPortConfig) {
        return;
    }

    while (gpsInitData[gpsData.baudrateIndex].baudrateIndex != gpsPortConfig->gps_baudrateIndex) {
        gpsData.baudrateIndex++;
        if (gpsData.baudrateIndex >= GPS_INIT_DATA_ENTRY_COUNT) {
            gpsData.baudrateIndex = DEFAULT_BAUD_RATE_INDEX;
            break;
        }
    }

    portMode_e mode = MODE_RXTX;
#if defined(GPS_NMEA_TX_ONLY)
    if (gpsConfig()->provider == GPS_NMEA) {
        mode &= ~MODE_TX;
    }
#endif

    // no callback - buffer will be consumed in gpsUpdate()
    gpsPort = openSerialPort(gpsPortConfig->identifier, FUNCTION_GPS, NULL, NULL, baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex], mode, SERIAL_NOT_INVERTED);
    if (!gpsPort) {
        return;
    }

    // signal GPS "thread" to initialize when it gets to it
    gpsSetState(GPS_STATE_INITIALIZING);
}

#ifdef USE_GPS_NMEA
void gpsInitNmea(void)
{
#if !defined(GPS_NMEA_TX_ONLY)
    uint32_t now;
#endif
    switch (gpsData.state) {
        case GPS_STATE_INITIALIZING:
#if !defined(GPS_NMEA_TX_ONLY)
           now = millis();
           if (now - gpsData.state_ts < 1000) {
               return;
           }
           gpsData.state_ts = now;
           if (gpsData.state_position < 1) {
               serialSetBaudRate(gpsPort, 4800);
               gpsData.state_position++;
           } else if (gpsData.state_position < 2) {
               // print our FIXED init string for the baudrate we want to be at
               serialPrint(gpsPort, "$PSRF100,1,115200,8,1,0*05\r\n");
               gpsData.state_position++;
           } else {
               // we're now (hopefully) at the correct rate, next state will switch to it
               gpsSetState(GPS_STATE_CHANGE_BAUD);
           }
           break;
#endif
        case GPS_STATE_CHANGE_BAUD:
#if !defined(GPS_NMEA_TX_ONLY)
           now = millis();
           if (now - gpsData.state_ts < 1000) {
               return;
           }
           gpsData.state_ts = now;
           if (gpsData.state_position < 1) {
               serialSetBaudRate(gpsPort, baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex]);
               gpsData.state_position++;
           } else if (gpsData.state_position < 2) {
               serialPrint(gpsPort, "$PSRF103,00,6,00,0*23\r\n");
               gpsData.state_position++;
           } else
#else
           {
               serialSetBaudRate(gpsPort, baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex]);
           }
#endif
               gpsSetState(GPS_STATE_RECEIVING_DATA);
            break;
    }
}
#endif // USE_GPS_NMEA

#ifdef USE_GPS_UBLOX
static void ubloxSendByteUpdateChecksum(const uint8_t data, uint8_t *checksumA, uint8_t *checksumB)
{
    *checksumA += data;
    *checksumB += *checksumA;
    serialWrite(gpsPort, data);
}

static void ubloxSendDataUpdateChecksum(const uint8_t *data, uint8_t len, uint8_t *checksumA, uint8_t *checksumB)
{
    while (len--) {
        ubloxSendByteUpdateChecksum(*data, checksumA, checksumB);
        data++;
    }
}

static void ubloxSendMessage(const uint8_t *data, uint8_t len)
{
    uint8_t checksumA = 0, checksumB = 0;
    serialWrite(gpsPort, data[0]);
    serialWrite(gpsPort, data[1]);
    ubloxSendDataUpdateChecksum(&data[2], len - 2, &checksumA, &checksumB);
    serialWrite(gpsPort, checksumA);
    serialWrite(gpsPort, checksumB);

    // Save state for ACK waiting
    gpsData.ackWaitingMsgId = data[3]; //save message id for ACK
    gpsData.ackTimeoutCounter = 0;
    gpsData.ackState = UBLOX_ACK_WAITING;
}

static void ubloxSendConfigMessage(ubx_message *message, uint8_t msg_id, uint8_t length)
{
    message->header.preamble1 = PREAMBLE1;
    message->header.preamble2 = PREAMBLE2;
    message->header.msg_class = CLASS_CFG;
    message->header.msg_id = msg_id;
    message->header.length = length;
    ubloxSendMessage((const uint8_t *) message, length + 6);
}

static void ubloxSendPollMessage(uint8_t msg_id)
{
    ubx_message tx_buffer;
    tx_buffer.header.preamble1 = PREAMBLE1;
    tx_buffer.header.preamble2 = PREAMBLE2;
    tx_buffer.header.msg_class = CLASS_CFG;
    tx_buffer.header.msg_id = msg_id;
    tx_buffer.header.length = 0;
    ubloxSendMessage((const uint8_t *) &tx_buffer, 6);
}

static void ubloxSendNAV5Message(bool airborne) {
    ubx_message tx_buffer;
    tx_buffer.payload.cfg_nav5.mask = 0xFFFF;
    if (airborne) {
#if defined(GPS_UBLOX_MODE_AIRBORNE_1G)
        tx_buffer.payload.cfg_nav5.dynModel = UBLOX_DYNMODE_AIRBORNE_1G;
#else
        tx_buffer.payload.cfg_nav5.dynModel = UBLOX_DYNMODE_AIRBORNE_4G;
#endif
    } else {
        tx_buffer.payload.cfg_nav5.dynModel = UBLOX_DYNMODE_PEDESTRIAN;
    }
    tx_buffer.payload.cfg_nav5.fixMode = 3;
    tx_buffer.payload.cfg_nav5.fixedAlt = 0;
    tx_buffer.payload.cfg_nav5.fixedAltVar = 10000;
    tx_buffer.payload.cfg_nav5.minElev = 5;
    tx_buffer.payload.cfg_nav5.drLimit = 0;
    tx_buffer.payload.cfg_nav5.pDOP = 250;
    tx_buffer.payload.cfg_nav5.tDOP = 250;
    tx_buffer.payload.cfg_nav5.pAcc = 100;
    tx_buffer.payload.cfg_nav5.tAcc = 300;
    tx_buffer.payload.cfg_nav5.staticHoldThresh = 0;
    tx_buffer.payload.cfg_nav5.dgnssTimeout = 60;
    tx_buffer.payload.cfg_nav5.cnoThreshNumSVs = 0;
    tx_buffer.payload.cfg_nav5.cnoThresh = 0;
    tx_buffer.payload.cfg_nav5.reserved0[0] = 0;
    tx_buffer.payload.cfg_nav5.reserved0[1] = 0;
    tx_buffer.payload.cfg_nav5.staticHoldMaxDist = 200;
    tx_buffer.payload.cfg_nav5.utcStandard = 0;
    tx_buffer.payload.cfg_nav5.reserved1[0] = 0;
    tx_buffer.payload.cfg_nav5.reserved1[1] = 0;
    tx_buffer.payload.cfg_nav5.reserved1[2] = 0;
    tx_buffer.payload.cfg_nav5.reserved1[3] = 0;
    tx_buffer.payload.cfg_nav5.reserved1[4] = 0;

    ubloxSendConfigMessage(&tx_buffer, MSG_CFG_NAV_SETTINGS, sizeof(ubx_cfg_nav5));
}

static void ubloxSetMessageRate(uint8_t messageClass, uint8_t messageID, uint8_t rate) {
    ubx_message tx_buffer;
    tx_buffer.payload.cfg_msg.msgClass = messageClass;
    tx_buffer.payload.cfg_msg.msgID = messageID;
    tx_buffer.payload.cfg_msg.rate = rate;
    ubloxSendConfigMessage(&tx_buffer, MSG_CFG_MSG, sizeof(ubx_cfg_msg));
}

static void ubloxSetNavRate(uint16_t measRate, uint16_t navRate, uint16_t timeRef) {
    ubx_message tx_buffer;
    tx_buffer.payload.cfg_rate.measRate = measRate;
    tx_buffer.payload.cfg_rate.navRate = navRate;
    tx_buffer.payload.cfg_rate.timeRef = timeRef;
    ubloxSendConfigMessage(&tx_buffer, MSG_CFG_RATE, sizeof(ubx_cfg_rate));
}

static void ubloxSetSbas() {
    ubx_message tx_buffer;

    //NOTE: default ublox config for sbas mode is: UBLOX_MODE_ENABLED, test is disabled
    tx_buffer.payload.cfg_sbas.mode = UBLOX_MODE_TEST;
    if (gpsConfig()->sbasMode != SBAS_NONE) {
        tx_buffer.payload.cfg_sbas.mode |= UBLOX_MODE_ENABLED;
    }

    //NOTE: default ublox config for sbas mode is: UBLOX_USAGE_RANGE | UBLOX_USAGE_DIFFCORR, integrity is disabled
    tx_buffer.payload.cfg_sbas.usage = UBLOX_USAGE_RANGE | UBLOX_USAGE_DIFFCORR;
    if (gpsConfig()->sbas_integrity) {
        tx_buffer.payload.cfg_sbas.usage |= UBLOX_USAGE_INTEGRITY;
    }

    tx_buffer.payload.cfg_sbas.maxSBAS = 3;
    tx_buffer.payload.cfg_sbas.scanmode2 = 0;
    switch (gpsConfig()->sbasMode) {
        case SBAS_AUTO:
            tx_buffer.payload.cfg_sbas.scanmode1 = 0;
            break;
        case SBAS_EGNOS:
            tx_buffer.payload.cfg_sbas.scanmode1 = 0x00010048; //PRN123, PRN126, PRN136
            break;
        case SBAS_WAAS:
            tx_buffer.payload.cfg_sbas.scanmode1 = 0x0004A800; //PRN131, PRN133, PRN135, PRN138
            break;
        case SBAS_MSAS:
            tx_buffer.payload.cfg_sbas.scanmode1 = 0x00020200; //PRN129, PRN137
            break;
        case SBAS_GAGAN:
            tx_buffer.payload.cfg_sbas.scanmode1 = 0x00001180; //PRN127, PRN128, PRN132
            break;
        default:
            tx_buffer.payload.cfg_sbas.scanmode1 = 0;
            break;
    }
    ubloxSendConfigMessage(&tx_buffer, MSG_CFG_SBAS, sizeof(ubx_cfg_sbas));
}

void gpsInitUblox(void)
{
    uint32_t now;
    // UBX will run at the serial port's baudrate, it shouldn't be "autodetected". So here we force it to that rate

    // Wait until GPS transmit buffer is empty
    if (!isSerialTransmitBufferEmpty(gpsPort))
        return;

    switch (gpsData.state) {
        case GPS_STATE_INITIALIZING:
            now = millis();
            if (now - gpsData.state_ts < GPS_BAUDRATE_CHANGE_DELAY)
                return;

            if (gpsData.state_position < GPS_INIT_ENTRIES) {
                // try different speed to INIT
                baudRate_e newBaudRateIndex = gpsInitData[gpsData.state_position].baudrateIndex;

                gpsData.state_ts = now;

                if (lookupBaudRateIndex(serialGetBaudRate(gpsPort)) != newBaudRateIndex) {
                    // change the rate if needed and wait a little
                    serialSetBaudRate(gpsPort, baudRates[newBaudRateIndex]);
#if DEBUG_SERIAL_BAUD
                    debug[0] = baudRates[newBaudRateIndex] / 100;
#endif
                    return;
                }

                // print our FIXED init string for the baudrate we want to be at
                serialPrint(gpsPort, gpsInitData[gpsData.baudrateIndex].ubx);

                gpsData.state_position++;
            } else {
                // we're now (hopefully) at the correct rate, next state will switch to it
                gpsSetState(GPS_STATE_CHANGE_BAUD);
            }
            break;

        case GPS_STATE_CHANGE_BAUD:
            serialSetBaudRate(gpsPort, baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex]);
#if DEBUG_SERIAL_BAUD
            debug[0] = baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex] / 100;
#endif
            gpsSetState(GPS_STATE_CONFIGURE);
            break;

        case GPS_STATE_CONFIGURE:
            // Either use specific config file for GPS or let dynamically upload config
            if (gpsConfig()->autoConfig == GPS_AUTOCONFIG_OFF) {
                gpsSetState(GPS_STATE_RECEIVING_DATA);
                break;
            }

            if (gpsData.ackState == UBLOX_ACK_IDLE) {
                switch (gpsData.state_position) {
                    case 0:
                        gpsData.ubloxUsePVT = true;
                        gpsData.ubloxUseSAT = true;
                        ubloxSendNAV5Message(gpsConfig()->gps_ublox_mode == UBLOX_AIRBORNE);
                        break;
                    case 1: //Disable NMEA Messages
                        ubloxSetMessageRate(0xF0, 0x05, 0); // VGS: Course over ground and Ground speed
                        break;
                    case 2:
                        ubloxSetMessageRate(0xF0, 0x03, 0); // GSV: GNSS Satellites in View
                        break;
                    case 3:
                        ubloxSetMessageRate(0xF0, 0x01, 0); // GLL: Latitude and longitude, with time of position fix and status
                        break;
                    case 4:
                        ubloxSetMessageRate(0xF0, 0x00, 0); // GGA: Global positioning system fix data
                        break;
                    case 5:
                        ubloxSetMessageRate(0xF0, 0x02, 0); // GSA: GNSS DOP and Active Satellites
                        break;
                    case 6:
                        ubloxSetMessageRate(0xF0, 0x04, 0); // RMC: Recommended Minimum data
                        break;
                    case 7: //Enable UBLOX Messages
                        if (gpsData.ubloxUsePVT) {
                            ubloxSetMessageRate(CLASS_NAV, MSG_PVT, 1); // set PVT MSG rate
                        } else {
                            ubloxSetMessageRate(CLASS_NAV, MSG_SOL, 1); // set SOL MSG rate
                        }
                        break;
                    case 8:
                        if (gpsData.ubloxUsePVT) {
                            gpsData.state_position++;
                        } else {
                           ubloxSetMessageRate(CLASS_NAV, MSG_POSLLH, 1); // set POSLLH MSG rate
                        }
                        break;
                    case 9:
                        if (gpsData.ubloxUsePVT) {
                            gpsData.state_position++;
                        } else {
                            ubloxSetMessageRate(CLASS_NAV, MSG_STATUS, 1); // set STATUS MSG rate
                        }
                        break;
                    case 10:
                        if (gpsData.ubloxUsePVT) {
                            gpsData.state_position++;
                        } else {
                            ubloxSetMessageRate(CLASS_NAV, MSG_VELNED, 1); // set VELNED MSG rate
                        }
                        break;
                    case 11:
                        if (gpsData.ubloxUseSAT) {
                            ubloxSetMessageRate(CLASS_NAV, MSG_SAT, 5); // set SAT MSG rate (every 5 cycles)
                        } else {
                            ubloxSetMessageRate(CLASS_NAV, MSG_SVINFO, 5); // set SVINFO MSG rate (every 5 cycles)
                        }
                        break;
                    case 12:
                        ubloxSetNavRate(0xC8, 1, 1); // set rate to 5Hz (measurement period: 200ms, navigation rate: 1 cycle)
                        break;
                    case 13:
                        ubloxSetSbas();
                        break;
                    case 14:
                        if ((gpsConfig()->sbasMode == SBAS_NONE) || (gpsConfig()->gps_ublox_use_galileo)) {
                            ubloxSendPollMessage(MSG_CFG_GNSS);
                        } else {
                            gpsSetState(GPS_STATE_RECEIVING_DATA);
                        }
                        break;
                    default:
                        break;
                }
            }

            switch (gpsData.ackState) {
                case UBLOX_ACK_IDLE:
                    break;
                case UBLOX_ACK_WAITING:
                    if ((++gpsData.ackTimeoutCounter) == UBLOX_ACK_TIMEOUT_MAX_COUNT) {
                        gpsSetState(GPS_STATE_LOST_COMMUNICATION);
                    }
                    break;
                case UBLOX_ACK_GOT_ACK:
                    if (gpsData.state_position == 14) {
                        // ublox should be initialised, try receiving
                        gpsSetState(GPS_STATE_RECEIVING_DATA);
                    } else {
                        gpsData.state_position++;
                        gpsData.ackState = UBLOX_ACK_IDLE;
                    }
                    break;
                case UBLOX_ACK_GOT_NACK:
                    if (gpsData.state_position == 7) { // If we were asking for NAV-PVT...
                        gpsData.ubloxUsePVT = false;   // ...retry asking for NAV-SOL
                        gpsData.ackState = UBLOX_ACK_IDLE;
                    } else {
                        if (gpsData.state_position == 11) { // If we were asking for NAV-SAT...
                            gpsData.ubloxUseSAT = false;   // ...retry asking for NAV-SVINFO
                            gpsData.ackState = UBLOX_ACK_IDLE;
                        } else {
                            gpsSetState(GPS_STATE_CONFIGURE);
                        }
                    }
                    break;
            }

            break;
    }
}
#endif // USE_GPS_UBLOX

void gpsInitHardware(void)
{
    switch (gpsConfig()->provider) {
    case GPS_NMEA:
#ifdef USE_GPS_NMEA
        gpsInitNmea();
#endif
        break;

    case GPS_UBLOX:
#ifdef USE_GPS_UBLOX
        gpsInitUblox();
#endif
        break;
    default:
        break;
    }
}

static void updateGpsIndicator(timeUs_t currentTimeUs)
{
    static uint32_t GPSLEDTime;
    if ((int32_t)(currentTimeUs - GPSLEDTime) >= 0 && (gpsSol.numSat >= 5)) {
        GPSLEDTime = currentTimeUs + 150000;
        LED1_TOGGLE;
    }
}

void gpsUpdate(timeUs_t currentTimeUs)
{
    static gpsState_e gpsStateDurationUs[GPS_STATE_COUNT];
    timeUs_t executeTimeUs;
    gpsState_e gpsCurrentState = gpsData.state;

    // read out available GPS bytes
    if (gpsPort) {
        while (serialRxBytesWaiting(gpsPort)) {
            if (cmpTimeUs(micros(), currentTimeUs) > GPS_MAX_WAIT_DATA_RX) {
                // Wait 1ms and come back
                rescheduleTask(TASK_SELF, TASK_PERIOD_HZ(TASK_GPS_RATE_FAST));
                return;
            }
            gpsNewData(serialRead(gpsPort));
        }
        // Restore default task rate
        rescheduleTask(TASK_SELF, TASK_PERIOD_HZ(TASK_GPS_RATE));
   } else if (GPS_update & GPS_MSP_UPDATE) { // GPS data received via MSP
        gpsSetState(GPS_STATE_RECEIVING_DATA);
        onGpsNewData();
        GPS_update &= ~GPS_MSP_UPDATE;
    }

#if DEBUG_UBLOX_INIT
    debug[0] = gpsData.state;
    debug[1] = gpsData.state_position;
    debug[2] = gpsData.ackState;
#endif

    switch (gpsData.state) {
        case GPS_STATE_UNKNOWN:
        case GPS_STATE_INITIALIZED:
            break;

        case GPS_STATE_INITIALIZING:
        case GPS_STATE_CHANGE_BAUD:
        case GPS_STATE_CONFIGURE:
            gpsInitHardware();
            break;

        case GPS_STATE_LOST_COMMUNICATION:
            gpsData.timeouts++;
            if (gpsConfig()->autoBaud) {
                // try another rate
                gpsData.baudrateIndex++;
                gpsData.baudrateIndex %= GPS_INIT_ENTRIES;
            }
            gpsSol.numSat = 0;
            DISABLE_STATE(GPS_FIX);
            gpsSetState(GPS_STATE_INITIALIZING);
            break;

        case GPS_STATE_RECEIVING_DATA:
            // check for no data/gps timeout/cable disconnection etc
            if (millis() - gpsData.lastMessage > GPS_TIMEOUT) {
                gpsSetState(GPS_STATE_LOST_COMMUNICATION);
#ifdef USE_GPS_UBLOX
            } else {
                if (gpsConfig()->autoConfig == GPS_AUTOCONFIG_ON) { // Only if autoconfig is enabled
                    switch (gpsData.state_position) {
                        case 0:
                            if (!isConfiguratorConnected()) {
                                if (gpsData.ubloxUseSAT) {
                                    ubloxSetMessageRate(CLASS_NAV, MSG_SAT, 0); // disable SAT MSG
                                } else {
                                    ubloxSetMessageRate(CLASS_NAV, MSG_SVINFO, 0); // disable SVINFO MSG
                                }
                                gpsData.state_position = 1;
                            }
                            break;
                        case 1:
                            if (STATE(GPS_FIX) && (gpsConfig()->gps_ublox_mode == UBLOX_DYNAMIC)) {
                                ubloxSendNAV5Message(true);
                                gpsData.state_position = 2;
                            }
                            if (isConfiguratorConnected()) {
                                gpsData.state_position = 2;
                            }
                            break;
                        case 2:
                            if (isConfiguratorConnected()) {
                                if (gpsData.ubloxUseSAT) {
                                    ubloxSetMessageRate(CLASS_NAV, MSG_SAT, 5); // set SAT MSG rate (every 5 cycles)
                                } else {
                                    ubloxSetMessageRate(CLASS_NAV, MSG_SVINFO, 5); // set SVINFO MSG rate (every 5 cycles)
                                }
                                gpsData.state_position = 0;
                            }
                            break;
                    }
                }
#endif //USE_GPS_UBLOX
            }
            break;
    }

    executeTimeUs = micros() - currentTimeUs;

    if (executeTimeUs > gpsStateDurationUs[gpsCurrentState]) {
        gpsStateDurationUs[gpsCurrentState] = executeTimeUs;
    }
    schedulerSetNextStateTime(gpsStateDurationUs[gpsData.state]);

    if (sensors(SENSOR_GPS)) {
        updateGpsIndicator(currentTimeUs);
    }
    if (!ARMING_FLAG(ARMED) && !gpsConfig()->gps_set_home_point_once) {
        DISABLE_STATE(GPS_FIX_HOME);
    }

    uint8_t minSats = 5;

#if defined(USE_GPS_RESCUE)
    if (gpsRescueIsConfigured()) {
        updateGPSRescueState();
        minSats = gpsRescueConfig()->minSats;
    }
#endif

    static bool hasFix = false;
    if (STATE(GPS_FIX)) {
        if (gpsIsHealthy() && gpsSol.numSat >= minSats && !hasFix) {
            // ready beep sequence on fix or requirements for gps rescue met.
            beeper(BEEPER_READY_BEEP);
            hasFix = true;
        }
    } else {
        hasFix = false;
    }
}

static void gpsNewData(uint16_t c)
{
    if (!gpsNewFrame(c)) {
        return;
    }

    if (gpsData.state == GPS_STATE_RECEIVING_DATA) {
        // new data received and parsed, we're in business
        gpsData.lastLastMessage = gpsData.lastMessage;
        gpsData.lastMessage = millis();
        sensorsSet(SENSOR_GPS);
    }

    GPS_update ^= GPS_DIRECT_TICK;

#if DEBUG_UBLOX_INIT
    debug[3] = GPS_update;
#endif

    onGpsNewData();
}

bool gpsNewFrame(uint8_t c)
{
    switch (gpsConfig()->provider) {
    case GPS_NMEA:          // NMEA
#ifdef USE_GPS_NMEA
        return gpsNewFrameNMEA(c);
#endif
        break;
    case GPS_UBLOX:         // UBX binary
#ifdef USE_GPS_UBLOX
        return gpsNewFrameUBLOX(c);
#endif
        break;
    default:
        break;
    }
    return false;
}

// Check for healthy communications
bool gpsIsHealthy()
{
    return (gpsData.state == GPS_STATE_RECEIVING_DATA);
}

/* This is a light implementation of a GPS frame decoding
   This should work with most of modern GPS devices configured to output 5 frames.
   It assumes there are some NMEA GGA frames to decode on the serial bus
   Now verifies checksum correctly before applying data

   Here we use only the following data :
     - latitude
     - longitude
     - GPS fix is/is not ok
     - GPS num sat (4 is enough to be +/- reliable)
     // added by Mis
     - GPS altitude (for OSD displaying)
     - GPS speed (for OSD displaying)
*/

#define NO_FRAME   0
#define FRAME_GGA  1
#define FRAME_RMC  2
#define FRAME_GSV  3


// This code is used for parsing NMEA data

/* Alex optimization
  The latitude or longitude is coded this way in NMEA frames
  dm.f   coded as degrees + minutes + minute decimal
  Where:
    - d can be 1 or more char long. generally: 2 char long for latitude, 3 char long for longitude
    - m is always 2 char long
    - f can be 1 or more char long
  This function converts this format in a unique unsigned long where 1 degree = 10 000 000

  EOS increased the precision here, even if we think that the gps is not precise enough, with 10e5 precision it has 76cm resolution
  with 10e7 it's around 1 cm now. Increasing it further is irrelevant, since even 1cm resolution is unrealistic, however increased
  resolution also increased precision of nav calculations
static uint32_t GPS_coord_to_degrees(char *coordinateString)
{
    char *p = s, *d = s;
    uint8_t min, deg = 0;
    uint16_t frac = 0, mult = 10000;

    while (*p) {                // parse the string until its end
        if (d != s) {
            frac += (*p - '0') * mult;  // calculate only fractional part on up to 5 digits  (d != s condition is true when the . is located)
            mult /= 10;
        }
        if (*p == '.')
            d = p;              // locate '.' char in the string
        p++;
    }
    if (p == s)
        return 0;
    while (s < d - 2) {
        deg *= 10;              // convert degrees : all chars before minutes ; for the first iteration, deg = 0
        deg += *(s++) - '0';
    }
    min = *(d - 1) - '0' + (*(d - 2) - '0') * 10;       // convert minutes : 2 previous char before '.'
    return deg * 10000000UL + (min * 100000UL + frac) * 10UL / 6;
}
*/

// helper functions
#ifdef USE_GPS_NMEA
static uint32_t grab_fields(char *src, uint8_t mult)
{                               // convert string to uint32
    uint32_t i;
    uint32_t tmp = 0;
    int isneg = 0;
    for (i = 0; src[i] != 0; i++) {
        if ((i == 0) && (src[0] == '-')) { // detect negative sign
            isneg = 1;
            continue; // jump to next character if the first one was a negative sign
        }
        if (src[i] == '.') {
            i++;
            if (mult == 0) {
                break;
            } else {
                src[i + mult] = 0;
            }
        }
        tmp *= 10;
        if (src[i] >= '0' && src[i] <= '9') {
            tmp += src[i] - '0';
        }
        if (i >= 15) {
            return 0; // out of bounds
        }
    }
    return isneg ? -tmp : tmp;    // handle negative altitudes
}

typedef struct gpsDataNmea_s {
    int32_t latitude;
    int32_t longitude;
    uint8_t numSat;
    int32_t altitudeCm;
    uint16_t speed;
    uint16_t hdop;
    uint16_t ground_course;
    uint32_t time;
    uint32_t date;
} gpsDataNmea_t;

static bool gpsNewFrameNMEA(char c)
{
    static gpsDataNmea_t gps_Msg;

    uint8_t frameOK = 0;
    static uint8_t param = 0, offset = 0, parity = 0;
    static char string[15];
    static uint8_t checksum_param, gps_frame = NO_FRAME;
    static uint8_t svMessageNum = 0;
    uint8_t svSatNum = 0, svPacketIdx = 0, svSatParam = 0;

    switch (c) {
        case '$':
            param = 0;
            offset = 0;
            parity = 0;
            break;
        case ',':
        case '*':
            string[offset] = 0;
            if (param == 0) {       //frame identification
                gps_frame = NO_FRAME;
                if (0 == strcmp(string, "GPGGA") || 0 == strcmp(string, "GNGGA")) {
                    gps_frame = FRAME_GGA;
                } else if (0 == strcmp(string, "GPRMC") || 0 == strcmp(string, "GNRMC")) {
                    gps_frame = FRAME_RMC;
                } else if (0 == strcmp(string, "GPGSV")) {
                    gps_frame = FRAME_GSV;
                }
            }

            switch (gps_frame) {
                case FRAME_GGA:        //************* GPGGA FRAME parsing
                    switch (param) {
            //          case 1:             // Time information
            //              break;
                        case 2:
                            gps_Msg.latitude = GPS_coord_to_degrees(string);
                            break;
                        case 3:
                            if (string[0] == 'S')
                                gps_Msg.latitude *= -1;
                            break;
                        case 4:
                            gps_Msg.longitude = GPS_coord_to_degrees(string);
                            break;
                        case 5:
                            if (string[0] == 'W')
                                gps_Msg.longitude *= -1;
                            break;
                        case 6:
                            gpsSetFixState(string[0] > '0');
                            break;
                        case 7:
                            gps_Msg.numSat = grab_fields(string, 0);
                            break;
                        case 8:
                            gps_Msg.hdop = grab_fields(string, 1) * 100;          // hdop
                            break;
                        case 9:
                            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
                            break;
                    }
                    break;
                case FRAME_RMC:        //************* GPRMC FRAME parsing
                    switch (param) {
                        case 1:
                            gps_Msg.time = grab_fields(string, 2); // UTC time hhmmss.ss
                            break;
                        case 7:
                            gps_Msg.speed = ((grab_fields(string, 1) * 5144L) / 1000L);    // speed in cm/s added by Mis
                            break;
                        case 8:
                            gps_Msg.ground_course = (grab_fields(string, 1));      // ground course deg * 10
                            break;
                        case 9:
                            gps_Msg.date = grab_fields(string, 0); // date dd/mm/yy
                            break;
                    }
                    break;
                case FRAME_GSV:
                    switch (param) {
                      /*case 1:
                            // Total number of messages of this type in this cycle
                            break; */
                        case 2:
                            // Message number
                            svMessageNum = grab_fields(string, 0);
                            break;
                        case 3:
                            // Total number of SVs visible
                            GPS_numCh = grab_fields(string, 0);
                            if (GPS_numCh > GPS_SV_MAXSATS_LEGACY) {
                                GPS_numCh = GPS_SV_MAXSATS_LEGACY;
                            }
                            break;
                    }
                    if (param < 4)
                        break;

                    svPacketIdx = (param - 4) / 4 + 1; // satellite number in packet, 1-4
                    svSatNum    = svPacketIdx + (4 * (svMessageNum - 1)); // global satellite number
                    svSatParam  = param - 3 - (4 * (svPacketIdx - 1)); // parameter number for satellite

                    if (svSatNum > GPS_SV_MAXSATS_LEGACY)
                        break;

                    switch (svSatParam) {
                        case 1:
                            // SV PRN number
                            GPS_svinfo_chn[svSatNum - 1]  = svSatNum;
                            GPS_svinfo_svid[svSatNum - 1] = grab_fields(string, 0);
                            break;
                      /*case 2:
                            // Elevation, in degrees, 90 maximum
                            break;
                        case 3:
                            // Azimuth, degrees from True North, 000 through 359
                            break; */
                        case 4:
                            // SNR, 00 through 99 dB (null when not tracking)
                            GPS_svinfo_cno[svSatNum - 1] = grab_fields(string, 0);
                            GPS_svinfo_quality[svSatNum - 1] = 0; // only used by ublox
                            break;
                    }

                    GPS_svInfoReceivedCount++;

                    break;
            }

            param++;
            offset = 0;
            if (c == '*')
                checksum_param = 1;
            else
                parity ^= c;
            break;
        case '\r':
        case '\n':
            if (checksum_param) {   //parity checksum
                shiftPacketLog();
                uint8_t checksum = 16 * ((string[0] >= 'A') ? string[0] - 'A' + 10 : string[0] - '0') + ((string[1] >= 'A') ? string[1] - 'A' + 10 : string[1] - '0');
                if (checksum == parity) {
                    *gpsPacketLogChar = LOG_IGNORED;
                    GPS_packetCount++;
                    switch (gps_frame) {
                    case FRAME_GGA:
                      *gpsPacketLogChar = LOG_NMEA_GGA;
                      frameOK = 1;
                      if (STATE(GPS_FIX)) {
                            gpsSol.llh.lat = gps_Msg.latitude;
                            gpsSol.llh.lon = gps_Msg.longitude;
                            gpsSol.numSat = gps_Msg.numSat;
                            gpsSol.llh.altCm = gps_Msg.altitudeCm;
                            gpsSol.hdop = gps_Msg.hdop;
                        }
                        break;
                    case FRAME_RMC:
                        *gpsPacketLogChar = LOG_NMEA_RMC;
                        gpsSol.groundSpeed = gps_Msg.speed;
                        gpsSol.groundCourse = gps_Msg.ground_course;
#ifdef USE_RTC_TIME
                        // This check will miss 00:00:00.00, but we shouldn't care - next report will be valid
                        if(!rtcHasTime() && gps_Msg.date != 0 && gps_Msg.time != 0) {
                            dateTime_t temp_time;
                            temp_time.year = (gps_Msg.date % 100) + 2000;
                            temp_time.month = (gps_Msg.date / 100) % 100;
                            temp_time.day = (gps_Msg.date / 10000) % 100;
                            temp_time.hours = (gps_Msg.time / 1000000) % 100;
                            temp_time.minutes = (gps_Msg.time / 10000) % 100;
                            temp_time.seconds = (gps_Msg.time / 100) % 100;
                            temp_time.millis = (gps_Msg.time & 100) * 10;
                            rtcSetDateTime(&temp_time);
                        }
#endif
                        break;
                    } // end switch
                } else {
                    *gpsPacketLogChar = LOG_ERROR;
                }
            }
            checksum_param = 0;
            break;
        default:
            if (offset < 15)
                string[offset++] = c;
            if (!checksum_param)
                parity ^= c;
    }
    return frameOK;
}
#endif // USE_GPS_NMEA

#ifdef USE_GPS_UBLOX
// UBX support
typedef struct {
    uint32_t time;              // GPS msToW
    int32_t longitude;
    int32_t latitude;
    int32_t altitude_ellipsoid;
    int32_t altitudeMslMm;
    uint32_t horizontal_accuracy;
    uint32_t vertical_accuracy;
} ubx_nav_posllh;

typedef struct {
    uint32_t time;              // GPS msToW
    uint8_t fix_type;
    uint8_t fix_status;
    uint8_t differential_status;
    uint8_t res;
    uint32_t time_to_first_fix;
    uint32_t uptime;            // milliseconds
} ubx_nav_status;

typedef struct {
    uint32_t time;
    int32_t time_nsec;
    int16_t week;
    uint8_t fix_type;
    uint8_t fix_status;
    int32_t ecef_x;
    int32_t ecef_y;
    int32_t ecef_z;
    uint32_t position_accuracy_3d;
    int32_t ecef_x_velocity;
    int32_t ecef_y_velocity;
    int32_t ecef_z_velocity;
    uint32_t speed_accuracy;
    uint16_t position_DOP;
    uint8_t res;
    uint8_t satellites;
    uint32_t res2;
} ubx_nav_solution;

typedef struct {
    uint32_t time;
    uint16_t year;
    uint8_t month;
    uint8_t day;
    uint8_t hour;
    uint8_t min;
    uint8_t sec;
    uint8_t valid;
    uint32_t tAcc;
    int32_t nano;
    uint8_t fixType;
    uint8_t flags;
    uint8_t flags2;
    uint8_t numSV;
    int32_t lon;
    int32_t lat;
    int32_t height;
    int32_t hMSL;
    uint32_t hAcc;
    uint32_t vAcc;
    int32_t velN;
    int32_t velE;
    int32_t velD;
    int32_t gSpeed;
    int32_t headMot;
    uint32_t sAcc;
    uint32_t headAcc;
    uint16_t pDOP;
    uint8_t flags3;
    uint8_t reserved0[5];
    int32_t headVeh;
    int16_t magDec;
    uint16_t magAcc;
} ubx_nav_pvt;

typedef struct {
    uint32_t time;              // GPS msToW
    int32_t ned_north;
    int32_t ned_east;
    int32_t ned_down;
    uint32_t speed_3d;
    uint32_t speed_2d;
    int32_t heading_2d;
    uint32_t speed_accuracy;
    uint32_t heading_accuracy;
} ubx_nav_velned;

typedef struct {
    uint8_t chn;                // Channel number, 255 for SVx not assigned to channel
    uint8_t svid;               // Satellite ID
    uint8_t flags;              // Bitmask
    uint8_t quality;            // Bitfield
    uint8_t cno;                // Carrier to Noise Ratio (Signal Strength) // dbHz, 0-55.
    uint8_t elev;               // Elevation in integer degrees
    int16_t azim;               // Azimuth in integer degrees
    int32_t prRes;              // Pseudo range residual in centimetres
} ubx_nav_svinfo_channel;

typedef struct {
    uint8_t gnssId;
    uint8_t svId;               // Satellite ID
    uint8_t cno;                // Carrier to Noise Ratio (Signal Strength) // dbHz, 0-55.
    int8_t elev;                // Elevation in integer degrees
    int16_t azim;               // Azimuth in integer degrees
    int16_t prRes;              // Pseudo range residual in decimetres
    uint32_t flags;             // Bitmask
} ubx_nav_sat_sv;

typedef struct {
    uint32_t time;              // GPS Millisecond time of week
    uint8_t numCh;              // Number of channels
    uint8_t globalFlags;        // Bitmask, Chip hardware generation 0:Antaris, 1:u-blox 5, 2:u-blox 6
    uint16_t reserved2;         // Reserved
    ubx_nav_svinfo_channel channel[GPS_SV_MAXSATS_M8N];         // 32 satellites * 12 byte
} ubx_nav_svinfo;

typedef struct {
    uint32_t time;              // GPS Millisecond time of week
    uint8_t version;
    uint8_t numSvs;
    uint8_t reserved0[2];
    ubx_nav_sat_sv svs[GPS_SV_MAXSATS_M9N];
} ubx_nav_sat;

typedef struct {
    uint8_t clsId;               // Class ID of the acknowledged message
    uint8_t msgId;               // Message ID of the acknowledged message
} ubx_ack;

enum {
    FIX_NONE = 0,
    FIX_DEAD_RECKONING = 1,
    FIX_2D = 2,
    FIX_3D = 3,
    FIX_GPS_DEAD_RECKONING = 4,
    FIX_TIME = 5
} ubs_nav_fix_type;

enum {
    NAV_STATUS_FIX_VALID = 1,
    NAV_STATUS_TIME_WEEK_VALID = 4,
    NAV_STATUS_TIME_SECOND_VALID = 8
} ubx_nav_status_bits;

enum {
    NAV_VALID_DATE = 1,
    NAV_VALID_TIME = 2
} ubx_nav_pvt_valid;

// Packet checksum accumulators
static uint8_t _ck_a;
static uint8_t _ck_b;

// State machine state
static bool _skip_packet;
static uint8_t _step;
static uint8_t _msg_id;
static uint16_t _payload_length;
static uint16_t _payload_counter;

static bool next_fix;
static uint8_t _class;

// do we have new position information?
static bool _new_position;

// do we have new speed information?
static bool _new_speed;

// Example packet sizes from UBlox u-center from a Glonass capable GPS receiver.
//15:17:55  R -> UBX NAV-STATUS,  Size  24,  'Navigation Status'
//15:17:55  R -> UBX NAV-POSLLH,  Size  36,  'Geodetic Position'
//15:17:55  R -> UBX NAV-VELNED,  Size  44,  'Velocity in WGS 84'
//15:17:55  R -> UBX NAV-CLOCK,  Size  28,  'Clock Status'
//15:17:55  R -> UBX NAV-AOPSTATUS,  Size  24,  'AOP Status'
//15:17:55  R -> UBX 03-09,  Size 208,  'Unknown'
//15:17:55  R -> UBX 03-10,  Size 336,  'Unknown'
//15:17:55  R -> UBX NAV-SOL,  Size  60,  'Navigation Solution'
//15:17:55  R -> UBX NAV,  Size 100,  'Navigation'
//15:17:55  R -> UBX NAV-SVINFO,  Size 328,  'Satellite Status and Information'

// from the UBlox9 document, the largest payout we receive is the NAV-SAT and the payload size
// is calculated as 8 + 12*numCh.  numCh in the case of a M9N is 42.
#define UBLOX_PAYLOAD_SIZE (8 + 12 * 42)


// Receive buffer
static union {
    ubx_nav_posllh posllh;
    ubx_nav_status status;
    ubx_nav_solution solution;
    ubx_nav_velned velned;
    ubx_nav_pvt pvt;
    ubx_nav_svinfo svinfo;
    ubx_nav_sat sat;
    ubx_cfg_gnss gnss;
    ubx_ack ack;
    uint8_t bytes[UBLOX_PAYLOAD_SIZE];
} _buffer;

void _update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b)
{
    while (len--) {
        *ck_a += *data;
        *ck_b += *ck_a;
        data++;
    }
}

static bool UBLOX_parse_gps(void)
{
    uint32_t i;

    *gpsPacketLogChar = LOG_IGNORED;

    switch (_msg_id) {
    case MSG_POSLLH:
        *gpsPacketLogChar = LOG_UBLOX_POSLLH;
        //i2c_dataset.time                = _buffer.posllh.time;
        gpsSol.llh.lon = _buffer.posllh.longitude;
        gpsSol.llh.lat = _buffer.posllh.latitude;
        gpsSol.llh.altCm = _buffer.posllh.altitudeMslMm / 10;  //alt in cm
        gpsSetFixState(next_fix);
        _new_position = true;
        break;
    case MSG_STATUS:
        *gpsPacketLogChar = LOG_UBLOX_STATUS;
        next_fix = (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.status.fix_type == FIX_3D);
        if (!next_fix)
            DISABLE_STATE(GPS_FIX);
        break;
    case MSG_SOL:
        *gpsPacketLogChar = LOG_UBLOX_SOL;
        next_fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D);
        if (!next_fix)
            DISABLE_STATE(GPS_FIX);
        gpsSol.numSat = _buffer.solution.satellites;
        gpsSol.hdop = _buffer.solution.position_DOP;
#ifdef USE_RTC_TIME
        //set clock, when gps time is available
        if(!rtcHasTime() && (_buffer.solution.fix_status & NAV_STATUS_TIME_SECOND_VALID) && (_buffer.solution.fix_status & NAV_STATUS_TIME_WEEK_VALID)) {
            //calculate rtctime: week number * ms in a week + ms of week + fractions of second + offset to UNIX reference year - 18 leap seconds
            rtcTime_t temp_time = (((int64_t) _buffer.solution.week)*7*24*60*60*1000) + _buffer.solution.time + (_buffer.solution.time_nsec/1000000) + 315964800000LL - 18000;
            rtcSet(&temp_time);
        }
#endif
        break;
    case MSG_VELNED:
        *gpsPacketLogChar = LOG_UBLOX_VELNED;
        gpsSol.speed3d = _buffer.velned.speed_3d;       // cm/s
        gpsSol.groundSpeed = _buffer.velned.speed_2d;    // cm/s
        gpsSol.groundCourse = (uint16_t) (_buffer.velned.heading_2d / 10000);     // Heading 2D deg * 100000 rescaled to deg * 10
        _new_speed = true;
        break;
    case MSG_PVT:
        *gpsPacketLogChar = LOG_UBLOX_SOL;
        next_fix = (_buffer.pvt.flags & NAV_STATUS_FIX_VALID) && (_buffer.pvt.fixType == FIX_3D);
        gpsSol.llh.lon = _buffer.pvt.lon;
        gpsSol.llh.lat = _buffer.pvt.lat;
        gpsSol.llh.altCm = _buffer.pvt.hMSL / 10;  //alt in cm
        gpsSetFixState(next_fix);
        _new_position = true;
        gpsSol.numSat = _buffer.pvt.numSV;
        gpsSol.hdop = _buffer.pvt.pDOP;
        gpsSol.speed3d = (uint16_t) sqrtf(powf(_buffer.pvt.gSpeed / 10, 2.0f) + powf(_buffer.pvt.velD / 10, 2.0f));
        gpsSol.groundSpeed = _buffer.pvt.gSpeed / 10;    // cm/s
        gpsSol.groundCourse = (uint16_t) (_buffer.pvt.headMot / 10000);     // Heading 2D deg * 100000 rescaled to deg * 10
        _new_speed = true;
#ifdef USE_RTC_TIME
        //set clock, when gps time is available
        if (!rtcHasTime() && (_buffer.pvt.valid & NAV_VALID_DATE) && (_buffer.pvt.valid & NAV_VALID_TIME)) {
            dateTime_t dt;
            dt.year = _buffer.pvt.year;
            dt.month = _buffer.pvt.month;
            dt.day = _buffer.pvt.day;
            dt.hours = _buffer.pvt.hour;
            dt.minutes = _buffer.pvt.min;
            dt.seconds = _buffer.pvt.sec;
            dt.millis = (_buffer.pvt.nano > 0) ? _buffer.pvt.nano / 1000 : 0; //up to 5ms of error
            rtcSetDateTime(&dt);
        }
#endif
        break;
    case MSG_SVINFO:
        *gpsPacketLogChar = LOG_UBLOX_SVINFO;
        GPS_numCh = _buffer.svinfo.numCh;
        // 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
        // save up to GPS_SV_MAXSATS_LEGACY channels so the BF Configurator knows it's receiving the old sat list info format.
        if (GPS_numCh > GPS_SV_MAXSATS_LEGACY)
            GPS_numCh = GPS_SV_MAXSATS_LEGACY;
        for (i = 0; i < GPS_numCh; i++) {
            GPS_svinfo_chn[i] = _buffer.svinfo.channel[i].chn;
            GPS_svinfo_svid[i] = _buffer.svinfo.channel[i].svid;
            GPS_svinfo_quality[i] =_buffer.svinfo.channel[i].quality;
            GPS_svinfo_cno[i] = _buffer.svinfo.channel[i].cno;
        }
        for (i = GPS_numCh; i < GPS_SV_MAXSATS_LEGACY; i++) {
            GPS_svinfo_chn[i] = 0;
            GPS_svinfo_svid[i] = 0;
            GPS_svinfo_quality[i] = 0;
            GPS_svinfo_cno[i] = 0;
        }
        GPS_svInfoReceivedCount++;
        break;
    case MSG_SAT:
        *gpsPacketLogChar = LOG_UBLOX_SVINFO; // The logger won't show this is NAV-SAT instead of NAV-SVINFO
        GPS_numCh = _buffer.sat.numSvs;
        // We can receive here upto GPS_SV_MAXSATS_M9N channels, but since the majority of receivers currently in use are M8N or older,
        // 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
        // all their channel information on BF Configurator. When M9N's are more widespread it would be a good time to increase those arrays.
        if (GPS_numCh > GPS_SV_MAXSATS_M8N)
            GPS_numCh = GPS_SV_MAXSATS_M8N;
        for (i = 0; i < GPS_numCh; i++) {
            GPS_svinfo_chn[i] = _buffer.sat.svs[i].gnssId;
            GPS_svinfo_svid[i] = _buffer.sat.svs[i].svId;
            GPS_svinfo_cno[i] = _buffer.sat.svs[i].cno;
            GPS_svinfo_quality[i] =_buffer.sat.svs[i].flags;
        }
        for (i = GPS_numCh; i < GPS_SV_MAXSATS_M8N; i++) {
            GPS_svinfo_chn[i] = 255;
            GPS_svinfo_svid[i] = 0;
            GPS_svinfo_quality[i] = 0;
            GPS_svinfo_cno[i] = 0;
        }

        // Setting the number of channels higher than GPS_SV_MAXSATS_LEGACY is the only way to tell BF Configurator we're sending the
        // enhanced sat list info without changing the MSP protocol. Also, we're sending the complete list each time even if it's empty, so
        // BF Conf can erase old entries shown on screen when channels are removed from the list.
        GPS_numCh = GPS_SV_MAXSATS_M8N;
        GPS_svInfoReceivedCount++;
        break;
    case MSG_CFG_GNSS:
        {
            bool isSBASenabled = false;
            bool isM8NwithDefaultConfig = false;

            if ((_buffer.gnss.numConfigBlocks >= 2) &&
                (_buffer.gnss.configblocks[1].gnssId == 1) && //SBAS
                (_buffer.gnss.configblocks[1].flags & UBLOX_GNSS_ENABLE)) { //enabled

                isSBASenabled = true;
            }

            if ((_buffer.gnss.numTrkChHw == 32) &&  //M8N
                (_buffer.gnss.numTrkChUse == 32) &&
                (_buffer.gnss.numConfigBlocks == 7) &&
                (_buffer.gnss.configblocks[2].gnssId == 2) && //Galileo
                (_buffer.gnss.configblocks[2].resTrkCh == 4) && //min channels
                (_buffer.gnss.configblocks[2].maxTrkCh == 8) && //max channels
                !(_buffer.gnss.configblocks[2].flags & UBLOX_GNSS_ENABLE)) { //disabled

                isM8NwithDefaultConfig = true;
            }

            const uint16_t messageSize = 4 + (_buffer.gnss.numConfigBlocks * sizeof(ubx_configblock));

            ubx_message tx_buffer;
            memcpy(&tx_buffer.payload, &_buffer, messageSize);

            if (isSBASenabled && (gpsConfig()->sbasMode == SBAS_NONE)) {
                tx_buffer.payload.cfg_gnss.configblocks[1].flags &= ~UBLOX_GNSS_ENABLE; //Disable SBAS
            }

            if (isM8NwithDefaultConfig && gpsConfig()->gps_ublox_use_galileo) {
                tx_buffer.payload.cfg_gnss.configblocks[2].flags |= UBLOX_GNSS_ENABLE; //Enable Galileo
            }

            ubloxSendConfigMessage(&tx_buffer, MSG_CFG_GNSS, messageSize);
        }
        break;
    case MSG_ACK_ACK:
        if ((gpsData.ackState == UBLOX_ACK_WAITING) && (_buffer.ack.msgId == gpsData.ackWaitingMsgId)) {
            gpsData.ackState = UBLOX_ACK_GOT_ACK;
        }
        break;
    case MSG_ACK_NACK:
        if ((gpsData.ackState == UBLOX_ACK_WAITING) && (_buffer.ack.msgId == gpsData.ackWaitingMsgId)) {
            gpsData.ackState = UBLOX_ACK_GOT_NACK;
        }
        break;
    default:
        return false;
    }

    // we only return true when we get new position and speed data
    // this ensures we don't use stale data
    if (_new_position && _new_speed) {
        _new_speed = _new_position = false;
        return true;
    }
    return false;
}

static bool gpsNewFrameUBLOX(uint8_t data)
{
    bool parsed = false;

    switch (_step) {
        case 0: // Sync char 1 (0xB5)
            if (PREAMBLE1 == data) {
                _skip_packet = false;
                _step++;
            }
            break;
        case 1: // Sync char 2 (0x62)
            if (PREAMBLE2 != data) {
                _step = 0;
                break;
            }
            _step++;
            break;
        case 2: // Class
            _step++;
            _class = data;
            _ck_b = _ck_a = data;   // reset the checksum accumulators
            break;
        case 3: // Id
            _step++;
            _ck_b += (_ck_a += data);       // checksum byte
            _msg_id = data;
#if DEBUG_UBLOX_FRAMES
    debug[2] = _msg_id;
#endif
            break;
        case 4: // Payload length (part 1)
            _step++;
            _ck_b += (_ck_a += data);       // checksum byte
            _payload_length = data; // payload length low byte
            break;
        case 5: // Payload length (part 2)
            _step++;
            _ck_b += (_ck_a += data);       // checksum byte
            _payload_length += (uint16_t)(data << 8);
#if DEBUG_UBLOX_FRAMES
    debug[3] = _payload_length;
#endif
            if (_payload_length > UBLOX_PAYLOAD_SIZE) {
                _skip_packet = true;
            }
            _payload_counter = 0;   // prepare to receive payload
            if (_payload_length == 0) {
                _step = 7;
            }
            break;
        case 6:
            _ck_b += (_ck_a += data);       // checksum byte
            if (_payload_counter < UBLOX_PAYLOAD_SIZE) {
                _buffer.bytes[_payload_counter] = data;
            }
            if (++_payload_counter >= _payload_length) {
                _step++;
            }
            break;
        case 7:
            _step++;
            if (_ck_a != data) {
                _skip_packet = true;          // bad checksum
                gpsData.errors++;
            }
            break;
        case 8:
            _step = 0;

            shiftPacketLog();

            if (_ck_b != data) {
                *gpsPacketLogChar = LOG_ERROR;
                gpsData.errors++;
                break;              // bad checksum
            }

            GPS_packetCount++;

            if (_skip_packet) {
                *gpsPacketLogChar = LOG_SKIPPED;
                break;
            }

            if (UBLOX_parse_gps()) {
                parsed = true;
            }
    }
    return parsed;
}
#endif // USE_GPS_UBLOX

static void gpsHandlePassthrough(uint8_t data)
{
     gpsNewData(data);
 #ifdef USE_DASHBOARD
     if (featureIsEnabled(FEATURE_DASHBOARD)) {
         dashboardUpdate(micros());
     }
 #endif

 }

void gpsEnablePassthrough(serialPort_t *gpsPassthroughPort)
{
    waitForSerialPortToFinishTransmitting(gpsPort);
    waitForSerialPortToFinishTransmitting(gpsPassthroughPort);

    if (!(gpsPort->mode & MODE_TX))
        serialSetMode(gpsPort, gpsPort->mode | MODE_TX);

#ifdef USE_DASHBOARD
    if (featureIsEnabled(FEATURE_DASHBOARD)) {
        dashboardShowFixedPage(PAGE_GPS);
    }
#endif

    serialPassthrough(gpsPort, gpsPassthroughPort, &gpsHandlePassthrough, NULL);
}

float GPS_scaleLonDown = 1.0f;  // this is used to offset the shrinking longitude as we go towards the poles

void GPS_calc_longitude_scaling(int32_t lat)
{
    float rads = (fabsf((float)lat) / 10000000.0f) * 0.0174532925f;
    GPS_scaleLonDown = cos_approx(rads);
}

////////////////////////////////////////////////////////////////////////////////////
// Calculate the distance flown and vertical speed from gps position data
//
static void GPS_calculateDistanceFlownVerticalSpeed(bool initialize)
{
    static int32_t lastCoord[2] = { 0, 0 };
    static int32_t lastAlt;
    static int32_t lastMillis;

    int currentMillis = millis();

    if (initialize) {
        GPS_distanceFlownInCm = 0;
        GPS_verticalSpeedInCmS = 0;
    } else {
        if (STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED)) {
            uint16_t speed = gpsConfig()->gps_use_3d_speed ? gpsSol.speed3d : gpsSol.groundSpeed;
            // Only add up movement when speed is faster than minimum threshold
            if (speed > GPS_DISTANCE_FLOWN_MIN_SPEED_THRESHOLD_CM_S) {
                uint32_t dist;
                int32_t dir;
                GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &lastCoord[GPS_LATITUDE], &lastCoord[GPS_LONGITUDE], &dist, &dir);
                if (gpsConfig()->gps_use_3d_speed) {
                    dist = sqrtf(powf(gpsSol.llh.altCm - lastAlt, 2.0f) + powf(dist, 2.0f));
                }
                GPS_distanceFlownInCm += dist;
            }
        }
        GPS_verticalSpeedInCmS = (gpsSol.llh.altCm - lastAlt) * 1000 / (currentMillis - lastMillis);
        GPS_verticalSpeedInCmS = constrain(GPS_verticalSpeedInCmS, -1500, 1500);
    }
    lastCoord[GPS_LONGITUDE] = gpsSol.llh.lon;
    lastCoord[GPS_LATITUDE] = gpsSol.llh.lat;
    lastAlt = gpsSol.llh.altCm;
    lastMillis = currentMillis;
}

void GPS_reset_home_position(void)
{
    if (!STATE(GPS_FIX_HOME) || !gpsConfig()->gps_set_home_point_once) {
        if (STATE(GPS_FIX) && gpsSol.numSat >= 5) {
            GPS_home[GPS_LATITUDE] = gpsSol.llh.lat;
            GPS_home[GPS_LONGITUDE] = gpsSol.llh.lon;
            GPS_calc_longitude_scaling(gpsSol.llh.lat); // need an initial value for distance and bearing calc
            // Set ground altitude
            ENABLE_STATE(GPS_FIX_HOME);
        }
    }
    GPS_calculateDistanceFlownVerticalSpeed(true); //Initialize
}

////////////////////////////////////////////////////////////////////////////////////
#define DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS 1.113195f
#define TAN_89_99_DEGREES 5729.57795f
// Get distance between two points in cm
// Get bearing from pos1 to pos2, returns an 1deg = 100 precision
void GPS_distance_cm_bearing(int32_t *currentLat1, int32_t *currentLon1, int32_t *destinationLat2, int32_t *destinationLon2, uint32_t *dist, int32_t *bearing)
{
    float dLat = *destinationLat2 - *currentLat1; // difference of latitude in 1/10 000 000 degrees
    float dLon = (float)(*destinationLon2 - *currentLon1) * GPS_scaleLonDown;
    *dist = sqrtf(sq(dLat) + sq(dLon)) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS;

    *bearing = 9000.0f + atan2_approx(-dLat, dLon) * TAN_89_99_DEGREES;      // Convert the output radians to 100xdeg
    if (*bearing < 0)
        *bearing += 36000;
}

void GPS_calculateDistanceAndDirectionToHome(void)
{
    if (STATE(GPS_FIX_HOME)) {      // If we don't have home set, do not display anything
        uint32_t dist;
        int32_t dir;
        GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_home[GPS_LATITUDE], &GPS_home[GPS_LONGITUDE], &dist, &dir);
        GPS_distanceToHome = dist / 100;
        GPS_directionToHome = dir / 100;
    } else {
        GPS_distanceToHome = 0;
        GPS_directionToHome = 0;
    }
}

void onGpsNewData(void)
{
    if (!(STATE(GPS_FIX) && gpsSol.numSat >= 5)) {
        return;
    }

    //
    // Calculate time delta for navigation loop, range 0-1.0f, in seconds
    //
    // Time for calculating x,y speed and navigation pids
    static uint32_t nav_loopTimer;
    dTnav = (float)(millis() - nav_loopTimer) / 1000.0f;
    nav_loopTimer = millis();
    // prevent runup from bad GPS
    dTnav = MIN(dTnav, 1.0f);

    GPS_calculateDistanceAndDirectionToHome();
    if (ARMING_FLAG(ARMED)) {
        GPS_calculateDistanceFlownVerticalSpeed(false);
    }

#ifdef USE_GPS_RESCUE
    rescueNewGpsData();
#endif
}

void gpsSetFixState(bool state)
{
    if (state) {
        ENABLE_STATE(GPS_FIX);
        ENABLE_STATE(GPS_FIX_EVER);
    } else {
        DISABLE_STATE(GPS_FIX);
    }
}
#endif
