Forum Rule: Always post complete source code & details to reproduce any issue!
Page 2 of 5 FirstFirst 1 2 3 4 ... LastLast
Results 26 to 50 of 112

Thread: uBlox Library

  1. #26
    Senior Member
    Join Date
    Dec 2013
    Posts
    214
    Thanks Brian
    I do have improved version just not sure if i should release it now, has a bunch of debug print inside.
    I was hoping on releasing it sooner but unfortunately the recent weather up north USA put me into survival mode (no power, no internet, no water for almost a week ).

  2. #27
    Senior Member
    Join Date
    Dec 2013
    Posts
    214
    Update #3
    Ok so finally i had time to finish up this work-in-progress ver.1.0.2

    @ Mike
    I hope you still around i did manage to implement the commands CFG-NAV5 (0x06 0x24) and CFG-RATE (0x06 0x08)
    Gps.SetNAV5(3, true); //Set Dynamic platform model Navigation Engine Settings (0: portable, 2: stationary, 3: pedestrian, Etc)
    Gps.SetRATE(200, true); //Navigation/Measurement Rate Settings, e.G. 100ms => 10Hz, 200 => 5.00Hz, 1000ms => 1Hz, 10000ms => 0.1Hz

    The biggest change and challenge was implementing command generator, currently only three commands are using this new implementation.
    Because of this the v1.0.1 changing baud rate predefined SetGPSbaud9600() command is deprecated.
    The new way:
    SetGPSbaud(uint32_t baud, bool) ~ (Parameter, Boolean) = (true) Print Message Acknowledged on USB Serial Monitor
    SetNAV5(uint8_t dynModel, bool)
    SetRATE(uint16_t measRate, bool)

    I am also including example2.ino, hope this will explain the commands syntax without too many questions.

    v1.0.2
    UBLOX.cpp
    Code:
    /*
      UBLOX.cpp
      Brian R Taylor
      brian.taylor@bolderflight.com
      2016-11-03
    
      Copyright (c) 2016 Bolder Flight Systems
    
      Permission is hereby granted, free of charge, to any person obtaining a copy of this software
      and associated documentation files (the "Software"), to deal in the Software without restriction,
      including without limitation the rights to use, copy, modify, merge, publish, distribute,
      sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
      furnished to do so, subject to the following conditions:
    
      The above copyright notice and this permission notice shall be included in all copies or
      substantial portions of the Software.
    
      THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
      BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
      NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
      DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
      OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
    
      v1.0.2
      Chris O.
      2018-03-01 Changes:
      Identify the packet Class in receive message
      Identify the packet Message ID in receive message
      Identify the packet 16Bit Message payloadSize
      Fixed the checksum calculations, now it's based on*the*received message payload.
      This fixes the compatibility issues between NEO-M8 and NEO-M7-6 GPS series.
    
      2018-03-01 Addition:
      UBX-NAV-POSLLH - (0x01 0x02) Geodetic Position Solution
      2018-03-20
      UBX-NAV-ATT ---- (0x01 0x05) Attitude Solution
    
      /---  High level commands, for the user ---/
       NOTE: command(bool) == (true)Print Message Acknowledged on USB Serial Monitor
      end();                            // Disables Teensy serial communication, to re-enable, call begin(Baud)
      Poll_CFG_Port1(bool);             // Polls the configuration for one I/O Port, I/O Target 0x01=UART1
      Poll_NAV_PVT();                   // Polls UBX-NAV-PVT    (0x01 0x07) Navigation Position Velocity Time Solution
      Poll_NAV_POSLLH();                // Polls UBX-NAV-POSLLH (0x01 0x02) Geodetic Position Solution
      Poll_NAV_ATT();                   // Polls UBX-NAV-ATT    (0x01 0x05) Attitude Solution
      ### Periodic Auto Update ON,OFF Command ###
      Ena_NAV_PVT(bool);                // Enable periodic auto update NAV_PVT
      Dis_NAV_PVT(bool);                // Disable periodic auto update NAV_PVT
      Ena_NAV_ATT(bool);                // Enable periodic auto update NAV_ATT
      Dis_NAV_ATT(bool);                // Disable periodic auto update NAV_ATT
      Ena_NAV_POSLLH(bool);             // Enable periodic auto update NAV_POSLLH
      Dis_NAV_POSLLH(bool);             // Disable periodic auto update NAV_POSLLH
      ### u-blox Switch off all NMEA MSGs ###
      Dis_all_NMEA_Child_MSGs(bool);    // Disable All NMEA Child Messages Command
      ### High level Command Generator ###
      SetGPSbaud(uint32_t baud, bool)   // Set UBLOX GPS Port Configuration Baud rate
      SetNAV5(uint8_t dynModel, bool)   // Set Dynamic platform model Navigation Engine Settings (0:portable, 3:pedestrian, Etc)
      SetRATE(uint16_t measRate, bool)  // Set Navigation/Measurement Rate Settings (100ms=10.00Hz, 200ms=5.00Hz, 1000ms=1.00Hz, Etc)
    */
    
    /* Note: Acknowledgment
      When messages from the Class CFG are sent to the receiver, the receiver will send an Acknowledge (ACK-ACK)
      or a Not Acknowledge (ACK-NAK) message back to the sender, depending on whether or not the message
      was processed correctly.
    */
    
    // Teensy 3.0 || Teensy 3.1/3.2 || Teensy 3.5 || Teensy 3.6 || Teensy LC
    #if defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MK64FX512__) || \
      defined(__MK66FX1M0__) || defined(__MKL26Z64__)
    
    #include "Arduino.h"
    #include "UBLOX.h"
    
    /* default constructor */
    UBLOX::UBLOX() {}
    
    /* uBlox object, input the serial bus */
    UBLOX::UBLOX(uint8_t bus) {
      _bus = bus; // serial bus
    }
    
    void UBLOX::configure(uint8_t bus) {
      _bus = bus; // serial bus
    }
    
    /* starts the serial communication */
    void UBLOX::begin(int baud) {
    
      // initialize parsing state
      _fpos = 0;
    
      // select the serial port
    #if defined(__MK20DX128__) || defined(__MK20DX256__) ||  defined(__MKL26Z64__) // Teensy 3.0 || Teensy 3.1/3.2 || Teensy LC
    
      if (_bus == 3) {
        _port = &Serial3;
      }
      else if (_bus == 2) {
        _port = &Serial2;
      }
      else {
        _port = &Serial1;
      }
    
    #endif
    
    #if defined(__MK64FX512__) || defined(__MK66FX1M0__) // Teensy 3.5 || Teensy 3.6
    
      if (_bus == 6) {
        _port = &Serial6;
      }
      else if (_bus == 5) {
        _port = &Serial5;
      }
      else if (_bus == 4) {
        _port = &Serial4;
      }
      else if (_bus == 3) {
        _port = &Serial3;
      }
      else if (_bus == 2) {
        _port = &Serial2;
      }
      else {
        _port = &Serial1;
      }
    
    #endif
    
      // begin the serial port for uBlox
      _port->begin(baud);
      delay(50); // wait for a bit, need for commands execution @ cold boot
      // check if the hardware serial port has room for outgoing bytes to TX buffer
      while (38 > _port->availableForWrite()) { }
      _port->write(_Poll_CFG_PRT, sizeof(_Poll_CFG_PRT));
      _port->flush();
    }
    // end the serial port for uBlox
    void UBLOX::end() {
      _port->flush(); // Waits for the transmission of outgoing serial data to complete
      _port->end();   // Disables serial communication
    }
    
    // Generate the uBlox command configuration
    void UBLOX::writeCommand(uint8_t CLASS, uint8_t ID , uint8_t PayloadLen0, uint8_t PayloadLen1, uint8_t Identifier, uint8_t reserved0, uint8_t Par0, uint8_t Par1, uint8_t Par2, uint8_t Par3) const {
      const uint8_t HeaderLength = 2; // 2 synchronization characters: 0xB5 0x62.
      const uint8_t CLASSandID = 2;   // 1-byte Message Class field and 1-byte Message ID field
      const uint8_t LenF = 2; // 2-byte 16-bit Length field
      const uint8_t CRC = 2;  // 2-byte 16-bit checksum
      uint16_t Bit16PayloadLen = (PayloadLen1 << 8) | PayloadLen0; // Combining two uint8_t as uint16_t
      uint8_t DATA [Bit16PayloadLen] {}; // Set up DATA array with size of 16 bit payload length
    
      switch (CLASS) { //
        case 0x00:
        case 0x01:    // NAV 0x01
        case 0x02:    // RXM 0x02
        case 0x04:    // INF 0x04
        case 0x05:    // ACK 0x05
          Serial.println("Not Supported COMMAND CLASS:"), Serial.println(CLASS, HEX);
          break;
        case 0x06:    // CLASS CFG 0x06
    
          // ID CFG-PRT (0x00)
          if (ID == 0x00) { // ID CFG-PRT (0x06 0x00) Port Configuration
            // SET   9600_CFG_PRT[28] = {0xB5, 0x62, 0x06, 0x00, 0x14, 0x00, 0x01, 0x00, 0x00, 0x00, 0xC0, 0x08, 0x00, 0x00, 0x80, 0x25, 0x00, 0x00, 0x07, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x92, 0xB5};
            // SET 460800_CFG_PRT[28] = {0xB5, 0x62, 0x06, 0x00, 0x14, 0x00, 0x01, 0x00, 0x00, 0x00, 0xD0, 0x08, 0x00, 0x00, 0x00, 0x08, 0x07, 0x00, 0x07, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0C, 0xBC};
            //                          |HEADER    |CLASS| ID  |16-Bit LOAD| --[0]---[1]---[2]---[3]--m[4]---[5]---[6]---[7]->b[8]--[9]--[10]--[11]<--[12]--[13]--[14]--[15]--[16]--[17]--[18]--[19]|--- CRC----|
            //                          |          |6 CFG|0 PRT|0x14 =DEC20| ^------------------ packet - payload ----------->  baudRate uint32_t  <----------------------------------------------^ |           |
            if (Bit16PayloadLen == 0x14) { // Length (Bytes) DEC 20
              if (Identifier == 0x01) {   // Port Identifier Number uart1
                DATA [0] = 0x01;          // 0 U1 - portID
                DATA [1] = 0x00;          // 1 U1 - reserved1
                DATA [2] = 0x00;          // 2 X2 - txReady
                DATA [3] = 0x00;          //   X2 - txReady
                uint32_t GPSBaud = (((Par0 << 24) | (Par1 << 16)) | (Par2 << 8)) | Par3; // Combining
                if (GPSBaud == 9600) {    // 4 X4 - mode - A bit mask describing the UART mode
                  DATA [4] = 0xC0;        //        @9600=0xC0,   *b11000000‬
                } else { // NOT SURE HERE, ISSUE UBX-CFG-PRT (0x06 0x00) Bit Field mode ACCESSING UNDOCUMENTED BIT
                  DATA [4] = 0xD0;        //        @460800=0xD0, *b11010000‬ U-CENTER ISSUE ?
                }
                DATA [5] = 0x08;          //   X4 - mode
                DATA [6] = 0x00;          //   X4 - mode
                DATA [7] = 0x00;          //   X4 - mode
                DATA [8] = Par0;          // 8 U4 - 32bit baudRate Bits/s Baud rate in bits/second
                DATA [9] = Par1;          //           >baudRate uint32_t< DATA [8]-[9]-[10]-[11]
                DATA [10] = Par2;         //           Little Endian:     Hex:  00--08---07---00
                DATA [11] = Par3;         //           Big Endian:        Hex:  00--07---08---00 Decimal = **460800‬baud
                DATA [12] = 0x07;         // 12 X2 - inProtoMask
                DATA [13] = 0x00;         //    X2 - inProtoMask
                DATA [14] = 0x03;         // 14 X2 - outProtoMask
                DATA [15] = 0x00;         //    X2 - outProtoMask
                DATA [16] = 0x00;         // 16 X2 - flags
                DATA [17] = 0x00;         //    X2 - flags
                DATA [18] = 0x00;         // 18 U1 - reserved1
                DATA [19] = 0x00;         // 19 U1 - reserved2
              }
            }
          } // end UBX-CFG-PRT (0x06 0x00)
    
          // ID CFG-RATE (0x08)
          if (ID == 0x08) { // ID CFG-RATE (0x06 0x08) Navigation/Measurement Rate Settings, e.g. 100ms => 10Hz, 1000ms => 1Hz, 10000ms => 0.1Hz
            if (Bit16PayloadLen == 0x06) { // Length (Bytes) DEC 6
              DATA [0] = Identifier;     // 0  U2 - measRate ms
              DATA [1] = reserved0;      //    U2 - measRate
              DATA [2] = Par0;           // 2  U2 - navRate
              DATA [3] = Par1;           //    U2 - navRate
              DATA [4] = Par2;           // 4  U2 - timeRef
              DATA [5] = Par3;           //    U2 - timeRef
            }
          } // end UBX-CFG-RATE (0x06 0x08)
    
          // ID CFG-NAV5 (0x24)
          if (ID == 0x24) { // ID CFG-NAV5 (0x06 0x24) Navigation Engine Settings
            if (Bit16PayloadLen == 0x24) { // Length (Bytes) DEC 36
              DATA [0] = Identifier;     // 0  X2 - mask
              DATA [1] = reserved0;      //    X2 - mask
              DATA [2] = Par0;           // 2  U1 - dynModel
              DATA [3] = 0x00;           // 3  U1 - fixMode
              DATA [4] = 0x00;           // 4  I4 - fixedAlt, Scaling 0.01
              DATA [5] = 0x00;           //    I4 - fixedAlt
              DATA [6] = 0x00;           //    I4 - fixedAlt
              DATA [7] = 0x00;           //    I4 - fixedAlt
              DATA [8] = 0x00;           // 8  U4 - fixedAltVar, Scaling 0.0001
              DATA [9] = 0x00;           //    U4 - fixedAltVar
              DATA [10] = 0x00;          //    U4 - fixedAltVar
              DATA [11] = 0x00;          //    U4 - fixedAltVar
              DATA [12] = 0x00;          // 12 I1 - minElev
              DATA [13] = 0x00;          // 13 U1 - drLimit
              DATA [14] = 0x00;          // 14 U2 - pDop, Scaling 0.1
              DATA [15] = 0x00;          //    U2 - pDop
              DATA [16] = 0x00;          // 16 U2 - tDop, Scaling 0.1
              DATA [17] = 0x00;          //    U2 - tDop
              DATA [18] = 0x00;          // 18 U2 - pAcc
              DATA [19] = 0x00;          //    U2 - pAcc
              DATA [20] = 0x00;          // 20 U2 - tAcc
              DATA [21] = 0x00;          //    U2 - tAcc
              DATA [22] = 0x00;          // 22 U1 - staticHoldThresh
              DATA [23] = 0x00;          // 23 U1 - dgnssTimeout
              DATA [24] = 0x00;          // 24 U1 - cnoThreshNumSVs
              DATA [25] = 0x00;          // 25 U1 - cnoThresh
              DATA [26] = 0x00;          // 26 U1[2] - reserved1
              DATA [27] = 0x00;          //    U1 -    reserved1
              DATA [28] = 0x00;          // 28 U2 - staticHoldMaxDist
              DATA [29] = 0x00;          //    U2 - staticHoldMaxDist
              DATA [30] = 0x00;          // 30 U1 - utcStandard
              DATA [31] = 0x00;          // 31 U1[5] - reserved2
              DATA [32] = 0x00;          //    U1 -    reserved2
              DATA [33] = 0x00;          //    U1 -    reserved2
              DATA [34] = 0x00;          //    U1 -    reserved2
              DATA [35] = 0x00;          //    U1 -    reserved2
            }
          } // end UBX-CFG-NAV5 (0x06 0x24)
          break;
        case 0x0A:    // MON 0x0A
        case 0x0B:    // AID 0x0B
        case 0x0D:    // TIM 0x0D
        case 0x10:    // ESF 0x10
        case 0x13:    // MGA 0x13
        case 0x21:    // LOG 0x21
        case 0x27:    // SEC 0x27
        case 0x28:    // HNR 0x28
        //break;
        default:
          // Not Supported
          Serial.println("Not Supported COMMAND CLASS:"), Serial.println(CLASS, HEX);
          break;
      }
      // The checksum is calculated over the packet, starting and including the
      // CLASS field, ID field and payload, up until, but excluding, the Checksum Field
      // (CRC) Compute checksum
      // NOTE: @ GPS baud 460800 CRC = 0x0C, 0xBC
      uint8_t CK_A = 0, CK_B = 0;
      CK_A = CK_A + CLASS;
      CK_B = CK_B + CK_A;
      CK_A = CK_A + ID;
      CK_B = CK_B + CK_A;
      CK_A = CK_A + PayloadLen0;
      CK_B = CK_B + CK_A;
      CK_A = CK_A + PayloadLen1;
      CK_B = CK_B + CK_A;
      for (int i = 0; i < Bit16PayloadLen ; i++) {
        CK_A = CK_A + DATA[i];
        CK_B = CK_B + CK_A;
      }
      // Generate the command
      if (Bit16PayloadLen == 0x06) { // Payload Length HEX 06 ~ DEC 6
        uint8_t COMMpayload[HeaderLength + CLASSandID + LenF + Bit16PayloadLen + CRC] = {
          /* Header */
          0xB5,           //[ 0] synchronization characters: 0xB5 0x62
          0x62,           //[ 1]
          CLASS,          //[ 2] Destination,  // B5 62 06 08 06 00 D0 07 01 00 01 00 ED BD
          ID,             //[ 3]
          PayloadLen0,    //[ 4] 8bit Payload Length
          PayloadLen1,    //[ 5] 8bit Payload Length
          (DATA [0]),     //[ 6] Payload HEX 0x06 ~ DEC 6
          (DATA [1]),     //[ 7]
          (DATA [2]),     //[ 8]
          (DATA [3]),     //[ 9]
          (DATA [4]),     //[10]
          (DATA [5]),     //[11]
          CK_A,           //[12] CRC 16-bit
          CK_B            //[13]
        };
        _port->flush(); // Waits for the transmission of outgoing serial data to complete
        _port->write(COMMpayload, sizeof(COMMpayload));
      }
      if (Bit16PayloadLen == 0x14) { // Payload Length HEX 14 ~ DEC 20
        uint8_t COMMpayload[HeaderLength + CLASSandID + LenF + Bit16PayloadLen + CRC] = {
          /* Header */
          0xB5,           //[ 0] synchronization characters: 0xB5 0x62
          0x62,           //[ 1]
          CLASS,          //[ 2] Destination
          ID,             //[ 3]
          PayloadLen0,    //[ 4] 8bit Payload Length
          PayloadLen1,    //[ 5] 8bit Payload Length
          (DATA [0]),     //[ 6] Payload HEX 14 ~ DEC 20
          (DATA [1]),     //[ 7]
          (DATA [2]),     //[ 8]
          (DATA [3]),     //[ 9]
          (DATA [4]),     //[10]
          (DATA [5]),     //[11]
          (DATA [6]),     //[12]
          (DATA [7]),     //[13]
          (DATA [8]),     //[14]
          (DATA [9]),     //[15]
          (DATA [10]),    //[16]
          (DATA [11]),    //[17]
          (DATA [12]),    //[18]
          (DATA [13]),    //[19]
          (DATA [14]),    //[20]
          (DATA [15]),    //[21]
          (DATA [16]),    //[22]
          (DATA [17]),    //[23]
          (DATA [18]),    //[24]
          (DATA [19]),    //[25]
          CK_A,           //[26] CRC 16-bit
          CK_B            //[27]
        };
        _port->flush(); // Waits for the transmission of outgoing serial data to complete
        _port->write(COMMpayload, sizeof(COMMpayload));
      }
      if (Bit16PayloadLen == 0x24) { // Payload Length HEX 24 ~ DEC 36
        uint8_t COMMpayload[HeaderLength + CLASSandID + LenF + Bit16PayloadLen + CRC] = {
          /* Header */
          0xB5,           //[ 0] synchronization characters: 0xB5 0x62
          0x62,           //[ 1]
          CLASS,          //[ 2] Destination
          ID,             //[ 3]
          PayloadLen0,    //[ 4] 8bit Payload Length
          PayloadLen1,    //[ 5] 8bit Payload Length
          (DATA [0]),     //[ 6] Payload HEX 24 ~ DEC 36
          (DATA [1]),     //[ 7]
          (DATA [2]),     //[ 8]
          (DATA [3]),     //[ 9]
          (DATA [4]),     //[10]
          (DATA [5]),     //[11]
          (DATA [6]),     //[12]
          (DATA [7]),     //[13]
          (DATA [8]),     //[14]
          (DATA [9]),     //[15]
          (DATA [10]),    //[16]
          (DATA [11]),    //[17]
          (DATA [12]),    //[18]
          (DATA [13]),    //[19]
          (DATA [14]),    //[20]
          (DATA [15]),    //[21]
          (DATA [16]),    //[22]
          (DATA [17]),    //[23]
          (DATA [18]),    //[24]
          (DATA [19]),    //[25]
          (DATA [20]),    //[26]
          (DATA [21]),    //[27]
          (DATA [22]),    //[28]
          (DATA [23]),    //[29]
          (DATA [24]),    //[30]
          (DATA [25]),    //[31]
          (DATA [26]),    //[32]
          (DATA [27]),    //[33]
          (DATA [28]),    //[34]
          (DATA [29]),    //[35]
          (DATA [30]),    //[36]
          (DATA [31]),    //[37]
          (DATA [32]),    //[38]
          (DATA [33]),    //[39]
          (DATA [34]),    //[40]
          (DATA [35]),    //[41]
          CK_A,           //[42] CRC 16-bit
          CK_B            //[43]
        };
        _port->flush(); // Waits for the transmission of outgoing serial data to complete
        _port->write(COMMpayload, sizeof(COMMpayload));
      }
    }
    
    /* write the uBlox data */
    /******************************************/
    /**  High level commands, for the user    */
    /******************************************/
    
    // UBX-CFG-PRT (0x06 0x00) SetGPSbaud
    // Generate the configuration command's syntax
    void UBLOX::SetGPSbaud(uint32_t baud32rate, bool printACK) {
      bool Supported = false;
      if ( printACK == true ) {
        Serial.print("Setting CFG-PRT (0x06 0x00) GPS UART baud rate:");
        _pACK2 = true; // print ACK to USB
      }
      // Set GPS Uart1 baud rate (32bit)
      // Possible Configurations
      switch (baud32rate) {
        case 4800:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("4800");
          }
          break;
        case 9600:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("9600");
          }
          break;
        case 19200:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("19200");
          }
          break;
        case 38400:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("38400");
          }
          break;
        case 57600:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("57600");
          }
          break;
        case 115200:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("115200");
          }
          break;
        case 230400:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("230400");
          }
          break;
        case 460800:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("460800");
          }
          break;
        default:
          Supported = false;
          _pACK2 = false;
          Serial.print("\tNot Supported :"), Serial.println(baud32rate), Serial.println("\tPossible Baud Rate Configurations");
          Serial.println("\t4800~9600~19200~38400~57600~115200~230400~460800"), Serial.println();
          break;
      }
      if ( Supported == true ) {
        // Splitting an uint32_t baud32rate
        uint8_t Par3 = ((baud32rate & 0xFF000000) >> 24); // Parameter3
        uint8_t Par2 = ((baud32rate & 0x00FF0000) >> 16);
        uint8_t Par1 = ((baud32rate & 0x0000FF00) >> 8);
        uint8_t Par0 =  (baud32rate & 0x000000FF);
        uint8_t CLASS = 0x06, ID = 0x00, Length0 = 0x14, Length1 = 0x00, Identifier = 0x01, res0 = 0; // reserved0 -- NOT IN USE
        // Identifier Port # Electrical Interface
        // 0 DDC (IC compatible)
        // 1 UART 1
        // 3 USB
        // 4 SPI
        writeCommand(CLASS, ID, Length0, Length1, Identifier, res0, Par0, Par1, Par2, Par3);
      }
    }
    
    // UBX-CFG-RATE (0x06 0x08)  Set Navigation/Measurement Rate Settings (100ms=10.00Hz, 200ms=5.00Hz, 1000ms=1.00Hz, Etc)
    // Generate the configuration command's syntax
    void UBLOX::SetRATE(uint16_t measRate, bool printACK) {
      bool Supported = false;
      if ( printACK == true ) {
        Serial.print("Setting CFG-RATE (0x06 0x08) Navigation/Measurement Rate ");
        _pACK_RATE = true; // print ACK to USB
      }
      // Possible Configurations
      // _______________________________________________________________
      // 60ms    16.67Hz <-- | 192ms    5.21Hz     | 600ms    1.67Hz
      // 64ms    15.63Hz <-- | 200ms    5.00Hz <-- | 625ms    1.60Hz
      // 72ms    13.89Hz <-- | 225ms    4.44Hz     | 640ms    1.56Hz
      // 75ms    13.33Hz <-- | 240ms    4.17Hz     | 720ms    1.39Hz
      // 80ms    12.50Hz <-- | 250ms    4.00Hz <-- | 750ms    1.33Hz
      // 90ms    11.11Hz     | 288ms    3.47Hz     | 800ms    1.25Hz <--
      // 96ms    10.42Hz     | 300ms    3.33Hz     | 900ms    1.11Hz
      // 100ms   10.00Hz <-- | 320ms    3.13Hz     | 960ms    1.04Hz
      // 120ms    8.33Hz     | 360ms    2.78Hz     | 1000ms   1.00Hz <-- 
      // 125ms    8.00Hz <-- | 375ms    2.67Hz     | 2000ms   0.50Hz <-- 
      // 128ms    7.81Hz     | 400ms    2.50Hz <-- | 4000ms   0.25Hz <--
      // 144ms    6.94Hz     | 450ms    2.22Hz     | 10000ms  0.10Hz <--
      // 150ms    6.67Hz     | 480ms    2.08Hz     | 20000ms  0.05Hz <--
      // 160ms    6.25Hz     | 500ms    2.00Hz <-- | 50000ms  0.02Hz <-- 
      // 180ms    5.56Hz     | 576ms    1.74Hz     |
      // NOTE: Only rate settings marked with arrows are the implemented configuration.
      switch (measRate) {
        case 60:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("60ms 16.67Hz");
          }
          break;
        case 64:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("64ms 15.63Hz");
          }
          break;
        case 72:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("72ms 13.89Hz");
          }
          break;
        case 80:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("80ms 12.50Hz");
          }
          break;
        case 100:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("100ms 10.00Hz");
          }
          break;
        case 125:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("125ms 8.00Hz");
          }
          break;
        case 200:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("200ms 5.00Hz");
          }
          break;
        case 250:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("250ms 4.00Hz");
          }
          break;
        case 400:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("400ms 2.50Hz");
          }
          break;
        case 500:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("500ms 2.00Hz");
          }
          break;
        case 800:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("800ms 1.25Hz");
          }
          break;
        case 1000:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("1000ms 1.00Hz");
          }
          break;
        case 2000:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("2000ms 0.50Hz");
          }
          break;
        case 4000:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("4000ms 0.25Hz");
          }
          break;
        case 10000:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("10000ms 0.10Hz");
          }
          break;
        case 20000:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("20000ms 0.05Hz");
          }
          break;
        case 50000:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("50000ms 0.02Hz");
          }
          break;
        default:
          Supported = false;
          _pACK_RATE = false;
          Serial.print("\tNot Supported :"), Serial.println(measRate), Serial.println("\tPossible Configurations");
          Serial.println("\t60=16.67Hz 64=15.63Hz 72=13.89Hz 80=12.50Hz 100=10.00Hz 125=8.00Hz 200=5.00Hz 250=4.00Hz 500=2.00Hz");
          Serial.println("\t800=1.25Hz 1000=1.00Hz 2000=0.50Hz 4000=0.25Hz 10000=0.10Hz 20000=0.05Hz 50000=0.02Hz "), Serial.println();
          break;
      }
      if ( Supported == true ) {
        // Splitting an uint16_t measRate
        // Identifier & res0 - 16bit Parameter defines the rate. e.g. 100ms => 10Hz, 1000ms => 1Hz, 10000ms => 0.1Hz.
        uint8_t res0 = ((measRate & 0xFF00) >> 8);
        uint8_t Identifier =  (measRate & 0x00FF);
        uint8_t CLASS = 0x06, ID = 0x08, Length0 = 0x06, Length1 = 0x00;
        uint8_t Par0 = 0x01, Par1 = 0x00, Par2 = 0x01, Par3 = 0x00; // navRate and timeRef Parameters.
        writeCommand(CLASS, ID, Length0, Length1, Identifier, res0, Par0, Par1, Par2, Par3);
      }
    }
    
    // UBX-CFG-NAV5 (0x06 0x24) - Set Navigation Engine Settings
    // Generate the configuration command's syntax
    void UBLOX::SetNAV5(uint8_t dynModel, bool printACK) {
      bool Supported = false;
      uint8_t Par0 = 0;
      if ( printACK == true ) {
        Serial.print("Setting CFG-NAV5 (0x06 0x24) Dynamic platform model ");
        _pACK_NAV5 = true;
      }
      // Possible Configurations Dynamic platform model:
      // 0: portable
      // 2: stationary
      // 3: pedestrian
      // 4: automotive
      // 5: sea
      // 6: airborne with <1g acceleration
      // 7: airborne with <2g acceleration
      // 8: airborne with <4g acceleration
      // 9: wrist worn watch (not supported in protocol versions less than 18)
      switch (dynModel) {
        case 0:
          Supported = true;
          Par0 = 0;        // Parameter0
          if ( printACK == true ) {
            Serial.println("0: portable");
          }
          break;
        case 2:
          Supported = true;
          Par0 = 2;
          if ( printACK == true ) {
            Serial.println("2: stationary");
          }
          break;
        case 3:
          Supported = true;
          Par0 = 3;
          if ( printACK == true ) {
            Serial.println("3: pedestrian");
          }
          break;
        case 4:
          Supported = true;
          Par0 = 4;
          if ( printACK == true ) {
            Serial.println("4: automotive");
          }
          break;
        case 5:
          Supported = true;
          Par0 = 5;
          if ( printACK == true ) {
            Serial.println("5: sea");
          }
          break;
        case 6:
          Supported = true;
          Par0 = 6;
          if ( printACK == true ) {
            Serial.println("6: airborne with <1g acceleration");
          }
          break;
        case 7:
          Supported = true;
          Par0 = 7;
          if ( printACK == true ) {
            Serial.println("7: airborne with <2g acceleration");
          }
          break;
        case 8:
          Supported = true;
          Par0 = 8;
          if ( printACK == true ) {
            Serial.println("8: airborne with <4g acceleration");
          }
          break;
        case 9:
          Supported = true;
          Par0 = 9;
          if ( printACK == true ) {
            Serial.println("9: wrist worn watch (not supported in protocol Ver. less than 18");
          }
          break;
        default:
          Supported = false;
          _pACK_NAV5 = false;
          Serial.print("\tNot Supported :"), Serial.println(dynModel), Serial.println("\tPossible Configurations");
          Serial.println("\t0: portable 2: stationary 3: pedestrian 4: automotive 5: sea 6: airborne with <1g 7: airborne with <2g");
          Serial.println("\t8: airborne with <4g 9: wrist worn watch (not supported in protocol v.less than 18)"), Serial.println();
          break;
      }
      if ( Supported == true ) {
        // Identifier & res0 - 16bit Parameter bitmask. Only the masked parameters will be applied.
        uint8_t CLASS = 0x06, ID = 0x24, Length0 = 0x24, Length1 = 0x00, Identifier = B00000001, res0 = B00000000; // reserved0
        uint8_t Par1 = 0, Par2 = 0, Par3 = 0; // Parameters, Par0 set in switch case.
        writeCommand(CLASS, ID, Length0, Length1, Identifier, res0, Par0, Par1, Par2, Par3);
      }
    }
    
    void UBLOX::Poll_GPSbaud_Port1(bool printACK) {
      // CFG-PRT (0x06 0x00) Polls the configuration for one I/O Port, (I/O Target # MSG) 0x01=I/O UART1 0x06001
      while (10 > _port->availableForWrite()) { }
      _port->write(_Poll_CFG_PRT, sizeof(_Poll_CFG_PRT));
      if ( printACK == true ) {
        _printACK = 0x06001; // print ACK to USB - Clas 06, Id 00, print ACK I/O UART1 0x06001
        parse(); // force reading serial on cold boot
      }
    }
    void UBLOX::Poll_NAV_PVT() {
      while (12 > _port->availableForWrite()) { } // check if the hardware serial port has room for outgoing bytes to TX buffer
      _port->write(_Poll_NAV_PVT, sizeof(_Poll_NAV_PVT));
    }
    void UBLOX::Poll_NAV_POSLLH() {
      while (12 > _port->availableForWrite()) { }
      _port->write(_Poll_NAV_POSLLH, sizeof(_Poll_NAV_POSLLH));
    }
    void UBLOX::Poll_NAV_ATT() {
      _port->write(_Poll_NAV_ATT, sizeof(_Poll_NAV_ATT));
    }
    // #### Periodic auto update ON,OFF command ####
    void UBLOX::Ena_NAV_PVT(bool printACK) { // Enable periodic auto update NAV_PVT
      while (12 > _port->availableForWrite()) { }
      _port->write(_Ena_NAV_PVT, sizeof(_Ena_NAV_PVT));
      if ( printACK == true ) {
        Serial.println("Setting Ena_NAV_PVT");
        _printACK_PVT = 0x06011; // Clas 06, Id 01, Ena 1 - print ACK Ena_NAV_PVT 0x06011
      }
    }
    void UBLOX::Dis_NAV_PVT(bool printACK) { // Disable periodic auto update NAV_PVT
      while (12 > _port->availableForWrite()) { }
      _port->write(_Dis_NAV_PVT, sizeof(_Dis_NAV_PVT));
      if ( printACK == true ) {
        Serial.println("Setting Dis_NAV_PVT");
        _printACK_PVT = 0x06010; // Clas 06, Id 01, Dis 0 - print ACK Dis_NAV_PVT 0x06010
      }
    }
    void UBLOX::Ena_NAV_ATT(bool printACK) { // Enable periodic auto update NAV_ATT
      while (12 > _port->availableForWrite()) { }
      _port->write(_Ena_NAV_ATT, sizeof(_Ena_NAV_ATT));
      if ( printACK == true ) {
        Serial.println("Setting Ena_NAV_ATT");
        _printACK_ATT = 0x06011; // Clas 06, Id 01, Ena 1
      } else {
        _printACK_ATT = 0xFF; // need for ACK_NAK, NAV_ATT Supported only on protocol version 19 (only with ADR or UDR products)
      }
    }
    void UBLOX::Dis_NAV_ATT(bool printACK) { // Disable periodic auto update NAV_ATT
      while (12 > _port->availableForWrite()) { }
      _port->write(_Dis_NAV_ATT, sizeof(_Dis_NAV_ATT));
      if ( printACK == true ) {
        Serial.println("Setting Dis_NAV_ATT");
        _printACK_ATT = 0x06010; // Clas 06, Id 01, Dis 0
      } else {
        _printACK_ATT = 0xFF;
      }
    }
    void UBLOX::Ena_NAV_POSLLH(bool printACK) { // Enable periodic auto update NAV_POSLLH
      while (12 > _port->availableForWrite()) { }
      _port->write(_Ena_NAV_POSLLH, sizeof(_Ena_NAV_POSLLH));
      if ( printACK == true ) {
        Serial.println("Setting Ena_NAV_POSLLH");
        _printACK_POSLLH = 0x06011; // Clas 06, Id 01, Ena 1
      }
    }
    void UBLOX::Dis_NAV_POSLLH(bool printACK) { // Disable periodic auto update NAV_POSLLH
      while (12 > _port->availableForWrite()) { }
      _port->write(_Dis_NAV_POSLLH, sizeof(_Dis_NAV_POSLLH));
      if ( printACK == true ) {
        Serial.println("Setting Dis_NAV_POSLLH");
        _printACK_POSLLH = 0x06010; // Clas 06, Id 01, Dis 0
      }
    }
    
    // #### u-blox Disable All NMEA Child Messages Command ####
    void UBLOX::Dis_all_NMEA_Child_MSGs(bool printACK) {
      if ( printACK == true ) {
        // Ensure any pending writes are complete before writing new data
        while ( 38 >= _port->availableForWrite() ) {} // make sure we have at least 39 bytes available in TX buffer on start
        /*int wr;
          wr = _port->availableForWrite();
          if (wr <= 39) {
          Serial.print(" Buffer Available For Write only=");
          Serial.println(wr);
          Serial.print(" Please edit Teensyduino Core file serial"), Serial.print(_bus), Serial.println(".c, look for #define SERIAL#_TX_BUFFER_SIZE  40 // number of outgoing bytes to buffer");
          Serial.println(" This Command (Dis_all_NMEA_Child_MSGs(true) - needs 165 bytes of TX BUFFER) recommended TX buffer of 200");
          Serial.println(" I also strongly suggest changing RX buffer to 255 or more,*otherwise you will miss ACKs*on serial monitor");
          }*/
    
        _printACK_Dis_all_NMEA = 0x06010; // print ACK to USB - Clas 06, Id 01, print ACK Dis_all_NMEA 0x0601
        Serial.println("Dis_all_NMEA_Child_MSGs");
        Serial.println("Setting Dis_NMEA_GxDTM");
        Serial.println("Setting Dis_NMEA_GxGBS");
        Serial.println("Setting Dis_NMEA_GxGGA");
        Serial.println("Setting Dis_NMEA_GxGLL");
        Serial.println("Setting Dis_NMEA_GxGNS");
        Serial.println("Setting Dis_NMEA_GxGRS");
        Serial.println("Setting Dis_NMEA_GxGSA");
        Serial.println("Setting Dis_NMEA_GxGST");
        Serial.println("Setting Dis_NMEA_GxGSV");
        Serial.println("Setting Dis_NMEA_GxRMC");
        // Serial.println("Setting Dis_NMEA_GxVLW");   // u-blox6 -- Message Not-Acknowledged
        Serial.println("Setting Dis_NMEA_GxVTG");
        Serial.println("Setting Dis_NMEA_GxZDA");
        Serial.println("Setting Dis_PUBXe_NMEA00");
        // Serial.println("Setting Dis_PUBXe_NMEA01"); // u-blox6 -- Message Not-Acknowledged
        Serial.println("Setting Dis_PUBXe_NMEA03");
        Serial.println("Setting Dis_PUBXe_NMEA04");
        // Serial.println("Setting Dis_PUBXe_NMEA05"); // u-blox6 -- Message Not-Acknowledged
        // Serial.println("Setting Dis_PUBXe_NMEA06"); // u-blox6 -- Message Not-Acknowledged
      } else {
        _printACK_Dis_all_NMEA = 0x00;
      }
      _ACKcount = 15; // Set NMEA ACK counter #
      while (12 > _port->availableForWrite()) {} // check if the hardware serial port has room for outgoing bytes to TX buffer
      _port->write(_Dis_NMEA_GxDTM, sizeof(_Dis_NMEA_GxDTM));             //#1  / NMEA GxDTM (DTM 0xF0 0x0A) Datum Reference)
      while (12 > _port->availableForWrite()) {}
      _port->write(_Dis_NMEA_GxGBS, sizeof(_Dis_NMEA_GxGBS));             //#2  / NMEA GxGBS (GBS 0xF0 0x09) GNSS Satellite Fault Detection)
      while (12 > _port->availableForWrite()) {}
      _port->write(_Dis_NMEA_GxGGA, sizeof(_Dis_NMEA_GxGGA));             //#3  / NMEA GxGGA (GGA 0xF0 0x00) Global positioning system fix data)
      while (12 > _port->availableForWrite()) {}
      _port->write(_Dis_NMEA_GxGLL, sizeof(_Dis_NMEA_GxGLL));             //#4  / NMEA GxGLL (GLL 0xF0 0x01) Latitude and longitude, with time of position fix and status)
      while (12 > _port->availableForWrite()) {}
      _port->write(_Dis_NMEA_GxGNS, sizeof(_Dis_NMEA_GxGNS));             //#5  / NMEA GxGNS (GNS 0xF0 0x0D) GNSS fix data)
      while (12 > _port->availableForWrite()) {}
      _port->write(_Dis_NMEA_GxGRS, sizeof(_Dis_NMEA_GxGRS));             //#6  / NMEA GxGRS (GRS 0xF0 0x06) GNSS Range Residuals)
      while (12 > _port->availableForWrite()) {}
      _port->write(_Dis_NMEA_GxGSA, sizeof(_Dis_NMEA_GxGSA));             //#7  / NMEA GxGSA (GSA 0xF0 0x02) GNSS DOP and Active Satellites)
      while (12 > _port->availableForWrite()) {}
      _port->write(_Dis_NMEA_GxGST, sizeof(_Dis_NMEA_GxGST));             //#8  / NMEA GxGST (GST 0xF0 0x07) GNSS Pseudo Range Error Statistics)
      while (12 > _port->availableForWrite()) {}
      _port->write(_Dis_NMEA_GxGSV, sizeof(_Dis_NMEA_GxGSV));             //#9  / NMEA GxGSV (GSV 0xF0 0x03) GNSS Satellites in View)
      while (12 > _port->availableForWrite()) {}
      _port->write(_Dis_NMEA_GxRMC, sizeof(_Dis_NMEA_GxRMC));             //#10 / NMEA GxRMC (RMC 0xF0 0x04) Recommended Minimum data)
      while (12 > _port->availableForWrite()) {}
      // u-blox6 -- Message Not-Acknowledged
      //_port->write(_Dis_NMEA_GxVLW, sizeof(_Dis_NMEA_GxVLW));           //    / NMEA GxVLW (VLW 0xF0 0x0F) Dual ground/water distance)
      _port->write(_Dis_NMEA_GxVTG, sizeof(_Dis_NMEA_GxVTG));             //#11 / NMEA GxVTG (VTG 0xF0 0x05) Course over ground and Ground speed)
      while (12 > _port->availableForWrite()) {}
      _port->write(_Dis_NMEA_GxZDA, sizeof(_Dis_NMEA_GxZDA));             //#12 / NMEA GxZDA (ZDA 0xF0 0x08) Time and Date)
      while (12 > _port->availableForWrite()) {}
      // #### PUBX- u-blox NMEA extension - Switch OFF ALL ####
      _port->write(_Dis_PUBXe_NMEA00, sizeof(_Dis_PUBXe_NMEA00));         //#13 / PUBX 00 (Position Data)
      while (12 > _port->availableForWrite()) {}
      // u-blox6 -- Message Not-Acknowledged
      //_port->write(_Dis_PUBXe_NMEA01, sizeof(_Dis_PUBXe_NMEA01));       //    / PUBX 01 (UTM Position Data)
      _port->write(_Dis_PUBXe_NMEA03, sizeof(_Dis_PUBXe_NMEA03));         //#14 / PUBX 03 (Satellite Data)
      while (12 > _port->availableForWrite()) {}
      _port->write(_Dis_PUBXe_NMEA04, sizeof(_Dis_PUBXe_NMEA04));         //#15 / PUBX 04 (Time of Day)
      while (12 > _port->availableForWrite()) {}
      // u-blox6 -- Message Not-Acknowledged
      //_port->write(_Dis_PUBXe_NMEA05, sizeof(_Dis_PUBXe_NMEA05));       //    / PUBX 05 (EKF Status)
      //_port->write(_Dis_PUBXe_NMEA06, sizeof(_Dis_PUBXe_NMEA06));       //    / PUBX 06 (GPS-only on EKF products)
    }
    
    /* read the uBlox data */
    bool UBLOX::read(gpsData *gpsData_ptr) {
    
      const double mm2m = 1.0e-3;
      const double en7 = 1.0e-7;
      const double en5 = 1.0e-5;
      const double en2 = 1.0e-2;
    
      union {
        unsigned long val;
        uint8_t b[4];
      } iTOW;
    
      union {
        unsigned short val;
        uint8_t b[2];
      } utcYear;
    
      union {
        unsigned long val;
        uint8_t b[4];
      } tAcc;
    
      union {
        long val;
        uint8_t b[4];
      } utcNano;
    
      union {
        long val; // I4
        uint8_t b[4];
      } lon;
    
      union {
        long val;
        uint8_t b[4];
      } lat;
    
      union {
        long val;
        uint8_t b[4];
      } height;
    
      union {
        long val;
        uint8_t b[4];
      } hMSL;
    
      union {
        unsigned long val;
        uint8_t b[4];
      } hAcc;
    
      union {
        unsigned long val;
        uint8_t b[4];
      } vAcc;
    
      union {
        long val;
        uint8_t b[4];
      } velN;
    
      union {
        long val;
        uint8_t b[4];
      } velE;
    
      union {
        long val;
        uint8_t b[4];
      } velD;
    
      union {
        long val;
        uint8_t b[4];
      } gSpeed;
    
      union {
        long val;
        uint8_t b[4];
      } heading;
    
      union {
        unsigned long val;
        uint8_t b[4];
      } sAcc;
    
      union {
        unsigned long val;
        uint8_t b[4];
      } headingAcc;
    
      union {
        unsigned short val;
        uint8_t b[2];
      } pDOP;
    
      union {
        unsigned long val;
        uint8_t b[4];
      } headVeh;
    
      union {
        signed short val;
        uint8_t b[2];
      } magDec;
    
      union {
        unsigned short val;
        uint8_t b[2];
      } magAcc;
    
      // UBX-NAV-ATT (0x01 0x05) Attitude Solution
      union {
        long val;
        uint8_t b[4];
      } roll;
    
      union {
        long val;
        uint8_t b[4];
      } pitch;
    
      union {
        long val;
        uint8_t b[4];
      } accRoll;
    
      union {
        long val;
        uint8_t b[4];
      } accPitch;
    
      union {
        long val;
        uint8_t b[4];
      } accHeading;
    
      // Current GPS Uart1 Baud
      union {
        uint32_t val;
        uint8_t b[4];
      } GpsUart1Baud;
    
      // parse the uBlox packet
      if (parse()) {
        // uBlox Message (Class, ID) # Description #
        // UBX-NAV-PVT   (0x01 0x07) Navigation Position Velocity Time Solution
        if (_UBX_NAV_PVT_ID == true) {
          iTOW.b[0] = _gpsPayload[4];
          iTOW.b[1] = _gpsPayload[5];
          iTOW.b[2] = _gpsPayload[6];
          iTOW.b[3] = _gpsPayload[7];
          gpsData_ptr->iTOW = iTOW.val;
    
          utcYear.b[0] = _gpsPayload[8];
          utcYear.b[1] = _gpsPayload[9];
          gpsData_ptr->utcYear = utcYear.val;
    
          gpsData_ptr->utcMonth = _gpsPayload[10];
          gpsData_ptr->utcDay = _gpsPayload[11];
          gpsData_ptr->utcHour = _gpsPayload[12];
          gpsData_ptr->utcMin = _gpsPayload[13];
          gpsData_ptr->utcSec = _gpsPayload[14];
          gpsData_ptr->valid = _gpsPayload[15];
    
          tAcc.b[0] = _gpsPayload[16];
          tAcc.b[1] = _gpsPayload[17];
          tAcc.b[2] = _gpsPayload[18];
          tAcc.b[3] = _gpsPayload[19];
          gpsData_ptr->tAcc = tAcc.val;
    
          utcNano.b[0] = _gpsPayload[20];
          utcNano.b[1] = _gpsPayload[21];
          utcNano.b[2] = _gpsPayload[22];
          utcNano.b[3] = _gpsPayload[23];
          gpsData_ptr->utcNano = utcNano.val;
    
          gpsData_ptr->fixType = _gpsPayload[24];
          gpsData_ptr->flags = _gpsPayload[25];
          gpsData_ptr->flags2 = _gpsPayload[26];
          gpsData_ptr->numSV = _gpsPayload[27];
    
          lon.b[0] = _gpsPayload[28];
          lon.b[1] = _gpsPayload[29];
          lon.b[2] = _gpsPayload[30];
          lon.b[3] = _gpsPayload[31];
          gpsData_ptr->lon = lon.val * en7;
    
          lat.b[0] = _gpsPayload[32];
          lat.b[1] = _gpsPayload[33];
          lat.b[2] = _gpsPayload[34];
          lat.b[3] = _gpsPayload[35];
          gpsData_ptr->lat = lat.val * en7;
    
          height.b[0] = _gpsPayload[36];
          height.b[1] = _gpsPayload[37];
          height.b[2] = _gpsPayload[38];
          height.b[3] = _gpsPayload[39];
          gpsData_ptr->height = height.val * mm2m;
    
          hMSL.b[0] = _gpsPayload[40];
          hMSL.b[1] = _gpsPayload[41];
          hMSL.b[2] = _gpsPayload[42];
          hMSL.b[3] = _gpsPayload[43];
          gpsData_ptr->hMSL = hMSL.val * mm2m;
    
          hAcc.b[0] = _gpsPayload[44];
          hAcc.b[1] = _gpsPayload[45];
          hAcc.b[2] = _gpsPayload[46];
          hAcc.b[3] = _gpsPayload[47];
          gpsData_ptr->hAcc = hAcc.val * mm2m;
    
          vAcc.b[0] = _gpsPayload[48];
          vAcc.b[1] = _gpsPayload[49];
          vAcc.b[2] = _gpsPayload[50];
          vAcc.b[3] = _gpsPayload[51];
          gpsData_ptr->vAcc = vAcc.val * mm2m;
    
          velN.b[0] = _gpsPayload[52];
          velN.b[1] = _gpsPayload[53];
          velN.b[2] = _gpsPayload[54];
          velN.b[3] = _gpsPayload[55];
          gpsData_ptr->velN = velN.val * mm2m;
    
          velE.b[0] = _gpsPayload[56];
          velE.b[1] = _gpsPayload[57];
          velE.b[2] = _gpsPayload[58];
          velE.b[3] = _gpsPayload[59];
          gpsData_ptr->velE = velE.val * mm2m;
    
          velD.b[0] = _gpsPayload[60];
          velD.b[1] = _gpsPayload[61];
          velD.b[2] = _gpsPayload[62];
          velD.b[3] = _gpsPayload[63];
          gpsData_ptr->velD = velD.val * mm2m;
    
          gSpeed.b[0] = _gpsPayload[64];
          gSpeed.b[1] = _gpsPayload[65];
          gSpeed.b[2] = _gpsPayload[66];
          gSpeed.b[3] = _gpsPayload[67];
          gpsData_ptr->gSpeed = gSpeed.val * mm2m;
    
          heading.b[0] = _gpsPayload[68];
          heading.b[1] = _gpsPayload[69];
          heading.b[2] = _gpsPayload[70];
          heading.b[3] = _gpsPayload[71];
          gpsData_ptr->heading = heading.val * en5;
    
          sAcc.b[0] = _gpsPayload[72];
          sAcc.b[1] = _gpsPayload[73];
          sAcc.b[2] = _gpsPayload[74];
          sAcc.b[3] = _gpsPayload[75];
          gpsData_ptr->sAcc = sAcc.val  * mm2m;
    
          headingAcc.b[0] = _gpsPayload[76];
          headingAcc.b[1] = _gpsPayload[77];
          headingAcc.b[2] = _gpsPayload[78];
          headingAcc.b[3] = _gpsPayload[79];
          gpsData_ptr->headingAcc = headingAcc.val * en5;
    
          pDOP.b[0] = _gpsPayload[80];
          pDOP.b[1] = _gpsPayload[81];
          gpsData_ptr->pDOP = pDOP.val * 0.01L;
    
          headVeh.b[0] = _gpsPayload[88];
          headVeh.b[1] = _gpsPayload[89];
          headVeh.b[2] = _gpsPayload[90];
          headVeh.b[3] = _gpsPayload[91];
          gpsData_ptr->headVeh = headVeh.val * en5;
    
          // Edit - 2018 2 25 magDec, magAcc
          magDec.b[0] = _gpsPayload[92];
          magDec.b[0] = _gpsPayload[93];
          gpsData_ptr->magDec = magDec.val * en2;
    
          magAcc.b[0] = _gpsPayload[94];
          magAcc.b[0] = _gpsPayload[95];
          gpsData_ptr->magDec = magAcc.val * en2;
          _UBX_NAV_PVT_ID = false;
          // return true on receiving a full packet
          return true;
        }
    
        // UBX-NAV-POSLLH (0x01 0x02) Geodetic Position Solution
        if (_UBX_NAV_POSLLH_ID == true) {
          iTOW.b[0] = _gpsPayload[4]; // GPS time of week of the navigation epoch.
          iTOW.b[1] = _gpsPayload[5];
          iTOW.b[2] = _gpsPayload[6];
          iTOW.b[3] = _gpsPayload[7];
          gpsData_ptr->iTOW = iTOW.val;
    
          lon.b[0] = _gpsPayload[8]; // Longitude
          lon.b[1] = _gpsPayload[9];
          lon.b[2] = _gpsPayload[10];
          lon.b[3] = _gpsPayload[11];
          gpsData_ptr->lon = lon.val * en7;
    
          lat.b[0] = _gpsPayload[12]; // Latitude
          lat.b[1] = _gpsPayload[13];
          lat.b[2] = _gpsPayload[14];
          lat.b[3] = _gpsPayload[15];
          gpsData_ptr->lat = lat.val * en7;
    
          height.b[0] = _gpsPayload[16]; // Height above ellipsoid
          height.b[1] = _gpsPayload[17];
          height.b[2] = _gpsPayload[18];
          height.b[3] = _gpsPayload[19];
          gpsData_ptr->height = height.val * mm2m;
    
          hMSL.b[0] = _gpsPayload[20]; // Height above mean sea level
          hMSL.b[1] = _gpsPayload[21];
          hMSL.b[2] = _gpsPayload[22];
          hMSL.b[3] = _gpsPayload[23];
          gpsData_ptr->hMSL = hMSL.val * mm2m;
    
          hAcc.b[0] = _gpsPayload[24]; // Horizontal accuracy estimate
          hAcc.b[1] = _gpsPayload[25];
          hAcc.b[2] = _gpsPayload[26];
          hAcc.b[3] = _gpsPayload[27];
          gpsData_ptr->hAcc = hAcc.val * mm2m;
    
          _UBX_NAV_POSLLH_ID = false;
          // return true on receiving a full packet
          return true;
        }
    
        // UBX-NAV-ATT (0x01 0x05) Attitude Solution
        if (_UBX_NAV_ATT_ID == true) {
          iTOW.b[0] = _gpsPayload[4]; // GPS time of week of the navigation epoch.
          iTOW.b[1] = _gpsPayload[5];
          iTOW.b[2] = _gpsPayload[6];
          iTOW.b[3] = _gpsPayload[7];
          gpsData_ptr->iTOW = iTOW.val;
    
          gpsData_ptr->version = _gpsPayload[8]; // Message version (0 for this version)
    
          // U1[3] - reserved1 - Reserved
          // gpsData_ptr->reserved1 = _gpsPayload[9];
          // gpsData_ptr->reserved2 = _gpsPayload[10];
          // gpsData_ptr->reserved3 = _gpsPayload[11];
    
          roll.b[0] = _gpsPayload[12]; // Vehicle roll.
          roll.b[1] = _gpsPayload[13];
          roll.b[2] = _gpsPayload[14];
          roll.b[3] = _gpsPayload[15];
          gpsData_ptr->roll = roll.val * en5;
    
          pitch.b[0] = _gpsPayload[16]; // Vehicle pitch.
          pitch.b[1] = _gpsPayload[17];
          pitch.b[2] = _gpsPayload[18];
          pitch.b[3] = _gpsPayload[19];
          gpsData_ptr->pitch = pitch.val * en5;
    
          heading.b[0] = _gpsPayload[20]; // heading
          heading.b[1] = _gpsPayload[21];
          heading.b[2] = _gpsPayload[22];
          heading.b[3] = _gpsPayload[23];
          gpsData_ptr->heading = heading.val * en5;
    
          accRoll.b[0] = _gpsPayload[24]; // Vehicle roll accuracy (if null, roll angle is not available).
          accRoll.b[1] = _gpsPayload[25];
          accRoll.b[2] = _gpsPayload[26];
          accRoll.b[3] = _gpsPayload[27];
          gpsData_ptr->accRoll = accRoll.val * en5;
    
          accPitch.b[0] = _gpsPayload[28]; // Vehicle pitch accuracy (if null, pitch angle is not available).
          accPitch.b[1] = _gpsPayload[29];
          accPitch.b[2] = _gpsPayload[30];
          accPitch.b[3] = _gpsPayload[31];
          gpsData_ptr->accPitch = accPitch.val * en5;
    
          accHeading.b[0] = _gpsPayload[32]; // Vehicle heading accuracy (if null, heading angle is not available).
          accHeading.b[1] = _gpsPayload[33];
          accHeading.b[2] = _gpsPayload[34];
          accHeading.b[3] = _gpsPayload[35];
          gpsData_ptr->accHeading = accHeading.val * en5;
          _UBX_NAV_ATT_ID = false;
          // return true on receiving a full packet
          return true;
        }
    
        // UBX-CFG-PRT 0x06 0x00 Poll Request Polls the configuration for one I/O Port , len[1]
        // UBX-CFG-PRT 0x06 0x00 Get/Set Port Configuration for UART-USB-SPI-DDC(IC) Port, len[20]
        if (_UBX_CFG_PRT_ID == true) {
          // Decide what to do based on payload size, length in decimal format
          uint8_t MSGpayload = (_gpsPayload[3] << 8) | _gpsPayload[2]; // Combining two uint8_t as uint16_t
          switch (MSGpayload) { // 16Bit-Length Field
            case 1:   // if MSG payload size == 0x01 Poll Request Polls the configuration for one I/O Port , len[1] DEC
              Serial.print("Poll Request Polls the configuration for one I/O Port:"), Serial.println(_gpsPayload[4]), Serial.println(); // USB debug print
              break;
            case 20:  // if MSG payload size == (HEX0x14~DEC20) Get/Set Port Configuration for UART-USB-SPI-DDC(IC) Port, len[20]
              // Decide what to do based on _gpsPayload:[4] Payload Content-I/O portID: 0=DDC(I2C), 1=UART1, 2=UART2, 3=USB, 4=SPI, 5=reserved
              if (_gpsPayload[4] == 1) { // 1=UART1
                //  *** Example UART1 460800‬ baud ***
                // Message Structure
                //    Header,Clas,  Id, DataSize 16b,     MSG, Checksum
                // 0xB5 0x62,0x06 0x00,     20 Bytes, Payload, CK_A CK_B
                // Payload Contents:
                //       Byte Offset___Number Format______Scaling__________Name______Unit_______________________Description
                //gpsPayload:[4]  0---- U1--uint8_t-------------------- portID --------- I/O port Target Number assignment
                //gpsPayload:[5]  1---- U1--uint8_t----------------- reserved0 ---------------------------------- Reserved
                //      --------- 2---- X2-uint16_t------------------- txReady -------------------------- Bitfield txReady
                //      --------- 4---- X4-uint32_t---------------------- mode ----------------------------- Bitfield mode
                //gpsPayload:[12] 8---- U4-uint32_t------------------ baudRate ---Bits/s---------- Baudrate in bits/second
                //gpsPayload:[13] 9
                //gpsPayload:[14]10
                //gpsPayload:[15]11
                //               12---- X2-uint16_t--------------- inProtoMask --------------- Mask input protocols active
                //               14---- X2-uint16_t-------------- outProtoMask -------------- Mask output protocols active
                //      -------- 16---- U2-uint16_t----------------- reserved4 ------------------------ Always set to zero
                //      ---------18---- U2-uint16_t----------------- reserved5 ------------------------ Always set to zero
                //                 Header|CL|Id|DSize| MSG Payload                                                 |Checksum|
                // _gpsPayload:[n]       | 0| 1| 2  3| 4  5  6  7  8  9 10 11  12 13 14 15  16 17 18 18 19 20 21 22|23 24   |
                //23:46:54   0000   B5 62|06 00|14 00|01 00 00 00 D0 08 00 00 >00 08 07 00< 07 00 03 00 00 00 00 00|0C BC   |  b.........................
                // >baudRate uint32_t<
                // Little Endian: Hex: 0x00080700
                // Big Endian:    Hex: 0x00070800 Decimal = **460800‬baud
                GpsUart1Baud.b[0] = _gpsPayload[12];
                GpsUart1Baud.b[1] = _gpsPayload[13];
                GpsUart1Baud.b[2] = _gpsPayload[14];
                GpsUart1Baud.b[3] = _gpsPayload[15];
                gpsData_ptr->GpsUart1Baud = GpsUart1Baud.val;
                // need this for ACK_ACK print
                _GpsUartBaud = (((_gpsPayload[15] << 24) | (_gpsPayload[14] << 16)) | (_gpsPayload[13] << 8)) | _gpsPayload[12]; // Combining
              } else {
                Serial.print("I/O portID: 0=DDC(I2C), 2=UART2, 3=USB, 4=SPI, 5=reserved "), Serial.print(_gpsPayload[4]), Serial.println(" Not implemented"), Serial.println(); // USB debug print
              }
              break;
            default:
              // Not Supported
              Serial.print("Unknown CFG PRT Length Field:"), Serial.println((_MSGpayloadSize) - 4, DEC), Serial.println();
              break;
          }
          _UBX_CFG_PRT_ID = false;
          // return false even though its packets are successfully being received, not a NAV packet
          return false;
        }
    
        if (_UBX_ACK_ACK_ID == true) {
          uint8_t CC = 0; // use only one if statement
          switch (_gpsPayload[4]) { // UBX_ACK_ACK(0x05 0x01) response to Class CFG 0x06
            case 0x06:   // CFG 0x06 Configuration Input Messages: Set Dynamic Model, Set DOP Mask, Set Baud Rate, etc
              switch (_gpsPayload[5]) {    // ### Name ID Description ###  UBX_ACK_ACK response to ID
                case 0x00: // CFG-PRT 0x06 0x00:
                  if ((_pACK2 == 1) || (_printACK == 0x06001)) {
                    Serial.print("UBX_ACK_ACK:"), Serial.print(" Message Acknowledged -");
                    if ((_printACK == 0x06001) && CC == 0) {
                      CC = 1;
                      Serial.print(" Poll_GPSbaud_Port1 baudRate:"), Serial.println(_GpsUartBaud);
                      _printACK = 0x00;
                    }
                    if ((_pACK2 == 1) && CC == 0) {
                      CC = 1;
                      Serial.println(" CFG-PRT (0x06 0x00) Set GPS UART baud");
                      _pACK2 = 0;
                    }
                  } else {
                    if ((_pACK2 == 0) || (_printACK == 0x00)) {
                      // Do Nothing Here
                    } else { // print all Unknown CFG ACK on USB
                      Serial.print("UBX_ACK_ACK: CFG-PRT 0x06 0x00"), Serial.print(" Message Acknowledged "), Serial.println();
                    }
                  }
                  break;
    
                case 0x01:   // CFG-MSG 0x06 0x01: Set Message Rate(s)
                  if ((_printACK_Dis_all_NMEA == 0x06010) && CC == 0) { // Dis_all_NMEA_Child_MSGs 0x06010
                    CC = 1;
                    //Serial.print("UBX_ACK_ACK:"), Serial.print(" Message Acknowledged -");
                    Serial.print("UBX_ACK_ACK:"), Serial.print(" Message Acknowledged - ");
                    Serial.print("Dis_all_NMEA ");
                    Serial.print("-CFG-MSG(0x06 0x01): Set disable All NMEA "), Serial.println(_ACKcount);
                    _ACKcount = _ACKcount - 1;
                    if (_ACKcount == 0) { // NMEA ACK counter
                      _printACK_Dis_all_NMEA = 0x00; // reset
                      //Serial.println();
                    }
                  }
                  if ((_printACK_ATT == 0x06011 || _printACK_ATT == 0x06010) && CC == 0) {
                    CC = 1;
                    Serial.print("UBX_ACK_ACK:"), Serial.print(" Message Acknowledged - ");
                    if (_printACK_ATT == 0x06011) { // Ena_NAV_ATT 0x06011
                      Serial.print("Ena_NAV_ATT "), Serial.println("-Enable periodic auto update");
                      _printACK_ATT = 0x01; // 01
                    }
                    if (_printACK_ATT == 0x06010) { // Dis_NAV_ATT 0x06010
                      Serial.print("Dis_NAV_ATT "), Serial.println("-Disable periodic auto update");
                      _printACK_ATT = 0x02; // 01
                    }
                  }
                  if ((_printACK_PVT == 0x06011 || _printACK_PVT == 0x06010) && CC == 0) {
                    CC = 1;
                    Serial.print("UBX_ACK_ACK:"), Serial.print(" Message Acknowledged - ");
                    if (_printACK_PVT == 0x06011) { // Ena_NAV_PVT 0x06011
                      Serial.print("Ena_NAV_PVT "), Serial.println("-Enable periodic auto update");
                      _printACK_PVT = 0x00;
                    }
                    if (_printACK_PVT == 0x06010) { // Dis_NAV_PVT 0x06010
                      Serial.print("Dis_NAV_PVT "), Serial.println("-Disable periodic auto update");
                      _printACK_PVT = 0x00;
                    }
                  }
                  if ((_printACK_POSLLH == 0x06011 || _printACK_POSLLH == 0x06010) && CC == 0) {
                    CC = 1;
                    Serial.print("UBX_ACK_ACK:"), Serial.print(" Message Acknowledged - ");
                    if (_printACK_POSLLH == 0x06011) { // Ena_NAV_POSLLH 0x06011
                      Serial.print("Ena_NAV_POSLLH "), Serial.println("-Enable periodic auto update");
                      _printACK_POSLLH = 0x00;
                    }
                    if (_printACK_POSLLH == 0x06010) { // Dis_NAV_POSLLH 0x06010
                      Serial.print("Dis_NAV_POSLLH "), Serial.println("-Disable periodic auto update");
                      _printACK_POSLLH = 0x00;
                    }
                  }
                  break;
    
                case 0x02:    // CFG-INF 0x06 0x02:       Poll configuration for one protocol or Set Information message configuration
                  Serial.println("-CFG-INF 0x06 0x02"), Serial.println();
                  break;
                case 0x06:    // CFG-DAT 0x06 0x06:       Set User-defined Datum or Get The currently defined Datum
                  Serial.println("-CFG-DAT 0x06 0x06"), Serial.println();
                  break;
                case 0x09:    // CFG-CFG 0x06 0x09:       Command Clear, Save and Load configurations
                  Serial.println("-CFG-CFG 0x06 0x09"), Serial.println();
                  break;
                case 0x08:    // CFG-RATE 0x06 0x08:      Get/Set Navigation/Measurement Rate Settings
                  //_pACK_RATE =
                  if (_pACK_RATE == true) {
                    Serial.print("UBX_ACK_ACK:"), Serial.print(" Message Acknowledged ");
                    Serial.println("- CFG-RATE 0x06 0x08 Get/Set Navigation/Measurement Rate Settings");
                  }
                  break;
                case 0x11:    // CFG-RXM 0x06 0x11:       Get/Set RXM configuration
                  Serial.println("-CFG-RXM 0x06 0x11"), Serial.println();
                  break;
                case 0x13:    // CFG-ANT 0x06 0x13:       Get/Set Antenna Control Settings
                  Serial.println("-CFG-ANT 0x06 0x13"), Serial.println();
                  break;
                case 0x16:    // CFG-SBAS 0x06 0x16:      Get/Set SBAS Configuration
                  Serial.println("-CFG-SBAS 0x06 0x16"), Serial.println();
                  break;
                case 0x17:    // CFG-NMEA 0x06 0x17:      Get/Set NMEA protocol configuration, V0 (deprecated) or Extended NMEA protocol configuration V1
                  Serial.println("-CFG-NMEA 0x06 0x17"), Serial.println();
                  break;
                case 0x1B:    // CFG-USB 0x06 0x1B:       Get/Set USB Configuration
                  Serial.println("-CFG-USB 0x06 0x1B"), Serial.println();
                  break;
                case 0x1E:    // CFG-ODO 0x06 0x1E:       Get/Set Odometer, Low-speed COG Engine Settings
                  Serial.println("-CFG-ODO 0x06 0x1E"), Serial.println();
                  break;
                case 0x23:    // CFG-NAVX5 0x06 0x23:     Get/Set Navigation Engine Expert Settings
                  Serial.println("-CFG-NAVX5 0x06 0x23"), Serial.println();
                  break;
                case 0x24:    // CFG-NAV5 0x06 0x24:      Get/Set Navigation Engine Settings
                  if (_pACK_NAV5 == true) {
                    Serial.print("UBX_ACK_ACK:"), Serial.print(" Message Acknowledged ");
                    Serial.println("- CFG-NAV5 0x06 0x24 Get/Set Navigation Engine Settings");
                  }
                  break;
                case 0x31:    // CFG-TP5 0x06 0x31:       Poll Time Pulse Parameters for Time Pulse 0, Poll Time Pulse Parameters or Get/Set Time Pulse Parameters
                  Serial.println("-CFG-TP5 0x06 0x31"), Serial.println();
                  break;
                case 0x34:    // CFG-RINV 0x06 0x34       Get/Set Contents of Remote Inventory
                  Serial.println("-CFG-RINV 0x06 0x34"), Serial.println();
                  break;
                case 0x39:    // CFG-ITFM 0x06 0x39:      Get/Set Jamming/Interference Monitor configuration
                  Serial.println("-CFG-ITFM 0x06 0x39"), Serial.println();
                  break;
                case 0x3B:    // CFG-PM2 0x06 0x3B:       Get/Set Extended Power Management configuration
                  Serial.println("-CFG-PM2 0x06 0x3B"), Serial.println();
                  break;
                case 0x3D:    // CFG-TMODE2 0x06 0x3D:    Get/Set Time Mode Settings 2
                  Serial.println("-CFG-TMODE2 0x06 0x3D"), Serial.println();
                  break;
                case 0x3E:    // CFG-GNSS 0x06 0x3E:      Get/Set GNSS system configuration
                  Serial.println("-CFG-GNSS 0x06 0x3E"), Serial.println();
                  break;
                case 0x47:    // CFG-LOGFILTER 0x06 0x47: Get/Set Data Logger Configuration
                  Serial.println("-CFG-LOGFILTER 0x06 0x47"), Serial.println();
                  break;
                case 0x53:    // CFG-TXSLOT 0x06 0x53:    Set TX buffer time slots configuration
                  Serial.println("-CFG-TXSLOT 0x06 0x53"), Serial.println();
                  break;
                case 0x57:    // CFG-PWR 0x06 0x57:       Set Put receiver in a defined power state.
                  Serial.println("-CFG-PWR 0x06 0x57"), Serial.println();
                  break;
                case 0x5C:    // CFG-HNR 0x06 0x5C:       Get/Set High Navigation Rate Settings ### u-blox 8 only ###
                  Serial.println("-CFG-HNR 0x06 0x5C"), Serial.println();
                  break;
                case 0x60:    // CFG-ESRC 0x06 0x60:      Get/Set External synchronization source configuration
                  Serial.println("-CFG-ESRC 0x06 0x60"), Serial.println();
                  break;
                case 0x61:    // CFG-DOSC 0x06 0x61:      Get/Set Disciplined oscillator configuration
                  Serial.println("-CFG-DOSC 0x06 0x61"), Serial.println();
                  break;
                case 0x62:    // CFG-SMGR 0x06 0x62:      Get/Set Synchronization manager configuration
                  Serial.println("-CFG-SMGR 0x06 0x62"), Serial.println();
                  break;
                case 0x69:    // CFG-GEOFENCE 0x06 0x69:  Get/Set Geofencing configuration
                  Serial.println("-CFG-GEOFENCE 0x06 0x69"), Serial.println();
                  break;
                case 0x70:    // CFG-DGNSS 0x06 0x70:     Get/Set DGNSS configuration
                  Serial.println("-CFG-DGNSS 0x06 0x70"), Serial.println();
                  break;
                case 0x71:    // CFG-TMODE3 0x06 0x71:    Get/Set Time Mode Settings 3
                  Serial.println("-CFG-TMODE3 0x06 0x71"), Serial.println();
                  break;
                case 0x84:    // CFG-FIXSEED 0x06 0x84:   Set Programming the fixed seed for host...
                  Serial.println("-CFG-FIXSEED 0x06 0x84"), Serial.println();
                  break;
                case 0x85:    // CFG-DYNSEED 0x06 0x85:   Set Programming the dynamic seed for the host...
                  Serial.println("-CFG-DYNSEED 0x06 0x85"), Serial.println();
                  break;
                case 0x86:    // CFG-PMS 0x06 0x86:       Get/Set Power Mode Setup
                  Serial.println("-CFG-PMS 0x06 0x86"), Serial.println();
                  break;
                default:
                  // Not Supported
                  Serial.print("Unknown ACK CFG response to ID:");
                  if (_gpsPayload[5] > 16) {
                    Serial.print("0x");
                  } else {
                    Serial.print("0x0");
                  }
                  Serial.println(_gpsPayload[5], HEX), Serial.println();
                  break;
              }
              break;
            default:
              // Not Supported
              Serial.print("Unknown ACK ACK response to Class:"), Serial.println((_gpsPayload[4]), HEX), Serial.println();
              break;
          }
          _UBX_ACK_ACK_ID = false;
          // return false even though its packets are successfully being received, not a NAV packet
          return false;
        }
        // return true on receiving a full packet
        // return true;
        return true;
      } else {
        // return false if a full packet is not received
        return false;
      }
    }
    
    /* parse the uBlox data */
    bool UBLOX::parse() {
      // uBlox UBX header definition
      uint8_t const UBX_HEADER[] = { 0xB5, 0x62 };
    
      // checksum calculation
      static unsigned char checksum[2];
    
      // read a byte from the serial port
      while ( _port->available() ) {
        uint8_t c = _port->read();
    
        // identify the packet header
        if ( _fpos < 2 ) {
          if ( c == UBX_HEADER[_fpos] ) {
            _fpos++;
          } else {
            _fpos = 0;
            //if (c == 36) { // ASCII Table Dec 36~Char $
            //  Serial.write(c); // debug print NMEA message start $ Char
            //  Serial.println(" NMEA message start $ Char"); // USB debug print
            //}
          }
        }
        // Identify the packet Class
        else if (_fpos == 2) {
          switch (c) {    // ### Name Class Description ###
            case 0x01:    // NAV 0x01 Navigation Results: Position, Speed, Time, Acc, Heading, DOP, SVs used
              _CurrentClass = c; // Save for Message ID.
              ((unsigned char*)_gpsPayload)[_fpos - 2] = c; // grab the payload, need for checksum
              _fpos++;
              break;
            case 0x02:    // RXM 0x02 Receiver Manager Messages: Satellite Status, RTC Status
              _CurrentClass = c;
              Serial.print("UBX_RXM_CLASS:0x0"), Serial.print(c, HEX), Serial.println(" Not implemented"), Serial.println();
              _fpos = 0;
              break;
            case 0x04:    // INF 0x04 Information Messages: Printf-Style Messages, with IDs such as Error, Warning, Notice
              _CurrentClass = c;
              Serial.print("UBX_INF_CLASS:0x0"), Serial.print(c, HEX), Serial.println(" Not implemented"), Serial.println();
              _fpos = 0;
              break;
            case 0x05:    // ACK 0x05 Ack/Nack Messages: as replies to CFG Input Messages
              _CurrentClass = c;
              ((unsigned char*)_gpsPayload)[_fpos - 2] = c; // grab the payload, need for checksum
              _fpos++;
              break;
            case 0x06:    // CFG 0x06 Configuration Input Messages: Set Dynamic Model, Set DOP Mask, Set Baud Rate, etc.
              _CurrentClass = c; // Save for Message ID.
              ((unsigned char*)_gpsPayload)[_fpos - 2] = c; // grab the payload, need for checksum
              _fpos++;
              break;
            case 0x0A:    // MON 0x0A Monitoring Messages: Comunication Status, CPU Load, Stack Usage, Task Status
              Serial.print("UBX_MON_CLASS:0x0"), Serial.print(c, HEX), Serial.println(" Not implemented"), Serial.println();
              _CurrentClass = c;
              _fpos = 0;
              break;
            case 0x0B:    // AID 0x0B AssistNow Aiding Messages: Ephemeris, Almanac, other A-GPS data input
              Serial.print("UBX_AID_CLASS:0x0"), Serial.print(c, HEX), Serial.println(" Not implemented"), Serial.println();
              _CurrentClass = c;
              _fpos = 0;
              break;
            case 0x0D:    // TIM 0x0D Timing Messages: Time Pulse Output, Timemark Results
              Serial.print("UBX_TIM_CLASS:0x0"), Serial.print(c, HEX), Serial.println(" Not implemented"), Serial.println();
              _CurrentClass = c;
              _fpos = 0;
              break;
            // Not Supported
            // ESF 0x10 External Sensor Fusion Messages: External Sensor Measurements and Status Information
            // MGA 0x13 Multiple GNSS Assistance Messages: Assistance data for various GNSS
            case 0x21:    // LOG 0x21 Logging Messages: Log creation, deletion, info and retrieval
              Serial.print("UBX_LOG_CLASS:0x"), Serial.print(c, HEX), Serial.println(" Not implemented"), Serial.println();
              _CurrentClass = c;
              _fpos = 0;
              break;
            // Not Supported
            // SEC 0x27 Security Feature Messages
            // HNR 0x28 High Rate Navigation Results Messages: High rate time, position, speed, heading
            default:
              // Not Supported
              _CurrentClass = c;
              _fpos = 0;
              Serial.print("Unknown packet Class:");
              if (c > 16) {
                Serial.print("0x");
              } else {
                Serial.print("0x0");
              }
              Serial.println(c, HEX), Serial.println(); // USB
              break;
          }
        }
        // Identify the packet Message ID
        else if (_fpos == 3 ) {
          switch (_CurrentClass) {
            case 0x01:    // NAV 0x01
              NAV_IDs(c); // Go grab the uBlox NAV - ID
              break;
            case 0x02:    // RXM 0x02
              _fpos = 0;
              break;
            case 0x04:    // INF 0x04
              _fpos = 0;
              break;
            case 0x05:    // ACK 0x05
              ACK_IDs(c); // Go grab the uBlox ACK - ID
              break;
            case 0x06:    // CFG 0x06
              CFG_IDs(c); // Go grab the uBlox CFG - ID
              break;
            case 0x0A:    // MON 0x0A
              _fpos = 0;
              break;
            case 0x0B:    // AID 0x0B
              _fpos = 0;
              break;
            case 0x0D:    // TIM 0x0D
              _fpos = 0;
              break;
            case 0x21:    // LOG 0x21
              _fpos = 0;
              break;
            default:
              // 0x03 and 0x0E to 0x20 Not Supported
              _fpos = 0;
              Serial.println("Unknown packet ID:"), Serial.println(c, HEX), Serial.println();
              break;
          }
          _CurrentClass = 0x00;
        }
        // Identify the packet 16Bit Message payloadSize
        // It does not include 16Bit-Sync header, 8Bit-Class, 8Bit-ID, 16Bit-Length Field, and 16Bit-CRC fields
        else if (_fpos == 4 ) {
          //Serial.write(c); // USB u-center
          ((unsigned char*)_gpsPayload)[_fpos - 2] = c; // least significant byte (LSB)
          _fpos++;
        }
        else if (_fpos == 5 ) {
          //Serial.write(c); // USB u-center
          ((unsigned char*)_gpsPayload)[_fpos - 2] = c; // most significant byte (MSB)
          _MSGpayloadSize = (_gpsPayload[3] << 8) | _gpsPayload[2]; // Combining two uint8_t as uint16_t
          _MSGpayloadSize = _MSGpayloadSize + 4; // Fix for (CRC), Add 8Bit-Class, 8Bit-ID, 16Bit-Length Field
          _fpos++;
        }
        else {
          // grab the payload
          if ( (_fpos - 2) < _MSGpayloadSize )
            ((unsigned char*)_gpsPayload)[_fpos - 2] = c;
          _fpos++;
    
          // The checksum is calculated over the packet, starting and including the
          // CLASS field, ID and payload, up until, but excluding, the Checksum Field
          // (CRC) Compute checksum
          if ( (_fpos - 2) == _MSGpayloadSize ) {
            calcChecksum(checksum, _gpsPayload, _MSGpayloadSize);
          }
          else if ( (_fpos - 2) == (_MSGpayloadSize + 1) ) {
            if ( c != checksum[0] )
              _fpos = 0;
          }
          else if ( (_fpos - 2) == (_MSGpayloadSize + 2) ) {
            _fpos = 0;
            if ( c == checksum[1] ) {
              return true;
            }
          }
          else if ( _fpos > (_MSGpayloadSize + 4) ) {
            _fpos = 0;
          }
        }
      }
      return false;
    }
    
    /* uBlox checksum */
    void UBLOX::calcChecksum(unsigned char* CK, unsigned char* payload, uint8_t length) {
      CK[0] = 0;
      CK[1] = 0;
      for (uint8_t i = 0; i < length; i++) {
        CK[0] += payload[i]; // CK[0] = CK[0] + payload[i];
        CK[1] += CK[0];
      }
    }
    
    /* uBlox CFG - IDs */
    void UBLOX::CFG_IDs(uint8_t SERc) {
      switch (SERc) {       // ### Name CFG-Class & IDs Description, Msg Length ###
        case 0x00:          // UBX-CFG-PRT 0x06 0x00 Poll Request Polls the configuration for one I/O Port , len[1]
          //                   CFG-PRT 0x06 0x00 Get/Set Port Configuration for UART-USB-SPI-DDC(IC) Port, len[20]
          _UBX_CFG_PRT_ID = true; // next - decide what to do based on MSG payload size
          ((unsigned char*)_gpsPayload)[_fpos - 2] = SERc; // grab the payload, need for checksum
          _fpos++;
          break;
        case 0x01:          // UBX-CFG-MSG 0x06 0x01 Poll Request Poll a message configuration, len[2]
          //                   UBX-CFG-MSG 0x06 0x01 Get/Set Set Message Rate(s), len[8]
          //                   UBX-CFG-MSG 0x06 0x01 Get/Set Set Message Rat, len[3]
          Serial.print("UBX_CFG_MSG ID:"), Serial.print(SERc, HEX), Serial.println(" Not implemented"), Serial.println();
          _fpos = 0;
          break;
        default:
          // Not Supported
          _fpos = 0;
          Serial.println(" Not implemented or Unknown packet Class CFG ID:"), Serial.println(SERc, HEX), Serial.println();
          break;
      }
    }
    
    /* uBlox ACK - IDs */
    // When messages from the class CFG are sent to the receiver, the receiver will send an "acknowledge" (ACK-ACK) or a "not acknowledge" (ACK-NAK) message back to the sender.
    void UBLOX::ACK_IDs(uint8_t SERa) {
      uint8_t CN = 0;
      switch (SERa) {       // ### Name ACK-Class & IDs Description, Msg Length ###
        case 0x00:          //  UBX-ACK-NAK (0x05 0x00) Message Not-Acknowledged, len[2]
          //Serial.println(" NAK- "); // todo remove
          if ((_printACK == 0x06001 || _printACK_ATT == 0x01 || _printACK_ATT == 0x02) && CN == 0) {
            Serial.print("UBX_ACK_NAK:"), Serial.print(" Message Not-Acknowledged ");
            if (_printACK == 0x06001) {
              CN = 1;
              Serial.println("- CFG_PRT baudRate"), Serial.println();
              _printACK = 0x00; // reset
            }
            if ((_printACK_ATT == 0x01) && CN == 0) { // Ena_NAV_ATT 0x06011
              CN = 1;
              Serial.println("- Ena_NAV_ATT, ATT Supported only on protocol v. 19 (only with ADR or UDR products)");
              _printACK_ATT = 0x00; // reset
            }
            if ((_printACK_ATT == 0x02) && CN == 0) { // Dis_NAV_ATT 0x06010
              CN = 1;
              Serial.println("- Dis_NAV_ATT, ATT Supported only on protocol v. 19 (only with ADR or UDR products)");
              _printACK_ATT = 0x00; // reset
            }
          } else {
            if ((_printACK_ATT == 0xFF) && CN == 0) {
              CN = 1;
              _printACK_ATT = 0x00;
            } else {
              Serial.print("UBX_ACK_NAK:"), Serial.println(" Message Not-Acknowledged "); //, Serial.println(SERa);
            }
          }
          // _UBX_ACK_NAK_ID = true; // Not implemented
          ((unsigned char*)_gpsPayload)[_fpos - 2] = SERa;
          _fpos++;
          break;
        case 0x01:          //  UBX-ACK-ACK (0x05 0x01) Message Acknowledged, len[2]
          _UBX_ACK_ACK_ID = true;
          ((unsigned char*)_gpsPayload)[_fpos - 2] = SERa;
          _fpos++;
          break;
        default:
          // Not Supported
          _fpos = 0;
          Serial.println("Unknown packet Class ACK ID:"), Serial.println(SERa, HEX), Serial.println();
          break;
      }
    }
    
    /* uBlox NAV - IDs */
    void UBLOX::NAV_IDs(uint8_t SERn) {
    
      switch (SERn) {    // ### Name NAV-Class & IDs Description, Msg Length ###
        case 0x01:          // NAV-POSECEF 0x01 0x01 Position Solution in ECEF, U-Blox7 = len[20]
          Serial.print("UBX_NAV_POSECEF ID:"), Serial.print(SERn, HEX), Serial.println(" Not implemented"), Serial.println();
          _fpos = 0;
          break;
        case 0x02:          // NAV-POSLLH 0x01 0x02 Geodetic Position Solution, U-Blox7 = len[28]
          _UBX_NAV_POSLLH_ID = true;
          ((unsigned char*)_gpsPayload)[_fpos - 2] = SERn; // grab the payload, need for checksum
          _fpos++;
          break;
        case 0x03:          // NAV-STATUS 0x01 0x03 Receiver Navigation Status, U-Blox7 = len[16]
          Serial.print("UBX_NAV_STATUS ID:"), Serial.print(SERn, HEX), Serial.println(" Not implemented"), Serial.println();
          _fpos = 0;
          break;
        case 0x04:         // NAV-DOP 0x01 0x04 Dilution of precision, U-Blox7 = len[18]
          Serial.print("UBX_NAV_DOP ID:"), Serial.print(SERn, HEX), Serial.println(" Not implemented"), Serial.println();
          _fpos = 0;
          break;
        //                    NAV-ATT (0x01 0x05) Attitude Solution, U-Blox8 = len[32]
        case 0x05:         // #### NOTE: u-blox 8 only from protocol version 19 up to version 23.01 (only with ADR or UDR products) ###
          _UBX_NAV_ATT_ID = true;
          ((unsigned char*)_gpsPayload)[_fpos - 2] = SERn; // grab the payload, need for checksum
          _fpos++;
          break;
        case 0x06:         // NAV-SOL 0x01 0x06 Navigation Solution Information, U-Blox7 = len[52]
          Serial.print("UBX_NAV_SOL ID:"), Serial.print(SERn, HEX), Serial.println(" Not implemented"), Serial.println();
          _fpos = 0;
          break;
        case 0x07:        // NAV-PVT 0x01 0x07 Navigation Position Velocity Time Solution, U-Blox7=len[84], U-Blox8=len[92]
          _UBX_NAV_PVT_ID = true;
          ((unsigned char*)_gpsPayload)[_fpos - 2] = SERn; // grab the payload, need for checksum
          _fpos++;
          break;
        case 0x11:        // NAV-VELECEF 0x01 0x11 Velocity Solution in ECEF, U-Blox7 = len[20]
          Serial.print("UBX_NAV_VELECEF ID:"), Serial.print(SERn, HEX), Serial.println(" Not implemented"), Serial.println();
          _fpos = 0;
          break;
        case 0x12:        // NAV-VELNED 0x01 0x12 Velocity Solution in NED, U-Blox7 = len[36]
          Serial.print("UBX_NAV_VELNED ID:"), Serial.print(SERn, HEX), Serial.println(" Not implemented"), Serial.println();
          _fpos = 0;
          break;
        case 0x20:        // NAV-TIMEGPS 0x01 0x20 16 Periodic/Polled GPS Time Solution, U-Blox7 = len[16]
          Serial.print("UBX_NAV_TIMEGPS ID:"), Serial.print(SERn, HEX), Serial.println(" Not implemented"), Serial.println();
          _fpos = 0;
          break;
        case 0x21:        // NAV-TIMEUTC 0x01 0x21 UTC Time Solution, U-Blox7 = len[20]
          Serial.print("UBX_NAV_TIMEUTC ID:"), Serial.print(SERn, HEX), Serial.println(" Not implemented"), Serial.println();
          _fpos = 0;
          break;
        case 0x22:       // NAV-CLOCK 0x01 0x22 Clock Solution, U-Blox7 = len[20]
          Serial.print("UBX_NAV_CLOCK ID:"), Serial.print(SERn, HEX), Serial.println(" Not implemented"), Serial.println();
          _fpos = 0;
          break;
        case 0x30:       // NAV-SVINFO U-Blox7 = len[8 + 12*numCh]
          Serial.print("UBX_NAV_SVINFO ID:"), Serial.print(SERn, HEX), Serial.println(" Not implemented"), Serial.println();
          _fpos = 0;
          break;
        case 0x31:       // NAV-DGPS 0x01 0x31 DGPS Data Used for NAV, U-Blox7 = len[16 + 12*numCh]
          Serial.print("UBX_NAV_DGPS ID:"), Serial.print(SERn, HEX), Serial.println(" Not implemented"), Serial.println();
          _fpos = 0;
          break;
        case 0x32:       // NAV-SBAS 0x01 0x32 12 + 12*cnt Periodic/Polled SBAS Status Data
          Serial.print("UBX_NAV_SBAS ID:"), Serial.print(SERn, HEX), Serial.println(" Not implemented"), Serial.println();
          _fpos = 0;
          break;
        case 0x60:       // NAV-AOPSTATUS 0x01 0x60 AssistNow Autonomous Status, U-Blox7 = len[20]
          Serial.print("UBX_NAV_AOPSTATUS ID:"), Serial.print(SERn, HEX), Serial.println(" Not implemented"), Serial.println();
          _fpos = 0;
          break;
        default:
          // Not Supported
          _fpos = 0;
          Serial.println("Unknown packet Class NAV ID:"), Serial.println(SERn, HEX), Serial.println();
          break;
      }
    }
    
    void UBLOX::DEBUG() {
      // DEBUG PRINT
      Serial.println("HEADER [0]=HEX B5 DEC *181‬");
      Serial.println("HEADER [1]=HEX 62 DEC ** 98‬‬");
    
      uint8_t MSGpayload = (_gpsPayload[3] << 8) | _gpsPayload[2]; // Combining two uint8_t as uint16_t
      for (int i = 0; i < (MSGpayload) + 4; i++) {
        Serial.print("_gpsPayload["), Serial.print(i, DEC), Serial.print("]=");
        if (_gpsPayload[i] < 16) {
          Serial.print("HEX 0");
          Serial.print(_gpsPayload[i], HEX);
        } else {
          Serial.print("HEX ");
          Serial.print(_gpsPayload[i], HEX);
        }
        if (_gpsPayload[i] < 10) {
          Serial.print(" DEC 0");
          Serial.println(_gpsPayload[i], DEC);
        } else {
          Serial.print(" DEC ");
          Serial.println(_gpsPayload[i], DEC);
        }
      }
    }
    #endif
    v1.0.2
    UBLOX.h
    Code:
    /*
      UBLOX.h
      Brian R Taylor
      brian.taylor@bolderflight.com
      2016-11-03
    
      Copyright (c) 2016 Bolder Flight Systems
    
      Permission is hereby granted, free of charge, to any person obtaining a copy of this software
      and associated documentation files (the "Software"), to deal in the Software without restriction,
      including without limitation the rights to use, copy, modify, merge, publish, distribute,
      sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
      furnished to do so, subject to the following conditions:
    
      The above copyright notice and this permission notice shall be included in all copies or
      substantial portions of the Software.
    
      THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
      BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
      NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
      DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
      OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
    
      v1.0.2
      Chris O.
      2018-03-01 Changes:
      Identify the packet Class in receive message
      Identify the packet Message ID in receive message
      Identify the packet 16Bit Message payloadSize
      Fixed the checksum calculations, now it's based on*the*received message payload.
      This fixes the compatibility issues between NEO-M8 and NEO-M7-6 GPS series.
    
      2018-03-01 Addition:
      UBX-NAV-POSLLH - (0x01 0x02) Geodetic Position Solution
      2018-03-20
      UBX-NAV-ATT ---- (0x01 0x05) Attitude Solution
    
      /---  High level commands, for the user ---/
       NOTE: command(bool) == (true)Print Message Acknowledged on USB Serial Monitor
      end();                            // Disables Teensy serial communication, to re-enable, call begin(Baud)
      Poll_CFG_Port1(bool);             // Polls the configuration for one I/O Port, I/O Target 0x01=UART1
      Poll_NAV_PVT();                   // Polls UBX-NAV-PVT    (0x01 0x07) Navigation Position Velocity Time Solution
      Poll_NAV_POSLLH();                // Polls UBX-NAV-POSLLH (0x01 0x02) Geodetic Position Solution
      Poll_NAV_ATT();                   // Polls UBX-NAV-ATT    (0x01 0x05) Attitude Solution
      ### Periodic Auto Update ON,OFF Command ###
      Ena_NAV_PVT(bool);                // Enable periodic auto update NAV_PVT
      Dis_NAV_PVT(bool);                // Disable periodic auto update NAV_PVT
      Ena_NAV_ATT(bool);                // Enable periodic auto update NAV_ATT
      Dis_NAV_ATT(bool);                // Disable periodic auto update NAV_ATT
      Ena_NAV_POSLLH(bool);             // Enable periodic auto update NAV_POSLLH
      Dis_NAV_POSLLH(bool);             // Disable periodic auto update NAV_POSLLH
      ### u-blox Switch off all NMEA MSGs ###
      Dis_all_NMEA_Child_MSGs(bool);    // Disable All NMEA Child Messages Command
      ### High level Command Generator ###
      SetGPSbaud(uint32_t baud, bool)   // Set UBLOX GPS Port Configuration Baud rate
      SetNAV5(uint8_t dynModel, bool)   // Set Dynamic platform model Navigation Engine Settings (0:portable, 3:pedestrian, Etc)
      SetRATE(uint16_t measRate, bool)  // Set Navigation/Measurement Rate Settings (100ms=10.00Hz, 200ms=5.00Hz, 1000ms=1.00Hz, Etc)
    */
    
    //  UBX-NAV-POSLLH -- Geodetic Position Solution
    //  ### UBX Protocol, Class NAV 0x01, ID 0x02 ###
    // UBX-NAV-POSLLH (0x01 0x02) (Payload U-blox-M8&M7&M6=20)
    // iTOW                       ///< [ms], GPS time of the navigation epoch
    // lon                        ///< [deg], Longitude
    // lat                        ///< [deg], Latitude
    // height                     ///< [m], Height above ellipsoid
    // hMSL                       ///< [m], Height above mean sea level
    // hAcc                       ///< [m], Horizontal accuracy estimate
    
    //  UBX-NAV-ATT -- Attitude Solution
    //  ### UBX Protocol, Class NAV 0x01, ID 0x05 ###
    //  ### NOTE: U-blox M8 from protocol version 19 up to version 23.01 (only with ADR or UDR products) ###
    // UBX-NAV-ATT (0x01 0x05)    (Payload U-blox-M8=32)
    // version                    ///< [ND],  Message version (0 for this version)
    // roll                       ///< [deg], Vehicle roll.
    // pitch                      ///< [deg], Vehicle pitch.
    // heading                    ///< [deg], Heading of motion (2-D)
    // accRoll                    ///< [deg], Vehicle roll accuracy (if null, roll angle is not available).
    // accPitch                   ///< [deg], Vehicle pitch accuracy (if null, pitch angle is not available).
    // accHeading                 ///< [deg], Vehicle heading accuracy (if null, heading angle is not available).
    
    //  UBX-NAV-PVT --  Navigation Position Velocity Time Solution
    //  ### UBX Protocol, Class NAV 0x01, ID 0x07 ###
    // UBX-NAV-PVT (0x01 0x07)    (Payload U-blox-M8=92, M7&M6=84)
    // iTOW                       ///< [ms], GPS time of the navigation epoch
    // utcYear                    ///< [year], Year (UTC)
    // utcMonth                   ///< [month], Month, range 1..12 (UTC)
    // utcDay                     ///< [day], Day of month, range 1..31 (UTC)
    // utcHour                    ///< [hour], Hour of day, range 0..23 (UTC)
    // utcMin                     ///< [min], Minute of hour, range 0..59 (UTC)
    // utcSec                     ///< [s], Seconds of minute, range 0..60 (UTC)
    // valid                      ///< [ND], Validity flags
    // tAcc                       ///< [ns], Time accuracy estimate (UTC)
    // utcNano                    ///< [ns], Fraction of second, range -1e9 .. 1e9 (UTC)
    // fixType                    ///< [ND], GNSSfix Type: 0: no fix, 1: dead reckoning only, 2: 2D-fix, 3: 3D-fix, 4: GNSS + dead reckoning combined, 5: time only fix
    // flags                      ///< [ND], Fix status flags
    // flags2                     ///< [ND], Additional flags
    // numSV                      ///< [ND], Number of satellites used in Nav Solution
    // lon                        ///< [deg], Longitude
    // lat                        ///< [deg], Latitude
    // height                     ///< [m], Height above ellipsoid
    // hMSL                       ///< [m], Height above mean sea level
    // hAcc                       ///< [m], Horizontal accuracy estimate
    // vAcc                       ///< [m], Vertical accuracy estimate
    // velN                       ///< [m/s], NED north velocity
    // velE                       ///< [m/s], NED east velocity
    // velD                       ///< [m/s], NED down velocity
    // gSpeed                     ///< [m/s], Ground Speed (2-D)
    // heading                    ///< [deg], Heading of motion (2-D)
    // sAcc                       ///< [m/s], Speed accuracy estimate
    // headingAcc                 ///< [deg], Heading accuracy estimate (both motion and vehicle)
    // pDOP                       ///< [ND], Position DOP
    // headVeh                    ///< [deg], Heading of vehicle (2-D)             #### NOTE: u-blox8 only ####
    // --- magDec, magAcc --- TODO TEST
    // magDec                     ///< [deg], Magnetic declination                 #### NOTE: u-blox8 only ####
    // magAcc                     ///< [deg], Magnetic declination accuracy        #### NOTE: u-blox8 only ####
    
    #ifndef UBLOX_h
    #define UBLOX_h
    
    #include "Arduino.h"
    
    struct gpsData {
      unsigned long   iTOW;        ///< [ms], GPS time of the navigation epoch
      unsigned short  utcYear;     ///< [year], Year (UTC)
      unsigned char   utcMonth;    ///< [month], Month, range 1..12 (UTC)
      unsigned char   utcDay;      ///< [day], Day of month, range 1..31 (UTC)
      unsigned char   utcHour;     ///< [hour], Hour of day, range 0..23 (UTC)
      unsigned char   utcMin;      ///< [min], Minute of hour, range 0..59 (UTC)
      unsigned char   utcSec;      ///< [s], Seconds of minute, range 0..60 (UTC)
      unsigned char   valid;       ///< [ND], Validity flags
      unsigned long   tAcc;        ///< [ns], Time accuracy estimate (UTC)
      long            utcNano;     ///< [ns], Fraction of second, range -1e9 .. 1e9 (UTC)
      unsigned char   fixType;     ///< [ND], GNSSfix Type: 0: no fix, 1: dead reckoning only, 2: 2D-fix, 3: 3D-fix, 4: GNSS + dead reckoning combined, 5: time only fix
      unsigned char   flags;       ///< [ND], Fix status flags
      unsigned char   flags2;      ///< [ND], Additional flags
      unsigned char   numSV;       ///< [ND], Number of satellites used in Nav Solution
      double          lon;         ///< [deg], Longitude
      double          lat;         ///< [deg], Latitude
      double          height;      ///< [m], Height above ellipsoid
      double          hMSL;        ///< [m], Height above mean sea level
      double          hAcc;        ///< [m], Horizontal accuracy estimate
      double          vAcc;        ///< [m], Vertical accuracy estimate
      double          velN;        ///< [m/s], NED north velocity
      double          velE;        ///< [m/s], NED east velocity
      double          velD;        ///< [m/s], NED down velocity
      double          gSpeed;      ///< [m/s], Ground Speed (2-D)
      double          heading;     ///< [deg], Heading of motion (2-D) ***
      double          sAcc;        ///< [m/s], Speed accuracy estimate
      double          headingAcc;  ///< [deg], Heading accuracy estimate (both motion and vehicle)
      double          pDOP;        ///< [ND], Position DOP
      double          headVeh;     ///< [deg], Heading of vehicle (2-D)        #### NOTE: u-blox8 only ####
      double          magDec;      ///< [deg], Magnetic declination            #### NOTE: u-blox8 only ####
      double          magAcc;      ///< [deg], Magnetic declination accuracy   #### NOTE: u-blox8 only ####
      // UBX-NAV-ATT (0x01 0x05) Length (Payload 32)
      // #### NOTE: u-blox 8 from protocol version 19 up to version 23.01 (only with ADR or UDR products) ###
      unsigned char   version;     ///< [ND],  Message version (0 for this version)
      double          roll;        ///< [deg], Vehicle roll.
      double          pitch;       ///< [deg], Vehicle pitch.
      //              heading;     ///< [deg], heading *** see above ^^^^
      double          accRoll;     ///< [deg], Vehicle roll accuracy (if null, roll angle is not available).
      double          accPitch;    ///< [deg], Vehicle pitch accuracy (if null, pitch angle is not available).
      double          accHeading;  ///< [deg], Vehicle heading accuracy (if null, heading angle is not available).
    
      /* Returned after sending command */
      // Send command, gps.Poll_CFG_Port1(true); // true = print ACK on usb Serial Monitor, Ack/Nack Messages: i.e. as replies to CFG Input Messages.
      uint32_t        GpsUart1Baud; ///< [ND], Current GPS Uart1 baud rate --
    };
    
    class UBLOX {
      public:
        UBLOX();
        UBLOX(uint8_t bus);
        void configure(uint8_t bus);
        void begin(int baud);
        void end(); // Disables serial communication, to re-enable serial communication, call gps.begin(Baud);
        bool read(gpsData *gpsData_ptr);
    
        /******************************************/
        /**  High level commands, for the user    */
        /******************************************/
        void Poll_GPSbaud_Port1(bool printACK);             // Polls the configuration for one I/O Port, I/O Target 0x01=UART1
        void Poll_NAV_PVT();                                // Polls UBX-NAV-PVT    (0x01 0x07) Navigation Position Velocity Time Solution
        void Poll_NAV_POSLLH();                             // Polls UBX-NAV-POSLLH (0x01 0x02) Geodetic Position Solution
        void Poll_NAV_ATT();                                // Polls UBX-NAV-ATT (0x01 0x05) Attitude Solution
        // ### Periodic auto update ON,OFF command ###
        void Ena_NAV_PVT(bool printACK);                    // Enable periodic auto update NAV_PVT
        void Dis_NAV_PVT(bool printACK);                    // Disable periodic auto update NAV_PVT
        void Ena_NAV_ATT(bool printACK);                    // Enable periodic auto update NAV_ATT
        void Dis_NAV_ATT(bool printACK);                    // Disable periodic auto update NAV_ATT
        void Ena_NAV_POSLLH(bool printACK);                 // Enable periodic auto update NAV_POSLLH
        void Dis_NAV_POSLLH(bool printACK);                 // Disable periodic auto update NAV_POSLLH
        // #### u-blox Switch OFF ALL NMEA MSGs ####
        void Dis_all_NMEA_Child_MSGs(bool printACK);        // Disable All NMEA Child Messages Command
        /** High level Command Generator GPS control write operations **/
        void SetGPSbaud(uint32_t GPS32baud, bool printACK); // UBX-CFG-PRT  (0x06 0x00)  Set UBLOX GPS Port Configuration Baud rate
        void SetNAV5(uint8_t dynModel, bool printACK);      // UBX-CFG-NAV5 (0x06 0x24)  Set Dynamic platform model Navigation Engine Settings (0:portable, 3:pedestrian, Etc)
        void SetRATE(uint16_t measRate, bool printACK);     // UBX-CFG-RATE (0x06 0x08)  Set Navigation/Measurement Rate Settings (100ms=10.00Hz, 200ms=5.00Hz, 1000ms=1.00Hz, Etc)
    
      private:
        uint8_t _bus;
        uint8_t _fpos;
        uint8_t _CurrentClass;
        uint8_t _portID; // I/O Port
        uint32_t _printACK;
        uint32_t _printACK_Dis_all_NMEA;
        uint32_t _printACK_PVT;
        uint32_t _printACK_ATT;
        uint32_t _printACK_POSLLH;
        uint8_t _ACKcount;
        bool _pACK2 = false;
        bool _pACK_RATE = false;
        bool _pACK_NAV5 = false;
        bool _UBX_NAV_PVT_ID = false;
        bool _UBX_NAV_POSLLH_ID = false;
        bool _UBX_NAV_ATT_ID = false;
        bool _UBX_CFG_PRT_ID = false;
        bool _UBX_ACK_ACK_ID = false;
        uint16_t _MSGpayloadSize;
        uint32_t _GpsUartBaud;
    
        static const uint8_t _payloadSize = 100;
        uint8_t _gpsPayload[_payloadSize];
        HardwareSerial* _port;
        bool parse();
        void calcChecksum(unsigned char* CK, unsigned char* payload, uint8_t length);
        void ACK_IDs(uint8_t SERa);
        void NAV_IDs(uint8_t SERn);
        void CFG_IDs(uint8_t SERc);
    
        /** High level Command GPS control write operations **/
        // Generate the uBlox command configuration
        void writeCommand(uint8_t CLASS, uint8_t ID , uint8_t PayloadLength0, uint8_t PayloadLength1, uint8_t Identifier, uint8_t Parameter0, uint8_t Parameter1, uint8_t Parameter2, uint8_t Parameter3, uint8_t reserved0) const;
    
        // ### Polling Commands Mechanism, MSG REQUEST ###
        // All messages that are output by the receiver in a periodic manner (i.e. messages in classes MON, NAV and RXM) can also be polled.
        uint8_t const _Poll_CFG_PRT[9] =       {0xB5, 0x62, 0x06, 0x00, 0x01, 0x00, 0x01, 0x08, 0x22};  // CFG-PRT (0x06 0x00) Polls the GPS baud configuration for one I/O Port, (I/O Target # MSG) 0x01=UART1
        uint8_t const _Poll_NAV_PVT[8] =       {0xB5, 0x62, 0x01, 0x07, 0x00, 0x00, 0x08, 0x19};        // UBX-NAV-PVT (0x01 0x07) Navigation Position Velocity Time Solution
        uint8_t const _Poll_NAV_POSLLH[8] =    {0xB5, 0x62, 0x01, 0x02, 0x00, 0x00, 0x03, 0x0A};        // UBX-NAV-POSLLH (0x01 0x02) Geodetic Position Solution
        uint8_t const _Poll_NAV_ATT[8] =       {0xB5, 0x62, 0x01, 0x05, 0x00, 0x00, 0x06, 0x13};        // UBX-NAV-ATT (0x01 0x05) Attitude Solution
        // TODO
        //uint8_t const _Poll_CFG_NAV5[8] =      {0xB5, 0x62, 0x06, 0x24, 0x00, 0x00, 0x2A, 0x84};        // UBX-CFG-NAV5 (0x06 0x24) Navigation Engine Settings
        //uint8_t const _Poll_CFG_RATE[8] =      {0xB5, 0x62, 0x06, 0x08, 0x00, 0x00, 0x0E, 0x30};        // UBX-CFG-RATE (0x06 0x08) Navigation/Measurement Rate Settings
    
        // ### Periodic auto update ON,OFF command ###
        // CFG-MSG 0x06 0x01
        uint8_t const _Ena_NAV_PVT[11] =       {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x07, 0x01, 0x13, 0x51}; // Enable periodic auto update NAV_PVT
        uint8_t const _Dis_NAV_PVT[11] =       {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x07, 0x00, 0x12, 0x50}; // Disable periodic auto update NAV_PVT
        uint8_t const _Ena_NAV_ATT[11] =       {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x05, 0x01, 0x11, 0x4D}; // Enable periodic auto update NAV_ATT
        uint8_t const _Dis_NAV_ATT[11] =       {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x05, 0x00, 0x10, 0x4C}; // Disable periodic auto update NAV_ATT
        uint8_t const _Ena_NAV_POSLLH[11] =    {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x02, 0x01, 0x0E, 0x47}; // Enable periodic auto update NAV_POSLLH
        uint8_t const _Dis_NAV_POSLLH[11] =    {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x02, 0x00, 0x0D, 0x46}; // Disable periodic auto update NAV_POSLLH
    
        // #### NMEA- u-blox Switch OFF ALL NMEA MSGs ####
        // #### ---------------------------------------------------------------------------------------------------- NMEA--Name - Description -------------------------------------------------------- ####
        uint8_t const _Dis_NMEA_GxDTM[11] =    {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x0A, 0x00, 0x04, 0x23}; // NMEA GxDTM (DTM 0xF0 0x0A) Datum Reference)
        uint8_t const _Dis_NMEA_GxGBS[11] =    {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x09, 0x00, 0x03, 0x21}; // NMEA GxGBS (GBS 0xF0 0x09) GNSS Satellite Fault Detection)
        uint8_t const _Dis_NMEA_GxGGA[11] =    {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x00, 0x00, 0xFA, 0x0F}; // NMEA GxGGA (GGA 0xF0 0x00) Global positioning system fix data)
        uint8_t const _Dis_NMEA_GxGLL[11] =    {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x01, 0x00, 0xFB, 0x11}; // NMEA GxGLL (GLL 0xF0 0x01) Latitude and longitude, with time of position fix and status)
        uint8_t const _Dis_NMEA_GxGNS[11] =    {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x0D, 0x00, 0x07, 0x29}; // NMEA GxGNS (GNS 0xF0 0x0D) GNSS fix data)
        uint8_t const _Dis_NMEA_GxGRS[11] =    {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x06, 0x00, 0x00, 0x1B}; // NMEA GxGRS (GRS 0xF0 0x06) GNSS Range Residuals)
        uint8_t const _Dis_NMEA_GxGSA[11] =    {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x02, 0x00, 0xFC, 0x13}; // NMEA GxGSA (GSA 0xF0 0x02) GNSS DOP and Active Satellites)
        uint8_t const _Dis_NMEA_GxGST[11] =    {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x07, 0x00, 0x01, 0x1D}; // NMEA GxGST (GST 0xF0 0x07) GNSS Pseudo Range Error Statistics)
        uint8_t const _Dis_NMEA_GxGSV[11] =    {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x03, 0x00, 0xFD, 0x15}; // NMEA GxGSV (GSV 0xF0 0x03) GNSS Satellites in View)
        uint8_t const _Dis_NMEA_GxRMC[11] =    {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x04, 0x00, 0xFE, 0x17}; // NMEA GxRMC (RMC 0xF0 0x04) Recommended Minimum data)
        uint8_t const _Dis_NMEA_GxVLW[11] =    {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x0F, 0x00, 0x09, 0x2D}; // NMEA GxVLW (VLW 0xF0 0x0F) Dual ground/water distance)
        uint8_t const _Dis_NMEA_GxVTG[11] =    {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x05, 0x00, 0xFF, 0x19}; // NMEA GxVTG (VTG 0xF0 0x05) Course over ground and Ground speed)
        uint8_t const _Dis_NMEA_GxZDA[11] =    {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x08, 0x00, 0x02, 0x1F}; // NMEA GxZDA (ZDA 0xF0 0x08) Time and Date)
        // #### PUBX- u-blox NMEA extension - Switch OFF ALL ####
        uint8_t const _Dis_PUBXe_NMEA00[11] =  {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF1, 0x00, 0x00, 0xFB, 0x12}; // PUBX 00 (Position Data)
        uint8_t const _Dis_PUBXe_NMEA01[11] =  {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF1, 0x01, 0x00, 0xFC, 0x14}; // PUBX 01 (UTM Position Data)
        uint8_t const _Dis_PUBXe_NMEA03[11] =  {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF1, 0x03, 0x00, 0xFE, 0x18}; // PUBX 03 (Satellite Data)
        uint8_t const _Dis_PUBXe_NMEA04[11] =  {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF1, 0x04, 0x00, 0xFF, 0x1A}; // PUBX 04 (Time of Day)
        uint8_t const _Dis_PUBXe_NMEA05[11] =  {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF1, 0x05, 0x00, 0x00, 0x1C}; // PUBX 05 (EKF Status)
        uint8_t const _Dis_PUBXe_NMEA06[11] =  {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF1, 0x06, 0x00, 0x01, 0x1E}; // PUBX 06 (GPS-only on EKF products)
    
        void DEBUG(); // debug print helper
    };
    #endif
    UBLOX_example2.ino
    Code:
    /*
      UBLOX_example2.ino
      Brian R Taylor
      brian.taylor@bolderflight.com
      2016-07-06
    
      Copyright (c) 2016 Bolder Flight Systems
    
      Permission is hereby granted, free of charge, to any person obtaining a copy of this software
      and associated documentation files (the "Software"), to deal in the Software without restriction,
      including without limitation the rights to use, copy, modify, merge, publish, distribute,
      sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
      furnished to do so, subject to the following conditions:
    
      The above copyright notice and this permission notice shall be included in all copies or
      substantial portions of the Software.
    
      THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
      BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
      NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
      DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
      OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
    */
    
    /*
    **********************************************************************
    **  High level Commands, for the user ~ UBLOX lib. v1.0.2 2018-03-20 *
    **********************************************************************
       NOTE: gps.command(Boolean) == (true)Print Message Acknowledged on USB Serial Monitor
      end();                            // Disables Teensy serial communication, to re-enable, call begin(Baud)
      Poll_CFG_Port1(bool);             // Polls the configuration for one I/O Port, I/O Target 0x01=UART1
      Poll_NAV_PVT();                   // Polls UBX-NAV-PVT    (0x01 0x07) Navigation Position Velocity Time Solution
      Poll_NAV_POSLLH();                // Polls UBX-NAV-POSLLH (0x01 0x02) Geodetic Position Solution
      Poll_NAV_ATT();                   // Polls UBX-NAV-ATT    (0x01 0x05) Attitude Solution
      ### Periodic Auto Update ON,OFF Command ###
      Ena_NAV_PVT(bool);                // Enable periodic auto update NAV_PVT
      Dis_NAV_PVT(bool);                // Disable periodic auto update NAV_PVT
      Ena_NAV_ATT(bool);                // Enable periodic auto update NAV_ATT
      Dis_NAV_ATT(bool);                // Disable periodic auto update NAV_ATT
      Ena_NAV_POSLLH(bool);             // Enable periodic auto update NAV_POSLLH
      Dis_NAV_POSLLH(bool);             // Disable periodic auto update NAV_POSLLH
      #### u-blox Switch off all NMEA MSGs ####
      Dis_all_NMEA_Child_MSGs(bool);    // Disable All NMEA Child Messages Command
      ### High level Command Generator ###
      SetGPSbaud(uint32_t baud, bool)   // Set UBLOX GPS Port Configuration Baud rate
      SetNAV5(uint8_t dynModel, bool)   // Set Dynamic platform model Navigation Engine Settings (0:portable, 3:pedestrian, Etc)
      SetRATE(uint16_t measRate, bool)  // Set Navigation/Measurement Rate Settings (100ms=10.00Hz, 200ms=5.00Hz, 1000ms=1.00Hz, Etc)
    */
    
    #include "UBLOX.h"
    
    // The elapsedMillis feature is built into Teensyduino.
    // For non-Teensy boards, it is available as a library.
    elapsedMillis sinceMSG_poll;
    
    // Set this to the GPS hardware serial port you wish to use
    #define GPShwSERIAL 3 // 1 = Serial1, 2 = Serial2, 3 = Serial3, 4 = Serial4 ....
    uint32_t const BaudDefault = 9600; // default settings
    
    // a uBlox object, which is on Teensy hardware
    // GPS serial port
    UBLOX gps(GPShwSERIAL);
    
    // the uBlox data structure
    gpsData uBloxData;
    
    void setup() {
      // serial to display data
      Serial.begin(9600); // Teensy Serial object always communicates at 12 Mbit/sec USB speed.
      while ( !Serial && (millis() < 10000)) ; // wait until serial monitor is open or timeout 10 seconds
    
      // -- AutoBauding test --
      // Try communication with the GPS
      // receiver at 9600 baud, default settings
      // then set GPS UART Baud to 460800
      gps.begin(BaudDefault);                   // Enable Teensy serial communication @ 9600 baud, default settings.
      gps.SetGPSbaud(460800, false);            // Set GPS Port Baud, Possible Baud Rate Configurations 4800~9600~19200~38400~57600~115200~230400~460800
      gps.end();                                // Disables Teensy serial communication, to re-enable serial communication, call gps.begin(Baud, bool);.
      gps.begin(19200);
      gps.SetGPSbaud(460800, false);
      gps.end();
      gps.begin(38400);
      gps.SetGPSbaud(460800, false);
      gps.end();
      gps.begin(57600);
      gps.SetGPSbaud(460800, false);
      gps.end();
      gps.begin(115200);
      gps.SetGPSbaud(460800, false);
      gps.end();
      gps.begin(230400);
      gps.SetGPSbaud(460800, true);
      gps.end();
      // now start communication with the GPS
      // receiver at 460800 baud,
      gps.begin(460800);                        // Enable Teensy serial communication @ given baud rate
      gps.Poll_GPSbaud_Port1(true);             // Polls the GPS baud configuration for one I/O Port, I/O Target 0x01=UART1
    
      gps.Poll_NAV_PVT();                       // Polls UBX-NAV-PVT    (0x01 0x07) Navigation Position Velocity Time Solution
    
      gps.SetRATE(200, true);                   // Navigation/Measurement Rate Settings, e.g. 100ms => 10Hz, 200 => 5.00Hz, 1000ms => 1Hz, 10000ms => 0.1Hz
      // Possible Configurations:
      // 60=>16.67Hz, 64=>15.63Hz, 72=>13.89Hz, 80=>12.50Hz, 100=>10.00Hz, 125=>8.00Hz, 200=>5.00Hz, 250=>4.00Hz, 500=>2.00Hz
      // 800=>1.25Hz, 1000=>1.00Hz, 2000=>0.50Hz, 4000=>0.25Hz, 10000=>0.10Hz, 20000=>0.05Hz, 50000=>0.02Hz
    
      // NOTE: Dis_all_NMEA -strongly suggest changing RX buffer to 255 or more,*otherwise you will miss ACKs*on serial monitor
      gps.Dis_all_NMEA_Child_MSGs(false);       // Disable All NMEA Child Messages Command
    
      gps.SetNAV5(3, true);                     // Set Dynamic platform model Navigation Engine Settings (0:portable, 2: stationary, 3:pedestrian, Etc)
      // Possible Configurations
      // 0: portable, 2: stationary, 3: pedestrian, 4: automotive, 5: sea, 6: airborne with <1g, 7: airborne with <2g
      // 8: airborne with <4g, 9: wrist worn watch (not supported in protocol v.less than 18)
    
      // ### Periodic auto update ON,OFF Command ###
      //gps.Ena_NAV_PVT(true);                  // Enable periodic auto update NAV_PVT
      gps.Dis_NAV_PVT(true);                    // Disable periodic auto update NAV_PVT
    
      //gps.Ena_NAV_ATT(true);                  // Enable periodic auto update NAV_ATT ~ U-blox M8 from protocol version 19
      gps.Dis_NAV_ATT(true);                    // Disable periodic auto update NAV_ATT ~ ---^
    
      gps.Ena_NAV_POSLLH(true);                 // Enable periodic auto update NAV_POSLLH
      //gps.Dis_NAV_POSLLH(true);               // Disable periodic auto update NAV_POSLLH
    }
    
    void loop() {
    
      // The elapsedMillis feature is built into Teensyduino.
      // For non-Teensy boards, it is available as a library.
      // #### Polling Mechanism ####
      if (sinceMSG_poll >= 10000) { // SEND poll message request every 10 sec.
        sinceMSG_poll = 0;          // reset since
    
        // ### Polling Command(s) ###
        Serial.println(" SEND gps.Poll_NAV_PVT()");
        gps.Poll_NAV_PVT();                           // Polls UBX-NAV-PVT    (0x01 0x07) Navigation Position Velocity Time Solution
        //gps.Poll_NAV_POSLLH();                      // Polls UBX-NAV-POSLLH (0x01 0x02) Geodetic Position Solution
        //gps.Poll_NAV_ATT();                         // Polls UBX-NAV-ATT (0x01 0x05) Attitude Solution
    
        // UBX-NAV-PVT --  Navigation Position Velocity Time Solution
        // ### UBX Protocol, Class NAV 0x01, ID 0x07 ###
        // UBX-NAV-PVT (0x01 0x07)    (Payload U-blox-M8=92, M7&M6=84)
        // iTOW;                      ///< [ms], GPS time of the navigation epoch
        // utcYear;                   ///< [year], Year (UTC)
        // utcMonth;                  ///< [month], Month, range 1..12 (UTC)
        // utcDay;                    ///< [day], Day of month, range 1..31 (UTC)
        // utcHour;                   ///< [hour], Hour of day, range 0..23 (UTC)
        // utcMin;                    ///< [min], Minute of hour, range 0..59 (UTC)
        // utcSec;                    ///< [s], Seconds of minute, range 0..60 (UTC)
        // valid;                     ///< [ND], Validity flags
        // tAcc;                      ///< [ns], Time accuracy estimate (UTC)
        // utcNano;                   ///< [ns], Fraction of second, range -1e9 .. 1e9 (UTC)
        // fixType;                   ///< [ND], GNSSfix Type: 0: no fix, 1: dead reckoning only, 2: 2D-fix, 3: 3D-fix, 4: GNSS + dead reckoning combined, 5: time only fix
        // flags;                     ///< [ND], Fix status flags
        // flags2;                    ///< [ND], Additional flags
        // numSV;                     ///< [ND], Number of satellites used in Nav Solution
        // lon;                       ///< [deg], Longitude
        // lat;                       ///< [deg], Latitude
        // height;                    ///< [m], Height above ellipsoid
        // hMSL;                      ///< [m], Height above mean sea level
        // hAcc;                      ///< [m], Horizontal accuracy estimate
        // vAcc;                      ///< [m], Vertical accuracy estimate
        // velN;                      ///< [m/s], NED north velocity
        // velE;                      ///< [m/s], NED east velocity
        // velD;                      ///< [m/s], NED down velocity
        // gSpeed;                    ///< [m/s], Ground Speed (2-D)
        // heading;                   ///< [deg], Heading of motion (2-D)
        // sAcc;                      ///< [m/s], Speed accuracy estimate
        // headingAcc;                ///< [deg], Heading accuracy estimate (both motion and vehicle)
        // pDOP;                      ///< [ND], Position DOP
        // headVeh;                   ///< [deg], Heading of vehicle (2-D)             #### NOTE: u-blox8 only ####
        //   magDec, magAcc --- TODO TEST
        // magDec;                    ///< [deg], Magnetic declination                 #### NOTE: u-blox8 only ####
        // magAcc;                    ///< [deg], Magnetic declination accuracy        #### NOTE: u-blox8 only ####
        Serial.print("UBX-NAV-PVT -\t");
        Serial.print(uBloxData.utcYear);  ///< [year], Year (UTC)
        Serial.print("y:");
        Serial.print(uBloxData.utcMonth); ///< [month], Month, range 1..12 (UTC)
        Serial.print("m:");
        Serial.print(uBloxData.utcDay);   ///< [day], Day of month, range 1..31 (UTC)
        Serial.print("d\t");
        Serial.print(uBloxData.utcHour);  ///< [hour], Hour of day, range 0..23 (UTC)
        Serial.print(":");
        Serial.print(uBloxData.utcMin);   ///< [min], Minute of hour, range 0..59 (UTC)
        Serial.print(":");
        Serial.print(uBloxData.utcSec);   ///< [s], Seconds of minute, range 0..60 (UTC)
        Serial.print("sec\t");
        Serial.print(uBloxData.numSV);    ///< [ND], Number of satellites used in Nav Solution
        Serial.print(" #sat\t");
        Serial.print(uBloxData.lat, 10);  ///< [deg], Latitude
        Serial.print("\t");
        Serial.print(uBloxData.lon, 10);  ///< [deg], Longitude
        Serial.print("\t m sea:");
        Serial.print(uBloxData.hMSL);     ///< [m], Height above mean sea level
        Serial.print("\t m ell:");
        Serial.print(uBloxData.height);   ///< [m], Height above ellipsoid
        Serial.print("\t acc:");
        Serial.println(uBloxData.hAcc);   ///< [m], Horizontal accuracy estimate
    
        // Current Baud
        Serial.print(" Current Baud: \t");
        Serial.println(uBloxData.GpsUart1Baud); // Current UBLOX GPS Uart1 baud rate
      }
    
      // checking to see if a good packet has
      // been received and displaying some
      // of the packet data
    
      if (gps.read(&uBloxData) ) {
        // UBX-NAV-POSLLH -- Geodetic Position Solution
        //   ### UBX Protocol, Class NAV 0x01, ID 0x02 ###
        // UBX-NAV-POSLLH (0x01 0x02) (Payload U-blox-M8&M7&M6=20)
        // iTOW;                      ///< [ms], GPS time of the navigation epoch
        // lon;                       ///< [deg], Longitude
        // lat;                       ///< [deg], Latitude
        // height;                    ///< [m], Height above ellipsoid
        // hMSL;                      ///< [m], Height above mean sea level
        // hAcc;                      ///< [m], Horizontal accuracy estimate
        Serial.print("UBX-NAV-POSLLH -\t");
        Serial.print(uBloxData.iTOW);     ///< [ms], GPS time of the navigation epoch
        float i;
        i = uBloxData.iTOW;
        i = i / 1000;
        Serial.print("\t i~iTOW ");
        Serial.print(i, 3);                  ///< float iTOW
        Serial.print("\t");
        Serial.print(uBloxData.lat, 7);      ///< [deg], Latitude
        Serial.print("\t");
        Serial.print(uBloxData.lon, 7);      ///< [deg], Longitude
        Serial.print("\t m sea:");
        Serial.print(uBloxData.hMSL);        ///< [m], Height above mean sea level
        Serial.print("\t m ell:");
        Serial.print(uBloxData.height);      ///< [m], Height above ellipsoid
        Serial.print("\t acc:");
        Serial.println(uBloxData.hAcc);      ///< [m], Horizontal accuracy estimate
      }
    
      /*
        // UBX-NAV-POSLLH -- Geodetic Position Solution
        if (gps.read(&uBloxData) ) {
        //   ### UBX Protocol, Class NAV 0x01, ID 0x02 ###
        // UBX-NAV-POSLLH (0x01 0x02) (Payload U-blox-M8&M7&M6=20)
        // iTOW;                      ///< [ms], GPS time of the navigation epoch
        // lon;                       ///< [deg], Longitude
        // lat;                       ///< [deg], Latitude
        // height;                    ///< [m], Height above ellipsoid
        // hMSL;                      ///< [m], Height above mean sea level
        // hAcc;                      ///< [m], Horizontal accuracy estimate
        Serial.println("UBX-NAV-POSLLH ID 0x02");
        Serial.print("UBX-NAV-POSLLH -\t");
        Serial.print(uBloxData.lat, 7);  ///< [deg], Latitude
        Serial.print("\t");
        Serial.print(uBloxData.lon, 7);  ///< [deg], Longitude
        Serial.print("\t");
        Serial.print(uBloxData.hMSL);     ///< [m], Height above mean sea level
        Serial.print("\t");
        Serial.print(uBloxData.hAcc);   ///< [m], Horizontal accuracy estimate
        }
      */
    
      /*
        // UBX-NAV-PVT --  Navigation Position Velocity Time Solution
        if ( gps.read(&uBloxData) ) {
        // ### UBX Protocol, Class NAV 0x01, ID 0x07 ###
        // UBX-NAV-PVT (0x01 0x07)    (Payload U-blox-M8=92, M7&M6=84)
        // iTOW;                      ///< [ms], GPS time of the navigation epoch
        // utcYear;                   ///< [year], Year (UTC)
        // utcMonth;                  ///< [month], Month, range 1..12 (UTC)
        // utcDay;                    ///< [day], Day of month, range 1..31 (UTC)
        // utcHour;                   ///< [hour], Hour of day, range 0..23 (UTC)
        // utcMin;                    ///< [min], Minute of hour, range 0..59 (UTC)
        // utcSec;                    ///< [s], Seconds of minute, range 0..60 (UTC)
        // valid;                     ///< [ND], Validity flags
        // tAcc;                      ///< [ns], Time accuracy estimate (UTC)
        // utcNano;                   ///< [ns], Fraction of second, range -1e9 .. 1e9 (UTC)
        // fixType;                   ///< [ND], GNSSfix Type: 0: no fix, 1: dead reckoning only, 2: 2D-fix, 3: 3D-fix, 4: GNSS + dead reckoning combined, 5: time only fix
        // flags;                     ///< [ND], Fix status flags
        // flags2;                    ///< [ND], Additional flags
        // numSV;                     ///< [ND], Number of satellites used in Nav Solution
        // lon;                       ///< [deg], Longitude
        // lat;                       ///< [deg], Latitude
        // height;                    ///< [m], Height above ellipsoid
        // hMSL;                      ///< [m], Height above mean sea level
        // hAcc;                      ///< [m], Horizontal accuracy estimate
        // vAcc;                      ///< [m], Vertical accuracy estimate
        // velN;                      ///< [m/s], NED north velocity
        // velE;                      ///< [m/s], NED east velocity
        // velD;                      ///< [m/s], NED down velocity
        // gSpeed;                    ///< [m/s], Ground Speed (2-D)
        // heading;                   ///< [deg], Heading of motion (2-D)
        // sAcc;                      ///< [m/s], Speed accuracy estimate
        // headingAcc;                ///< [deg], Heading accuracy estimate (both motion and vehicle)
        // pDOP;                      ///< [ND], Position DOP
        // headVeh;                   ///< [deg], Heading of vehicle (2-D)             #### NOTE: u-blox8 only ####
        //   magDec, magAcc --- TODO TEST
        // magDec;                    ///< [deg], Magnetic declination                 #### NOTE: u-blox8 only ####
        // magAcc;                    ///< [deg], Magnetic declination accuracy        #### NOTE: u-blox8 only ####
        Serial.print("UBX-NAV-PVT -\t");
        Serial.print(uBloxData.utcYear);  ///< [year], Year (UTC)
        Serial.print("\t");
        Serial.print(uBloxData.utcMonth); ///< [month], Month, range 1..12 (UTC)
        Serial.print("\t");
        Serial.print(uBloxData.utcDay);   ///< [day], Day of month, range 1..31 (UTC)
        Serial.print("\t");
        Serial.print(uBloxData.utcHour);  ///< [hour], Hour of day, range 0..23 (UTC)
        Serial.print("\t");
        Serial.print(uBloxData.utcMin);   ///< [min], Minute of hour, range 0..59 (UTC)
        Serial.print("\t");
        Serial.print(uBloxData.utcSec);   ///< [s], Seconds of minute, range 0..60 (UTC)
        Serial.print("\t");
        Serial.print(uBloxData.numSV);    ///< [ND], Number of satellites used in Nav Solution
        Serial.print("\t");
        Serial.print(uBloxData.lat, 10);  ///< [deg], Latitude
        Serial.print("\t");
        Serial.print(uBloxData.lon, 10);  ///< [deg], Longitude
        Serial.print("\t");
        Serial.println(uBloxData.hMSL);   ///< [m], Height above mean sea level
        }
      */
      /*
        // UBX-NAV-ATT -- Attitude Solution
        if (gps.read(&uBloxData) ) {
        //  ### UBX Protocol, Class NAV 0x01, ID 0x05 ###
        //  ### NOTE: U-blox M8 from protocol version 19 up to version 23.01 (only with ADR or UDR products) ###
        // UBX-NAV-ATT (0x01 0x05)    (Payload U-blox-M8=32)
        // version                    ///< [ND],  Message version (0 for this version)
        // roll                       ///< [deg], Vehicle roll.
        // pitch                      ///< [deg], Vehicle pitch.
        // heading                    ///< [deg], Heading of motion (2-D)
        // accRoll                    ///< [deg], Vehicle roll accuracy (if null, roll angle is not available).
        // accPitch                   ///< [deg], Vehicle pitch accuracy (if null, pitch angle is not available).
        // accHeading                 ///< [deg], Vehicle heading accuracy (if null, heading angle is not available).
        Serial.print("UBX-NAV-ATT ----\t");
        Serial.print(uBloxData.roll);         ///< [deg], Vehicle roll.
        Serial.print("\t");
        Serial.print(uBloxData.pitch);        ///< [deg], Vehicle pitch.
        Serial.print("\t");
        Serial.print(uBloxData.heading);      ///< [deg], Heading of motion (2-D)
        Serial.print("\t");
        Serial.print(uBloxData.accRoll);      ///< [deg], Vehicle roll accuracy (if null, roll angle is not available).
        Serial.print("\t");
        Serial.print(uBloxData.accPitch);     ///< [deg], Vehicle pitch accuracy (if null, pitch angle is not available).
        Serial.print("\t");
        Serial.println(uBloxData.accHeading); ///< [deg], Vehicle heading accuracy (if null, heading angle is not available).
        }
      */
    }

  3. #28
    Senior Member+
    Join Date
    Jul 2014
    Location
    New York
    Posts
    3,347
    Nice work Chris. Should be a good change and add some flexibility instead of having to go back and forth to ucenter. I will probably give it a try tomorrow. Suppose to get about 10inches of snow so I will be stuck in the house

  4. #29
    Senior Member
    Join Date
    Dec 2013
    Posts
    214
    Yep more snow as long as i don't lose power i'll be okay, Northeast PA Delaware Water Gap here

  5. #30
    Senior Member
    Join Date
    Dec 2013
    Posts
    214
    Quick update
    Not sure how I managed to miss 921600 baud rate. setting
    In case of anyone using this baud rate you can edit UBLOX.Cpp line 458.
    Code:
        case 921600:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("921600");
          }
          break;
        case 1500000: // experimental baud rate 1500000
          Supported = true;
          if ( printACK == true ) {
            Serial.println("1500000");
          }
          break;
        default:
          Supported = false;
          _pACK2 = false;
          Serial.print("\tNot Supported :"), Serial.println(baud32rate), Serial.println("\tPossible Baud Rate Configurations");
          Serial.println("\t4800~9600~19200~38400~57600~115200~230400~460800~921600~1500000<experimental"), Serial.println();
          break;
    I did try 1843200 baud rate but did not succeed, it does appear to work correctly at 1500000 baud rate.
    Yes I'm running my GPS on the 1500000 experimental baud rate setting no issues so far.

  6. #31
    Senior Member+
    Join Date
    Jul 2014
    Location
    New York
    Posts
    3,347
    Hi Chris. I just made a link to this over on the uNAVAHRS thread. Figured the guys over there would be interested. Nice job by the way

  7. #32
    Senior Member
    Join Date
    Dec 2016
    Location
    Montreal, Canada
    Posts
    3,020
    hi chris, giving u a heads up, i was able to do 3 megabaud in my ESP<-> teensy project, BUT, it was only possible when i put the polling function in the SerialEvent handler not just the loop, although my parser captures the entire buffer instead of single bytes this could make a difference if you dont capture your buffer as well. but its worth a shot to try, serialevents is called in many places not just end of loop like arduinos

  8. #33
    Senior Member
    Join Date
    Dec 2013
    Posts
    214
    Sounds good tanks.
    I don't believe it's Teensy issue here, i believe the limiting speed is the GPS Hardware here.

    Ok so I've been running stress test of this 1500000 baud rate.
    Basically i used u-center and enable all possible msg. Combination (NMEA, UBX, Monitoring msg. Etc)
    So far so good
    Click image for larger version. 

Name:	Stress testing.jpg 
Views:	77 
Size:	76.0 KB 
ID:	13345
    Now GPS UART1 @ 1500000 baud rate
    TX bytes 35001890

    EDIT:
    TX bytes 43863123
    Just a quick note this is all done on breadboard with 5-inch wires between GPS and Teensy 3.5 on serial 3

  9. #34
    Senior Member+
    Join Date
    Jul 2014
    Location
    New York
    Posts
    3,347
    Between these changes and the SPI_MSTransfer libraries things are going to get interesting fast

  10. #35
    Senior Member
    Join Date
    Dec 2013
    Posts
    214
    Yeah it does look interesting so far.
    I will try faster baud rate when i have more time.
    TX bytes @ 89576358 no errors

    I'm thinking about implementing Monitoring Message command ~ UBX-MON-IO (0x0A 0x02) I/O Subsystem Status.
    The size of the message is determined by the number of ports 'N' the receiver supports, i.e.
    on u-blox 5 the number of ports is 6.

    0xB5 0x62 0x0A 0x02 0 + 20*N
    rxBytes bytes Number of bytes ever received
    txBytes bytes Number of bytes ever sent
    parityErrs - Number of 100ms timeslots with parity errors
    framingErrs - Number of 100ms timeslots with framing errors
    overrunErrs - Number of 100ms timeslots with overrun errors
    breakCond - Number of 100ms timeslots with break conditions
    rxBusy - Flag is receiver is busy
    txBusy - Flag is transmitter is busy
    reserved1 - Reserved

    Then next command on the list:
    UBX-CFG-TP5 (0x06 0x31) Time Pulse Parameters
    u-blox receivers include a time pulse function providing clock pulses with configurable duration and frequency.
    The time pulse function can be configured using the UBX-CFG-TP5 message.
    The UBX-TIM-TP message provides time information for the next pulse, time source and the quantization error of the output pin.
    https://forum.pjrc.com/threads/46058...-for-beginners

  11. #36
    Senior Member
    Join Date
    Dec 2016
    Location
    Montreal, Canada
    Posts
    3,020
    you might wanna fix this:

    Code:
    uint8_t MSGpayload = (_gpsPayload[3] << 8) | _gpsPayload[2]; // Combining two uint8_t as uint16_t
    should be

    Code:
    uint16_t MSGpayload = (uint16_t)(_gpsPayload[3] << 8) | _gpsPayload[2]; // Combining two uint8_t as uint16_t

  12. #37
    Senior Member
    Join Date
    Dec 2013
    Posts
    214
    Whoa that's a good catch thanks man.
    It could have been an ugly bug to find later on, lucky me there is not too many messages exceeding 255 bytes

  13. #38
    Senior Member
    Join Date
    Dec 2016
    Location
    Montreal, Canada
    Posts
    3,020
    i caught it by chance while flipping code through my phone screen, i guess i was lucky

    is there a reason why you make a 16 bit but print out the 8 bit data instead?

  14. #39
    Senior Member
    Join Date
    Dec 2013
    Posts
    214
    I'm assuming you looking at my DEBUG(), i'm only grabbing the payload length field which is 16-bit from the received message which is stored in _gpsPayload[i] array.
    Code:
    void UBLOX::DEBUG() {
      // DEBUG PRINT
      Serial.println("HEADER [0]=HEX B5 DEC 181‬");
      Serial.println("HEADER [1]=HEX 62 DEC  98‬‬");
    
      uint16_t MSGpayload = (_gpsPayload[3] << 8) | _gpsPayload[2]; // Combining two uint8_t as uint16_t
      for (int i = 0; i < (MSGpayload) + 4; i++) {
        Serial.print("_gpsPayload["), Serial.print(i, DEC), Serial.print("]=");
        if (_gpsPayload[i] < 16) {
          Serial.print("HEX 0");
          Serial.print(_gpsPayload[i], HEX);
        } else {
          Serial.print("HEX ");
          Serial.print(_gpsPayload[i], HEX);
        }
        if (_gpsPayload[i] < 10) {
          Serial.print(" DEC 0");
          Serial.println(_gpsPayload[i], DEC);
        } else {
          Serial.print(" DEC ");
          Serial.println(_gpsPayload[i], DEC);
        }
      }
    }
    UBX Frame Structure
    Click image for larger version. 

Name:	UBX Frame Structure.jpg 
Views:	100 
Size:	55.6 KB 
ID:	13346
    This is how it prints the array data

    HEADER [0]=HEX B5 DEC 181‬
    HEADER [1]=HEX 62 DEC 98‬‬
    _gpsPayload[0]=HEX 06 DEC 06 Class
    _gpsPayload[1]=HEX 00 DEC 00 Id
    _gpsPayload[2]=HEX 14 DEC 20 <---2-byte Length field (Little Endian)
    _gpsPayload[3]=HEX 00 DEC 00 <---2-byte Length field
    _gpsPayload[4]=HEX 01 DEC 01 Payload
    _gpsPayload[5]=HEX 00 DEC 00
    _gpsPayload[6]=HEX 00 DEC 00
    _gpsPayload[7]=HEX 00 DEC 00
    _gpsPayload[8]=HEX C0 DEC 192
    _gpsPayload[9]=HEX 08 DEC 08
    _gpsPayload[10]=HEX 00 DEC 00
    _gpsPayload[11]=HEX 00 DEC 00
    _gpsPayload[12]=HEX 60 DEC 96 <---- BAUD RATE ~~ HEX 0016E360, DEC 1500000
    _gpsPayload[13]=HEX E3 DEC 227 <---- BAUD RATE
    _gpsPayload[14]=HEX 16 DEC 22 <---- BAUD RATE
    _gpsPayload[15]=HEX 00 DEC 00 <---- BAUD RATE
    _gpsPayload[16]=HEX 07 DEC 07
    _gpsPayload[17]=HEX 00 DEC 00
    _gpsPayload[18]=HEX 03 DEC 03
    _gpsPayload[19]=HEX 00 DEC 00
    _gpsPayload[20]=HEX 00 DEC 00
    _gpsPayload[21]=HEX 00 DEC 00
    _gpsPayload[22]=HEX 00 DEC 00
    _gpsPayload[23]=HEX 00 DEC 00

    not printing CRC

    I must have used copy and paste as i have this bug in two places @ line 1280 and @ line 1882 the DEBUG()
    Code:
      if (_UBX_CFG_PRT_ID == true) {
       // Decide what to do based on payload size, length in decimal format
       uint16_t MSGpayload = (_gpsPayload[3] << 8) | _gpsPayload[2]; // Combining two uint8_t as uint16_t

  15. #40
    Senior Member+ defragster's Avatar
    Join Date
    Feb 2015
    Posts
    8,620
    Quote Originally Posted by Chris O. View Post
    ....

    Just a quick note this is all done on breadboard with 5-inch wires between GPS and Teensy 3.5 on serial 3
    Note on Teensy3.x the Serial1 and Serial2 have an 8 byte FIFO that limits interrupts to service transfers, Serial3 has one byte meaning more interrupts if delayed result in data loss, and many more interrupts.

    About serialEvent#():: that is called on Teensy with every delay( # ); as well at the end of each loop(). Increasing the default Rx buffer ( ex: SERIAL1_TX_BUFFER_SIZE ) in the core file can allow buffering of a complete GPS message. I generally don't use serialEvent#() and redefine a 'void yield() {}' that stops system from iterating over all 3 to 6 Serial# ports looking for the need to call serialEvent#() that reduces system overhead.

  16. #41
    Senior Member
    Join Date
    Dec 2013
    Posts
    214
    All good points except my first two (Serial1-Serial2) are already busy with my custom UART implementation (not much left from the Teensyduino implementation).
    I'm using Serial1 for Chrysler--CCD (Chrysler Collision Detection) Data Bus and Serial2 for ECU Chrysler SCI BUS programming.

    This is for my pickup truck overhead console replacement project capable of doing diagnostics and reprogramming. This is why I need the GPS as it's a overhead console it would be nice to have a compass plus other data available

  17. #42
    Senior Member
    Join Date
    Dec 2016
    Location
    Montreal, Canada
    Posts
    3,020
    no problem, i wonder how much it fares against MST slave using the remote Serial1/2 over 30MHz, youd have access to those FIFO ports the serial methods are already built in the library for all 6 uarts and usbserial (7 total)

  18. #43
    Senior Member
    Join Date
    Dec 2013
    Posts
    214
    I've been doing this for about 2 years already but between ATmega328P Slave and Tennsy 3.6 as Master.
    Of course the ATmega328P can only handle 4Mhz SPI communication.

    The part responsible for SPI communication on the Slave ATmega328P.
    Code:
    // Pin change interrupt D8-D13.
    ISR (PCINT0_vect) // D8-D13 = PCINT 0-5 = PCIR0 = PB = PCIE0 = pcmsk0
    {
      // -------------------SPI-------------------------------------------------------------------------
      /* SS (Slave Select (D10) ), This tells the slave that it should wake up and receive / send data
         and is also used when multiple slaves are present to select the one you’d like to talk to.
         The SS line is normally held high, which disconnects the slave from the SPI bus.
         This type of logic is known as “active low,” and you’ll often see used it for enable and reset lines.
         Just before data is sent to the slave, the line is brought low, which activates the slave.
         When you’re done using the slave, the line is made high again.
    
         Important point is that we need to add a slight delay on master MCU (something like 30 microseconds).
         Otherwise the slave doesn't have a chance to react to the incoming data and do something with it.
      */
    
      if (PINB & (1 << PB2)) { //PB2 // true if the pin in question is high  /* Slave Disabled */
        digitalWrite_LOW(PORTD, Debug_led); // debug led on pin 6
      }
    
      if (LastWrite == 1) {
        ccd_ptr = ccd_ptr_write; // copy pointer
        for (int i = 0; i < ccd_ptr; i ++) {
          ccd_buff[i] = ccd_buff_write[i]; // copy to ccd buff from ccd_buff_write /* SPI SHOW */
        }
        LastWrite = 0;
      }
    
      //if (digitalRead(10) == LOW) { // Pin 10 <-> SS Pin (Slave Select)
      if (~PINB & (1 << PB2)) { // true when PB2 is low / Pin 10 <-> SS Pin (Slave Select)
        /* Slave Enabled */
        Set_OUTPUT(DDRB, PB4); // Set MISO to OUTPUT --> TEST
        //digitalWrite(Debug_led, HIGH);
        digitalWrite_HIGH(PORTD, Debug_led);
    
        uint8_t ACK = TransmitStatus;
    
        // 1--------
        asm volatile("nop");
        while (!(SPSR & (1 << SPIF))); // wait
        TimeToWrite = SPDR; // 1ST READ / receive from the SPI bus, read from master
        SPDR = ACK; // send back ACK: TransmitStatus
        if (TimeToWrite == 0) {
          TransmitStatus = 0; // 0 = was not transmiting
        }
    
        // 2--------
        asm volatile("nop");
        while (!(SPSR & _BV(SPIF))) ; // wait
        CCD_Transmit_Len = SPDR; // receive from the SPI bus, read from master
        SPDR = ccd_ptr; // send back ccd_ptr
    
        // 3-------
        asm volatile("nop");
        while (!(SPSR & _BV(SPIF))) ; // wait
        CCD_Transmit_Buff[0] = SPDR; // receive from the SPI bus, read from master
        SPDR = ccd_buff[0]; // send back ccd_buff[0]
    
        // 4-------
        asm volatile("nop");
        while (!(SPSR & _BV(SPIF))) ; // wait
        CCD_Transmit_Buff[1] = SPDR; // receive from the SPI bus, read from master
        SPDR = ccd_buff[1]; // send back ccd_buff[1]
    
        // 5-------
        asm volatile("nop");
        while (!(SPSR & _BV(SPIF))) ; // wait
        CCD_Transmit_Buff[2] = SPDR; // receive from the SPI bus, read from master
        SPDR = ccd_buff[2]; // send back ccd_buff[2]
    
        // 6-------
        asm volatile("nop");
        while (!(SPSR & _BV(SPIF))) ; // wait
        CCD_Transmit_Buff[3] = SPDR; //  receive from the SPI bus, read from master
        SPDR = ccd_buff[3]; // send back ccd_buff[3]
    
        // 7-------
        asm volatile("nop");
        while (!(SPSR & _BV(SPIF))) ; // wait
        CCD_Transmit_Buff[4] = SPDR; // receive from the SPI bus, read from master
        SPDR = ccd_buff[4]; // send back ccd_buff[4]
    
        // 8-------
        asm volatile("nop");
        while (!(SPSR & _BV(SPIF))) ; // wait
        CCD_Transmit_Buff[5] = SPDR; // receive from the SPI bus, read from master
        SPDR = ccd_buff[5]; // send back ccd_buff[5]
    
        // 9-------
        asm volatile("nop");
        while (!(SPSR & _BV(SPIF))) ; // wait
        CCD_Transmit_Buff[6] = SPDR; //  receive from the SPI bus, read from master
        SPDR = ccd_buff[6]; // send back ccd_buff[6]
    
        // 10-------
        asm volatile("nop");
        while (!(SPSR & _BV(SPIF))) ; // wait
        CCD_Transmit_Buff[7] = SPDR; // receive from the SPI bus, read from master
        SPDR = ccd_buff[7]; // send back ccd_buff[7]
    
        Set_INPUT(DDRB, PB4); // Set MISO to INPUT ( 3-state logic )
      }
    }

  19. #44
    Senior Member
    Join Date
    Dec 2013
    Posts
    214
    Update #4
    UBLOX v1.0.3
    Addition:
    Code:
     
      // CFG-TP5 (0x06 0x31) Time Pulse Parameters  https://forum.pjrc.com/threads/46058...-for-beginners
      SetCFG_TP5(uint32_t FreqLocked, double DutyLocked, uint16_t antCableDelay, bool printACK); // UBX-CFG-TP5 (0x06 0x31) Time Pulse 0 Parameters
    
      Ena_Dis_MON_IO(bool En~Di, bool)  // UBX-MON-IO (0x0A 0x02) Ena/Dis periodic auto update I/O Subsystem Status, bytes(received, sent), parity , framing , overrun)
      Poll_MON_IO(bool);                // Polls UBX-MON-IO (0x0A 0x02) I/O Subsystem Status
      Poll_MON_VER(bool);               // Polls UBX-MON-VER (0x0A 0x04) Receiver/Software Version
    v1.0.3
    UBLOX.cpp
    Code:
    /*
      UBLOX.cpp
      Brian R Taylor
      brian.taylor@bolderflight.com
      2016-11-03
    
      Copyright (c) 2016 Bolder Flight Systems
    
      Permission is hereby granted, free of charge, to any person obtaining a copy of this software
      and associated documentation files (the "Software"), to deal in the Software without restriction,
      including without limitation the rights to use, copy, modify, merge, publish, distribute,
      sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
      furnished to do so, subject to the following conditions:
    
      The above copyright notice and this permission notice shall be included in all copies or
      substantial portions of the Software.
    
      THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
      BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
      NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
      DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
      OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
    
      v1.0.3
      Chris O.
      2018-03-29
    
      Supported Navigation IDs:
      UBX-NAV-PVT ---- (0x01 0x07) Navigation Position Velocity Time Solution
      UBX-NAV-POSLLH - (0x01 0x02) Geodetic Position Solution
      UBX-NAV-ATT ---- (0x01 0x05) Attitude Solution
    
      /---  High level Commands, for the user ---/
       NOTE: command(bool) == (true)Print Message Acknowledged on USB Serial Monitor
      begin(Baud)                       // Starting communication with the GPS
      end();                            // Disables Teensy serial communication, to re-enable, call begin(Baud)
      Poll_CFG_Port1(bool);             // Polls the configuration for one I/O Port, I/O Target 0x01=UART1
      Poll_NAV_PVT();                   // Polls UBX-NAV-PVT    (0x01 0x07) Navigation Position Velocity Time Solution
      Poll_NAV_POSLLH();                // Polls UBX-NAV-POSLLH (0x01 0x02) Geodetic Position Solution
      Poll_NAV_ATT();                   // Polls UBX-NAV-ATT    (0x01 0x05) Attitude Solution
      Poll_MON_IO(bool);                // Polls UBX-MON-IO (0x0A 0x02) I/O Subsystem Status
      Poll_MON_VER(bool);               // Polls UBX-MON-VER (0x0A 0x04) Receiver/Software Version
      Poll_CFG_TP5(bool);               // Polls CFG-TP5        (0x06 0x31) Poll Time Pulse 0 Parameters
      ### Periodic Auto Update ON,OFF Command ###
      Ena_NAV_PVT(bool);                // Enable periodic auto update NAV_PVT
      Dis_NAV_PVT(bool);                // Disable periodic auto update NAV_PVT
      Ena_NAV_ATT(bool);                // Enable periodic auto update NAV_ATT
      Dis_NAV_ATT(bool);                // Disable periodic auto update NAV_ATT
      Ena_NAV_POSLLH(bool);             // Enable periodic auto update NAV_POSLLH
      Dis_NAV_POSLLH(bool);             // Disable periodic auto update NAV_POSLLH
      ### u-blox Switch off all NMEA MSGs ###
      Dis_all_NMEA_Child_MSGs(bool);    // Disable All NMEA Child Messages Command
      ### High level Command Generator ###
      SetGPSbaud(uint32_t baud, bool)   // Set UBLOX GPS Port Configuration Baud rate
      SetNAV5(uint8_t dynModel, bool)   // Set Dynamic platform model Navigation Engine Settings (0:portable, 3:pedestrian, Etc)
      SetRATE(uint16_t measRate, bool)  // Set Navigation/Measurement Rate Settings (100ms=10.00Hz, 200ms=5.00Hz, 1000ms=1.00Hz, Etc)
      SetCFG_TP5(uint32_t FreqLocked, double DutyLocked, uint16_t antCableDelay, bool printACK); // UBX-CFG-TP5 (0x06 0x31) Time Pulse 0 Parameters
      Ena_Dis_MON_IO(bool En~Di, bool) // UBX-MON-IO (0x0A 0x02) Ena/Dis periodic auto update I/O Subsystem Status, bytes(received, sent), parity , framing , overrun)
    */
    
    /* Note: Acknowledgment
      When messages from the Class CFG are sent to the receiver, the receiver will send an Acknowledge (ACK-ACK)
      or a Not Acknowledge (ACK-NAK) message back to the sender, depending on whether or not the message
      was processed correctly.
    */
    
    // Teensy 3.0 || Teensy 3.1/3.2 || Teensy 3.5 || Teensy 3.6 || Teensy LC
    #if defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MK64FX512__) || \
      defined(__MK66FX1M0__) || defined(__MKL26Z64__)
    
    #include "Arduino.h"
    #include "UBLOX.h"
    
    /* default constructor */
    UBLOX::UBLOX() {}
    
    /* uBlox object, input the serial bus */
    UBLOX::UBLOX(uint8_t bus) {
      _bus = bus; // serial bus
    }
    
    void UBLOX::configure(uint8_t bus) {
      _bus = bus; // serial bus
    }
    
    /* starts the serial communication */
    void UBLOX::begin(int baud) {
    
      // initialize parsing state
      _fpos = 0;
    
      // select the serial port
    #if defined(__MK20DX128__) || defined(__MK20DX256__) ||  defined(__MKL26Z64__) // Teensy 3.0 || Teensy 3.1/3.2 || Teensy LC
    
      if (_bus == 3) {
        _port = &Serial3;
      }
      else if (_bus == 2) {
        _port = &Serial2;
      }
      else {
        _port = &Serial1;
      }
    
    #endif
    
    #if defined(__MK64FX512__) || defined(__MK66FX1M0__) // Teensy 3.5 || Teensy 3.6
    
      if (_bus == 6) {
        _port = &Serial6;
      }
      else if (_bus == 5) {
        _port = &Serial5;
      }
      else if (_bus == 4) {
        _port = &Serial4;
      }
      else if (_bus == 3) {
        _port = &Serial3;
      }
      else if (_bus == 2) {
        _port = &Serial2;
      }
      else {
        _port = &Serial1;
      }
    
    #endif
    
      // begin the serial port for uBlox
      _port->begin(baud);
      delay(50); // wait for a bit, need for commands execution @ cold boot
      while (38 > _port->availableForWrite()) { }
      _port->write(_Poll_CFG_PRT, sizeof(_Poll_CFG_PRT));
      _port->flush();
    }
    // end the serial port for uBlox
    void UBLOX::end() {
      _port->flush(); // Waits for the transmission of outgoing serial data to complete
      _port->end();   // Disables serial communication
    }
    
    // # experimental Bridge between u-center and teensy UBLOX #
    ///>void UBLOX::USE_Ucenter(bool uc) {
    ///>  _use_ucenter = uc;
    ///>}
    
    // Generate the uBlox command configuration
    void UBLOX::writeCommand(uint8_t CLASS, uint8_t ID , uint8_t PayloadLen0, uint8_t PayloadLen1, uint8_t Identifier, uint8_t reserved0, uint8_t Par0, uint8_t Par1, uint8_t Par2, uint8_t Par3, uint32_t P32t0, uint32_t P32t1) const {
      const uint8_t HeaderLength = 2; // 2 synchronization characters: 0xB5 0x62.
      const uint8_t CLASSandID = 2;   // 1-byte Message Class field and 1-byte Message ID field
      const uint8_t LenF = 2; // 2-byte 16-bit Length field
      const uint8_t CRC = 2;  // 2-byte 16-bit checksum
      uint16_t Bit16PayloadLen = (PayloadLen1 << 8) | PayloadLen0; // Combining two uint8_t as uint16_t
      uint8_t DATA [Bit16PayloadLen] {}; // Set up DATA array with size of 16 bit payload length
    
      switch (CLASS) { //
        case 0x01:    // NAV 0x01
        case 0x02:    // RXM 0x02
        case 0x04:    // INF 0x04
        case 0x05:    // ACK 0x05
          Serial.println("Not Supported COMMAND CLASS:"), Serial.println(CLASS, HEX);
          break;
        case 0x06:    // CLASS CFG 0x06
    
          // ID CFG-PRT (0x00)
          if (ID == 0x00) { // ID CFG-PRT (0x06 0x00) Port Configuration
            if (Bit16PayloadLen == 0x14) { // Length (Bytes) DEC 20
              if (Identifier == 0x01) {   // Port Identifier Number uart1
                DATA [0] = 0x01;          // 0 U1 - portID
                DATA [1] = 0x00;          // 1 U1 - reserved1
                DATA [2] = 0x00;          // 2 X2 - txReady
                DATA [3] = 0x00;          //   X2 - txReady
                uint32_t GPSBaud = (((Par0 << 24) | (Par1 << 16)) | (Par2 << 8)) | Par3; // Combining
                if (GPSBaud == 9600) {    // 4 X4 - mode - A bit mask describing the UART mode
                  DATA [4] = 0xC0;        //        @9600=0xC0,   *b11000000‬
                } else { // NOT SURE HERE, ISSUE UBX-CFG-PRT (0x06 0x00) Bit Field mode ACCESSING UNDOCUMENTED BIT
                  DATA [4] = 0xD0;        //        @460800=0xD0, *b11010000‬ U-CENTER ISSUE ?
                }
                DATA [5] = 0x08;          //   X4 - mode
                DATA [6] = 0x00;          //   X4 - mode
                DATA [7] = 0x00;          //   X4 - mode
                DATA [8] = Par0;          // 8 U4 - 32bit baudRate Bits/s
                DATA [9] = Par1;          //        >baudRate uint32_t< DATA [8]-[9]-[10]-[11]
                DATA [10] = Par2;         //        Little Endian:     Hex:  00--08---07---00
                DATA [11] = Par3;         //        Big Endian:        Hex:  00--07---08---00 Decimal = **460800‬baud
                DATA [12] = 0x07;         // 12 X2 - inProtoMask
                DATA [13] = 0x00;         //    X2 - inProtoMask
                DATA [14] = 0x03;         // 14 X2 - outProtoMask
                DATA [15] = 0x00;         //    X2 - outProtoMask
                DATA [16] = 0x00;         // 16 X2 - flags
                DATA [17] = 0x00;         //    X2 - flags
                DATA [18] = 0x00;         // 18 U1 - reserved1
                DATA [19] = 0x00;         // 19 U1 - reserved2
              }
            }
          } // end UBX-CFG-PRT (0x06 0x00)
    
          if (ID == 0x01) {// UBX-CFG-MSG (0x06 0x01)
            // 0xB5 0x62 0x06 0x01 Length 3, Set periodic auto update message rate configuration for the current port
            if (Bit16PayloadLen == 0x03) { // Length (Bytes) DEC 3
              DATA [0] = Identifier;     // 0 U1 - msgClass - Message Class
              DATA [1] = reserved0;      // 1 U1 - msgID - Message Identifier
              DATA [2] = Par0;           // 2 U1 - rate - Send periodic auto update on-off rate on current Port
            }
          }// end UBX-CFG-MSG (0x06 0x01)
    
          // ID CFG-RATE (0x08)
          if (ID == 0x08) { // ID CFG-RATE (0x06 0x08) Navigation/Measurement Rate Settings, e.g. 100ms => 10Hz, 1000ms => 1Hz, 10000ms => 0.1Hz
            if (Bit16PayloadLen == 0x06) { // Length (Bytes) DEC 6
              DATA [0] = Identifier;     // 0  U2 - measRate ms
              DATA [1] = reserved0;      //    U2 - measRate
              DATA [2] = Par0;           // 2  U2 - navRate
              DATA [3] = Par1;           //    U2 - navRate
              DATA [4] = Par2;           // 4  U2 - timeRef
              DATA [5] = Par3;           //    U2 - timeRef
            }
          } // end UBX-CFG-RATE (0x06 0x08)
    
          // ID CFG-NAV5 (0x24)
          if (ID == 0x24) { // ID CFG-NAV5 (0x06 0x24) Navigation Engine Settings
            if (Bit16PayloadLen == 0x24) { // Length (Bytes) DEC 36
              DATA [0] = Identifier;     // 0  X2 - mask
              DATA [1] = reserved0;      //    X2 - mask
              DATA [2] = Par0;           // 2  U1 - dynModel
              DATA [3] = 0x00;           // 3  U1 - fixMode
              DATA [4] = 0x00;           // 4  I4 - fixedAlt, Scaling 0.01
              DATA [5] = 0x00;           //    I4 - fixedAlt
              DATA [6] = 0x00;           //    I4 - fixedAlt
              DATA [7] = 0x00;           //    I4 - fixedAlt
              DATA [8] = 0x00;           // 8  U4 - fixedAltVar, Scaling 0.0001
              DATA [9] = 0x00;           //    U4 - fixedAltVar
              DATA [10] = 0x00;          //    U4 - fixedAltVar
              DATA [11] = 0x00;          //    U4 - fixedAltVar
              DATA [12] = 0x00;          // 12 I1 - minElev
              DATA [13] = 0x00;          // 13 U1 - drLimit
              DATA [14] = 0x00;          // 14 U2 - pDop, Scaling 0.1
              DATA [15] = 0x00;          //    U2 - pDop
              DATA [16] = 0x00;          // 16 U2 - tDop, Scaling 0.1
              DATA [17] = 0x00;          //    U2 - tDop
              DATA [18] = 0x00;          // 18 U2 - pAcc
              DATA [19] = 0x00;          //    U2 - pAcc
              DATA [20] = 0x00;          // 20 U2 - tAcc
              DATA [21] = 0x00;          //    U2 - tAcc
              DATA [22] = 0x00;          // 22 U1 - staticHoldThresh
              DATA [23] = 0x00;          // 23 U1 - dgnssTimeout
              DATA [24] = 0x00;          // 24 U1 - cnoThreshNumSVs
              DATA [25] = 0x00;          // 25 U1 - cnoThresh
              DATA [26] = 0x00;          // 26 U1[2] - reserved1
              DATA [27] = 0x00;          //    U1 -    reserved1
              DATA [28] = 0x00;          // 28 U2 - staticHoldMaxDist
              DATA [29] = 0x00;          //    U2 - staticHoldMaxDist
              DATA [30] = 0x00;          // 30 U1 - utcStandard
              DATA [31] = 0x00;          // 31 U1[5] - reserved2
              DATA [32] = 0x00;          //    U1 -    reserved2
              DATA [33] = 0x00;          //    U1 -    reserved2
              DATA [34] = 0x00;          //    U1 -    reserved2
              DATA [35] = 0x00;          //    U1 -    reserved2
            }
          } // end UBX-CFG-NAV5 (0x06 0x24)
    
          // ID CFG-TP5 (0x31)
          // https://forum.pjrc.com/threads/46058-A-UBlox-GPS-Module-Primer-for-beginners
          if (ID == 0x31) { // ID CFG-TP5 (0x06 0x31) Time Pulse Parameters
            if (Bit16PayloadLen == 0x20) { // Length (Bytes) DEC 32
              uint8_t FreqLock3 = ((P32t0 & 0xFF000000) >> 24); // P32t0 FreqLocked
              uint8_t FreqLock2 = ((P32t0 & 0x00FF0000) >> 16);
              uint8_t FreqLock1 = ((P32t0 & 0x0000FF00) >> 8);
              uint8_t FreqLock0 =  (P32t0 & 0x000000FF);
              uint8_t DutyLocked3 = ((P32t1 & 0xFF000000) >> 24); // P32t1 Duty Locked %
              uint8_t DutyLocked2 = ((P32t1 & 0x00FF0000) >> 16);
              uint8_t DutyLocked1 = ((P32t1 & 0x0000FF00) >> 8);
              uint8_t DutyLocked0 =  (P32t1 & 0x000000FF);
              DATA [0] = Identifier;     // 0  U1 - tpIdx
              DATA [1] = reserved0;      // 1  U1 - version
              DATA [2] = 0x00;           // 2  U1[2] - reserved1
              DATA [3] = 0x00;           //    U1 -    reserved1
              DATA [4] = Par0;           // 4  I2 - antCableDelay, 0~32767, default 50ns
              DATA [5] = Par1;           //    I2 - antCableDelay
              DATA [6] = 0x00;           // 6  I2 - rfGroupDelay, default 0ns
              DATA [7] = 0x00;           //    I2 - rfGroupDelay
              DATA [8] = 0x01;           // 8  U4 - freqPeriod, default 1Hz or us
              DATA [9] = 0x00;           //    U4 - freqPeriod
              DATA [10] = 0x00;          //    U4 - freqPeriod
              DATA [11] = 0x00;          //    U4 - freqPeriod
              DATA [12] = FreqLock0;     // 12 U4 - freqPeriodLock, 24000000Hz ~ 1Hz or us
              DATA [13] = FreqLock1;     //    U4 - freqPeriodLock
              DATA [14] = FreqLock2;     //    U4 - freqPeriodLock
              DATA [15] = FreqLock3;     //    U4 - freqPeriodLock
              DATA [16] = 0x00;          // 16 U4 - pulseLenRatio, default 0us_or_2^-32
              DATA [17] = 0x00;          //    U4 - pulseLenRatio
              DATA [18] = 0x00;          //    U4 - pulseLenRatio
              DATA [19] = 0x00;          //    U4 - pulseLenRatio
              DATA [20] = DutyLocked0;   // 20 U4 - pulseLenRatioLock, us_or_2^-32
              DATA [21] = DutyLocked1;   //    U4 - pulseLenRatioLock,(100% 4294967295)
              DATA [22] = DutyLocked2;   //    U4 - pulseLenRatioLock
              DATA [23] = DutyLocked3;   //    U4 - pulseLenRatioLock
              DATA [24] = 0x00;          // 24 I4 - userConfigDelay, default 0ns
              DATA [25] = 0x00;          //    I4 - userConfigDelay
              DATA [26] = 0x00;          //    I4 - userConfigDelay
              DATA [27] = 0x00;          //    I4 - userConfigDelay
              DATA [28] = 0xEF;          // 28 X4 - flags, 0xEF *BIN-11101111‬
              DATA [29] = 0x00;          //    X4 - flags
              DATA [30] = 0x00;          //    X4 - flags
              DATA [31] = 0x00;          //    X4 - flags
            }
          } // end UBX-CFG-TP5 (0x06 0x31)
    
          break;
        case 0x0A:    // MON 0x0A
        case 0x0B:    // AID 0x0B
        case 0x0D:    // TIM 0x0D
        case 0x10:    // ESF 0x10
        case 0x13:    // MGA 0x13
        case 0x21:    // LOG 0x21
        case 0x27:    // SEC 0x27
        case 0x28:    // HNR 0x28
        //break;
        default:
          // Not Supported
          Serial.println("Not Supported COMMAND CLASS:"), Serial.println(CLASS, HEX);
          break;
      }
      // The checksum is calculated over the packet, starting and including the
      // CLASS field, ID field and payload, up until, but excluding, the Checksum Field
      // (CRC) Compute checksum
      uint8_t CK_A = 0, CK_B = 0;
      CK_A = CK_A + CLASS;
      CK_B = CK_B + CK_A;
      CK_A = CK_A + ID;
      CK_B = CK_B + CK_A;
      CK_A = CK_A + PayloadLen0;
      CK_B = CK_B + CK_A;
      CK_A = CK_A + PayloadLen1;
      CK_B = CK_B + CK_A;
      for (int i = 0; i < Bit16PayloadLen ; i++) {
        CK_A = CK_A + DATA[i];
        CK_B = CK_B + CK_A;
      }
      // Generate the command
      if (Bit16PayloadLen == 0x03) { // Payload Length 0x03 ~ DEC 3
        uint8_t COMMpayload[HeaderLength + CLASSandID + LenF + Bit16PayloadLen + CRC] = {
          /* Header */
          0xB5,           //[ 0] synchronization characters: 0xB5 0x62
          0x62,           //[ 1]
          CLASS,          //[ 2] Destination,
          ID,             //[ 3]
          PayloadLen0,    //[ 4] 8bit Payload Length
          PayloadLen1,    //[ 5] 8bit Payload Length
          (DATA [0]),     //[ 6] Payload HEX 0x06 ~ DEC 6
          (DATA [1]),     //[ 7]
          (DATA [2]),     //[ 8]
          CK_A,           //[12] CRC 16-bit
          CK_B            //[13]
        };
        _port->flush();
        _port->write(COMMpayload, sizeof(COMMpayload));
      }
      if (Bit16PayloadLen == 0x06) { // Payload Length 0x06 ~ DEC 6
        uint8_t COMMpayload[HeaderLength + CLASSandID + LenF + Bit16PayloadLen + CRC] = {
          /* Header */
          0xB5,           //[ 0] synchronization characters: 0xB5 0x62
          0x62,           //[ 1]
          CLASS,          //[ 2] Destination,
          ID,             //[ 3]
          PayloadLen0,    //[ 4] 8bit Payload Length
          PayloadLen1,    //[ 5] 8bit Payload Length
          (DATA [0]),     //[ 6] Payload HEX 0x06 ~ DEC 6
          (DATA [1]),     //[ 7]
          (DATA [2]),     //[ 8]
          (DATA [3]),     //[ 9]
          (DATA [4]),     //[10]
          (DATA [5]),     //[11]
          CK_A,           //[12] CRC 16-bit
          CK_B            //[13]
        };
        _port->flush();
        _port->write(COMMpayload, sizeof(COMMpayload));
      }
      if (Bit16PayloadLen == 0x14) { // Payload Length 0x14 ~ DEC 20
        uint8_t COMMpayload[HeaderLength + CLASSandID + LenF + Bit16PayloadLen + CRC] = {
          /* Header */
          0xB5,           //[ 0] synchronization characters: 0xB5 0x62
          0x62,           //[ 1]
          CLASS,          //[ 2] Destination
          ID,             //[ 3]
          PayloadLen0,    //[ 4] 8bit Payload Length
          PayloadLen1,    //[ 5] 8bit Payload Length
          (DATA [0]),     //[ 6] Payload HEX 14 ~ DEC 20
          (DATA [1]),     //[ 7]
          (DATA [2]),     //[ 8]
          (DATA [3]),     //[ 9]
          (DATA [4]),     //[10]
          (DATA [5]),     //[11]
          (DATA [6]),     //[12]
          (DATA [7]),     //[13]
          (DATA [8]),     //[14]
          (DATA [9]),     //[15]
          (DATA [10]),    //[16]
          (DATA [11]),    //[17]
          (DATA [12]),    //[18]
          (DATA [13]),    //[19]
          (DATA [14]),    //[20]
          (DATA [15]),    //[21]
          (DATA [16]),    //[22]
          (DATA [17]),    //[23]
          (DATA [18]),    //[24]
          (DATA [19]),    //[25]
          CK_A,           //[26] CRC 16-bit
          CK_B            //[27]
        };
        _port->flush();
        _port->write(COMMpayload, sizeof(COMMpayload));
      }
      if (Bit16PayloadLen == 0x20) { // Payload Length 0x20 ~ DEC 32
        uint8_t COMMpayload[HeaderLength + CLASSandID + LenF + Bit16PayloadLen + CRC] = {
          /* Header */
          0xB5,           //[ 0] synchronization characters: 0xB5 0x62
          0x62,           //[ 1]
          CLASS,          //[ 2] Destination
          ID,             //[ 3]
          PayloadLen0,    //[ 4] 8bit Payload Length
          PayloadLen1,    //[ 5] 8bit Payload Length
          (DATA [0]),     //[ 6] Payload HEX 20 ~ DEC 32
          (DATA [1]),     //[ 7]
          (DATA [2]),     //[ 8]
          (DATA [3]),     //[ 9]
          (DATA [4]),     //[10]
          (DATA [5]),     //[11]
          (DATA [6]),     //[12]
          (DATA [7]),     //[13]
          (DATA [8]),     //[14]
          (DATA [9]),     //[15]
          (DATA [10]),    //[16]
          (DATA [11]),    //[17]
          (DATA [12]),    //[18]
          (DATA [13]),    //[19]
          (DATA [14]),    //[20]
          (DATA [15]),    //[21]
          (DATA [16]),    //[22]
          (DATA [17]),    //[23]
          (DATA [18]),    //[24]
          (DATA [19]),    //[25]
          (DATA [20]),    //[26]
          (DATA [21]),    //[27]
          (DATA [22]),    //[28]
          (DATA [23]),    //[29]
          (DATA [24]),    //[30]
          (DATA [25]),    //[31]
          (DATA [26]),    //[32]
          (DATA [27]),    //[33]
          (DATA [28]),    //[34]
          (DATA [29]),    //[35]
          (DATA [30]),    //[36]
          (DATA [31]),    //[37]
          CK_A,           //[38] CRC 16-bit
          CK_B            //[39]
        };
        _port->flush();
        _port->write(COMMpayload, sizeof(COMMpayload));
      }
      if (Bit16PayloadLen == 0x24) { // Payload Length 0x24 ~ DEC 36
        uint8_t COMMpayload[HeaderLength + CLASSandID + LenF + Bit16PayloadLen + CRC] = {
          /* Header */
          0xB5,           //[ 0] synchronization characters: 0xB5 0x62
          0x62,           //[ 1]
          CLASS,          //[ 2] Destination
          ID,             //[ 3]
          PayloadLen0,    //[ 4] 8bit Payload Length
          PayloadLen1,    //[ 5] 8bit Payload Length
          (DATA [0]),     //[ 6] Payload HEX 24 ~ DEC 36
          (DATA [1]),     //[ 7]
          (DATA [2]),     //[ 8]
          (DATA [3]),     //[ 9]
          (DATA [4]),     //[10]
          (DATA [5]),     //[11]
          (DATA [6]),     //[12]
          (DATA [7]),     //[13]
          (DATA [8]),     //[14]
          (DATA [9]),     //[15]
          (DATA [10]),    //[16]
          (DATA [11]),    //[17]
          (DATA [12]),    //[18]
          (DATA [13]),    //[19]
          (DATA [14]),    //[20]
          (DATA [15]),    //[21]
          (DATA [16]),    //[22]
          (DATA [17]),    //[23]
          (DATA [18]),    //[24]
          (DATA [19]),    //[25]
          (DATA [20]),    //[26]
          (DATA [21]),    //[27]
          (DATA [22]),    //[28]
          (DATA [23]),    //[29]
          (DATA [24]),    //[30]
          (DATA [25]),    //[31]
          (DATA [26]),    //[32]
          (DATA [27]),    //[33]
          (DATA [28]),    //[34]
          (DATA [29]),    //[35]
          (DATA [30]),    //[36]
          (DATA [31]),    //[37]
          (DATA [32]),    //[38]
          (DATA [33]),    //[39]
          (DATA [34]),    //[40]
          (DATA [35]),    //[41]
          CK_A,           //[42] CRC 16-bit
          CK_B            //[43]
        };
        _port->flush(); // Waits for the transmission of outgoing serial data to complete
        _port->write(COMMpayload, sizeof(COMMpayload));
      }
    }
    
    /* write the uBlox data */
    /******************************************/
    /**  High level commands, for the user    */
    /******************************************/
    
    // UBX-CFG-PRT (0x06 0x00) SetGPSbaud
    // Generate the configuration command's syntax
    void UBLOX::SetGPSbaud(uint32_t baud32rate, bool printACK) {
      bool Supported = false;
      if ( printACK == true ) {
        Serial.print("Setting CFG-PRT (0x06 0x00) GPS UART baud rate:");
        _pACK2 = true; // print ACK to USB
      }
      // Set GPS Uart1 baud rate (32bit)
      // Possible Configurations
      switch (baud32rate) {
        case 4800:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("4800");
          }
          break;
        case 9600:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("9600");
          }
          break;
        case 19200:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("19200");
          }
          break;
        case 38400:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("38400");
          }
          break;
        case 57600:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("57600");
          }
          break;
        case 115200:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("115200");
          }
          break;
        case 230400:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("230400");
          }
          break;
        case 460800:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("460800");
          }
          break;
        case 921600:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("921600");
          }
          break;
        case 1500000: // experimental baud rate 1500000
          Supported = true;
          if ( printACK == true ) {
            Serial.println("1500000");
          }
          break;
        default:
          Supported = false;
          _pACK2 = false;
          Serial.print("\tNot Supported :"), Serial.println(baud32rate), Serial.println("\tPossible Baud Rate Configurations");
          Serial.println("\t4800~9600~19200~38400~57600~115200~230400~460800~921600~1500000<experimental"), Serial.println();
          break;
      }
      if ( Supported == true ) {
        // Splitting an uint32_t baud32rate
        uint8_t Par3 = ((baud32rate & 0xFF000000) >> 24); // Parameter3
        uint8_t Par2 = ((baud32rate & 0x00FF0000) >> 16);
        uint8_t Par1 = ((baud32rate & 0x0000FF00) >> 8);
        uint8_t Par0 =  (baud32rate & 0x000000FF);
        uint8_t CLASS = 0x06, ID = 0x00, Length0 = 0x14, Length1 = 0x00, Identifier = 0x01, res0 = 0; // reserved0 -- NOT IN USE
        // Identifier Port # Electrical Interface
        // 0 DDC (IC compatible)
        // 1 UART 1
        // 3 USB
        // 4 SPI
        uint32_t P32t0 = 0, P32t1 = 0; // not used for this command
        writeCommand(CLASS, ID, Length0, Length1, Identifier, res0, Par0, Par1, Par2, Par3, P32t0, P32t1);
      }
    }
    
    // UBX-CFG-RATE (0x06 0x08)  Set Navigation/Measurement Rate Settings (100ms=10.00Hz, 200ms=5.00Hz, 1000ms=1.00Hz, Etc)
    // Generate the configuration command's syntax
    void UBLOX::SetRATE(uint16_t measRate, bool printACK) {
      bool Supported = false;
      if ( printACK == true ) {
        Serial.print("Setting CFG-RATE (0x06 0x08) Navigation/Measurement Rate ");
        _pACK_RATE = true; // print ACK to USB
      }
      // Possible Configurations
      // _______________________________________________________________
      // 60ms    16.67Hz <-- | 192ms    5.21Hz     | 600ms    1.67Hz
      // 64ms    15.63Hz <-- | 200ms    5.00Hz <-- | 625ms    1.60Hz
      // 72ms    13.89Hz <-- | 225ms    4.44Hz     | 640ms    1.56Hz
      // 75ms    13.33Hz <-- | 240ms    4.17Hz     | 720ms    1.39Hz
      // 80ms    12.50Hz <-- | 250ms    4.00Hz <-- | 750ms    1.33Hz
      // 90ms    11.11Hz     | 288ms    3.47Hz     | 800ms    1.25Hz <--
      // 96ms    10.42Hz     | 300ms    3.33Hz     | 900ms    1.11Hz
      // 100ms   10.00Hz <-- | 320ms    3.13Hz     | 960ms    1.04Hz
      // 120ms    8.33Hz     | 360ms    2.78Hz     | 1000ms   1.00Hz <--
      // 125ms    8.00Hz <-- | 375ms    2.67Hz     | 2000ms   0.50Hz <--
      // 128ms    7.81Hz     | 400ms    2.50Hz <-- | 4000ms   0.25Hz <--
      // 144ms    6.94Hz     | 450ms    2.22Hz     | 10000ms  0.10Hz <--
      // 150ms    6.67Hz     | 480ms    2.08Hz     | 20000ms  0.05Hz <--
      // 160ms    6.25Hz     | 500ms    2.00Hz <-- | 50000ms  0.02Hz <--
      // 180ms    5.56Hz     | 576ms    1.74Hz     |
      // NOTE: Only rate settings marked with arrows are the implemented configuration.
      switch (measRate) {
        case 60:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("60ms 16.67Hz");
          }
          break;
        case 64:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("64ms 15.63Hz");
          }
          break;
        case 72:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("72ms 13.89Hz");
          }
          break;
        case 80:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("80ms 12.50Hz");
          }
          break;
        case 100:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("100ms 10.00Hz");
          }
          break;
        case 125:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("125ms 8.00Hz");
          }
          break;
        case 200:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("200ms 5.00Hz");
          }
          break;
        case 250:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("250ms 4.00Hz");
          }
          break;
        case 400:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("400ms 2.50Hz");
          }
          break;
        case 500:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("500ms 2.00Hz");
          }
          break;
        case 800:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("800ms 1.25Hz");
          }
          break;
        case 1000:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("1000ms 1.00Hz");
          }
          break;
        case 2000:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("2000ms 0.50Hz");
          }
          break;
        case 4000:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("4000ms 0.25Hz");
          }
          break;
        case 10000:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("10000ms 0.10Hz");
          }
          break;
        case 20000:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("20000ms 0.05Hz");
          }
          break;
        case 50000:
          Supported = true;
          if ( printACK == true ) {
            Serial.println("50000ms 0.02Hz");
          }
          break;
        default:
          Supported = false;
          _pACK_RATE = false;
          Serial.print("\tNot Supported :"), Serial.println(measRate), Serial.println("\tPossible Configurations");
          Serial.println("\t60=16.67Hz 64=15.63Hz 72=13.89Hz 80=12.50Hz 100=10.00Hz 125=8.00Hz 200=5.00Hz 250=4.00Hz 500=2.00Hz");
          Serial.println("\t800=1.25Hz 1000=1.00Hz 2000=0.50Hz 4000=0.25Hz 10000=0.10Hz 20000=0.05Hz 50000=0.02Hz "), Serial.println();
          break;
      }
      if ( Supported == true ) {
        // Splitting an uint16_t measRate
        // Identifier & res0 - 16bit Parameter defines the rate. e.g. 100ms => 10Hz, 1000ms => 1Hz, 10000ms => 0.1Hz.
        uint8_t res0 = ((measRate & 0xFF00) >> 8);
        uint8_t Identifier =  (measRate & 0x00FF);
        uint8_t CLASS = 0x06, ID = 0x08, Length0 = 0x06, Length1 = 0x00;
        uint8_t Par0 = 0x01, Par1 = 0x00, Par2 = 0x01, Par3 = 0x00; // navRate and timeRef Parameters.
        uint32_t P32t0 = 0, P32t1 = 0; // not used for this command
        writeCommand(CLASS, ID, Length0, Length1, Identifier, res0, Par0, Par1, Par2, Par3, P32t0, P32t1);
      }
    }
    
    // UBX-CFG-NAV5 (0x06 0x24) - Set Navigation Engine Settings
    // Generate the configuration command's syntax
    void UBLOX::SetNAV5(uint8_t dynModel, bool printACK) {
      bool Supported = false;
      uint8_t Par0 = 0;
      if ( printACK == true ) {
        Serial.print("Setting CFG-NAV5 (0x06 0x24) Dynamic platform model ");
        _pACK_NAV5 = true;
      }
      // Possible Configurations Dynamic platform model:
      // 0: portable
      // 2: stationary
      // 3: pedestrian
      // 4: automotive
      // 5: sea
      // 6: airborne with <1g acceleration
      // 7: airborne with <2g acceleration
      // 8: airborne with <4g acceleration
      // 9: wrist worn watch (not supported in protocol versions less than 18)
      switch (dynModel) {
        case 0:
          Supported = true;
          Par0 = 0;        // Parameter0
          if ( printACK == true ) {
            Serial.println("0: portable");
          }
          break;
        case 2:
          Supported = true;
          Par0 = 2;
          if ( printACK == true ) {
            Serial.println("2: stationary");
          }
          break;
        case 3:
          Supported = true;
          Par0 = 3;
          if ( printACK == true ) {
            Serial.println("3: pedestrian");
          }
          break;
        case 4:
          Supported = true;
          Par0 = 4;
          if ( printACK == true ) {
            Serial.println("4: automotive");
          }
          break;
        case 5:
          Supported = true;
          Par0 = 5;
          if ( printACK == true ) {
            Serial.println("5: sea");
          }
          break;
        case 6:
          Supported = true;
          Par0 = 6;
          if ( printACK == true ) {
            Serial.println("6: airborne with <1g acceleration");
          }
          break;
        case 7:
          Supported = true;
          Par0 = 7;
          if ( printACK == true ) {
            Serial.println("7: airborne with <2g acceleration");
          }
          break;
        case 8:
          Supported = true;
          Par0 = 8;
          if ( printACK == true ) {
            Serial.println("8: airborne with <4g acceleration");
          }
          break;
        case 9:
          Supported = true;
          Par0 = 9;
          if ( printACK == true ) {
            Serial.println("9: wrist worn watch (not supported in protocol Ver. less than 18");
          }
          break;
        default:
          Supported = false;
          _pACK_NAV5 = false;
          Serial.print("\tNot Supported :"), Serial.println(dynModel), Serial.println("\tPossible Configurations");
          Serial.println("\t0: portable 2: stationary 3: pedestrian 4: automotive 5: sea 6: airborne with <1g 7: airborne with <2g");
          Serial.println("\t8: airborne with <4g 9: wrist worn watch (not supported in protocol v.less than 18)"), Serial.println();
          break;
      }
      if ( Supported == true ) {
        // Identifier & res0 - 16bit Parameter bitmask. Only the masked parameters will be applied.
        uint8_t CLASS = 0x06, ID = 0x24, Length0 = 0x24, Length1 = 0x00, Identifier = B00000001, res0 = B00000000; // reserved0
        uint8_t Par1 = 0, Par2 = 0, Par3 = 0; // Parameters, Par0 set in switch case.
        uint32_t P32t0 = 0, P32t1 = 0; // not used for this command
        writeCommand(CLASS, ID, Length0, Length1, Identifier, res0, Par0, Par1, Par2, Par3, P32t0, P32t1);
      }
    }
    
    // UBX-CFG-TP5 (0x06 0x31) - Set Time Pulse Parameters for Time Pulse 0
    // Generate the configuration command's syntax
    void UBLOX::SetCFG_TP5(uint32_t FreqLocked, double DutyLocked, uint16_t antCableDelay, bool printACK) {
      bool Supported = false;
      uint8_t Par1 = 0;
      uint8_t Par0 = 0;
      uint32_t P32t0 = 0; // FreqLocked
      uint32_t P32t1 = 0; // DutyLocked
    
      if (( FreqLocked >= 1 && FreqLocked <= 24000000) && ( DutyLocked >= 0.000000 && DutyLocked <= 100) && (antCableDelay >= 0 && antCableDelay <= 32767)) {
        P32t0 = FreqLocked; // FreqLocked, range 24000000Hz ~ 1Hz.
        // arduino "map" function and large numbers won't fit in a 32-bit long integer, math overflow
        // rolling my own "map" function
        double out_max = 4294967296; // 0x*100000000‬
        double out_min = 0;
        double in_max = 100;
        double in_min = 0;
        double RetX = 0;
        RetX = ((DutyLocked - in_min) * (out_max - out_min) / (in_max - in_min) + out_min);
        if (RetX == 4294967296) {
          RetX = RetX - 1; // 4294967295
          Serial.print("\t(limit) ");
        }
        P32t1 = RetX ; // DutyLocked, range 0 ~ 4294967295(0~100%)
    
        // Par0, Par1 - 16bit Parameter defines the Antenna cable delay, MIN 0 MAX 32767, default 50ns Antenna cable delay
        Par1 = ((antCableDelay & 0xFF00) >> 8);
        Par0 = (antCableDelay & 0x00FF);
        Supported = true;
        if ( printACK == true ) {
          Serial.print("Setting CFG-TP5 (0x06 0x31) Time Pulse 0 Parameters"), Serial.print("\t(FreqLocked) "), Serial.print(FreqLocked);
          Serial.print("Hz  (DutyLocked) "), Serial.print(DutyLocked, 6), Serial.print("% map("), Serial.print(P32t1);
          Serial.print(")  (antCableDelay) "), Serial.print(antCableDelay), Serial.println("ns");
          _pACK_TP5 = true;
        }
      } else {
        Supported = false;
        Serial.println("\tSetting CFG-TP5 (0x06 0x31) Time Pulse 0 Parameters issue");
        Serial.print("\tNot Supported :"), Serial.println("\tPossible Configurations");
        Serial.println("\tSetCFG_TP5(FreqLocked- 1Hz ~ 24000000Hz, DutyLocked- 0.000000% ~ 100.000000%, antCableDelay- 0~32767ns, print usb ACK- true or false);");
        Serial.println();
      }
    
      uint8_t Par2 = 0, Par3 = 0; // Parameters not used for this command
      if ( Supported == true ) {
        // Identifier ~ Time pulse selection(0 = TIMEPULSE 0), res0 ~ Message version (1 for this version)
        uint8_t CLASS = 0x06, ID = 0x31, Length0 = 0x20, Length1 = 0x00, Identifier = 0, res0 = 1;
        writeCommand(CLASS, ID, Length0, Length1, Identifier, res0, Par0, Par1, Par2, Par3, P32t0, P32t1);
      }
    }
    
    void UBLOX::Poll_CFG_TP5(bool printACK) {
      // UBX-CFG-TP5 (0x06 0x31) Poll Time Pulse Parameters
      while (12 > _port->availableForWrite()) { }
      _port->write(_Poll_CFG_TP5, sizeof(_Poll_CFG_TP5));
      _port->flush();
      if ( printACK == true ) {
        Serial.println(" Poll_CFG_TP5, Time Pulse 0 Parameters");
        _pACK_TP5 = true;
      }
    }
    void UBLOX::Poll_GPSbaud_Port1(bool printACK) {
      // CFG-PRT (0x06 0x00) Polls the configuration for one I/O Port, (I/O Target # MSG) 0x01=I/O UART1 0x06001
      while (38 > _port->availableForWrite()) { }
      _port->write(_Poll_CFG_PRT, sizeof(_Poll_CFG_PRT));
      _port->flush();
      if ( printACK == true ) {
        _printACK = 0x06001; // USB - Clas 06, Id 00, print ACK I/O UART1 0x06001
        //parse(); // force reading serial on cold boot
      }
    }
    void UBLOX::Poll_MON_IO(bool printACK) { // Poll-MON-IO (0x0A 0x02) I/O Subsystem Status
      while (12 > _port->availableForWrite()) { }
      _port->write(_Poll_MON_IO, sizeof(_Poll_MON_IO));
      _port->flush();
      if ( printACK == true ) {
        Serial.println(" Poll_MON_IO, I/O Subsystem Status");
      }
    }
    void UBLOX::Poll_MON_VER(bool printACK) { // Poll-MON-VER (0x0A 0x04) Receiver/Software Version
      while (12 > _port->availableForWrite()) { }
      _port->write(_Poll_MON_VER, sizeof(_Poll_MON_VER));
      _port->flush();
      if ( printACK == true ) {
        Serial.println(" Poll_MON_VER, Receiver/Software Version");
      }
    }
    void UBLOX::Poll_NAV_PVT() {
      while (12 > _port->availableForWrite()) { }
      _port->write(_Poll_NAV_PVT, sizeof(_Poll_NAV_PVT));
    }
    void UBLOX::Poll_NAV_POSLLH() {
      while (12 > _port->availableForWrite()) { }
      _port->write(_Poll_NAV_POSLLH, sizeof(_Poll_NAV_POSLLH));
    }
    void UBLOX::Poll_NAV_ATT() {
      _port->write(_Poll_NAV_ATT, sizeof(_Poll_NAV_ATT));
    }
    // #### Periodic auto update ON,OFF command ####
    void UBLOX::Ena_NAV_PVT(bool printACK) { // Enable periodic auto update NAV_PVT
      while (12 > _port->availableForWrite()) { }
      _port->write(_Ena_NAV_PVT, sizeof(_Ena_NAV_PVT));
      if ( printACK == true ) {
        Serial.println("Setting Ena_NAV_PVT");
        _printACK_PVT = 0x06011; // Clas 06, Id 01, Ena 1 - print ACK Ena_NAV_PVT 0x06011
      }
    }
    void UBLOX::Dis_NAV_PVT(bool printACK) { // Disable periodic auto update NAV_PVT
      while (12 > _port->availableForWrite()) { }
      _port->write(_Dis_NAV_PVT, sizeof(_Dis_NAV_PVT));
      if ( printACK == true ) {
        Serial.println("Setting Dis_NAV_PVT");
        _printACK_PVT = 0x06010; // Clas 06, Id 01, Dis 0 - print ACK Dis_NAV_PVT 0x06010
      }
    }
    void UBLOX::Ena_NAV_ATT(bool printACK) { // Enable periodic auto update NAV_ATT
      while (12 > _port->availableForWrite()) { }
      _port->write(_Ena_NAV_ATT, sizeof(_Ena_NAV_ATT));
      if ( printACK == true ) {
        Serial.println("Setting Ena_NAV_ATT");
        _printACK_ATT = 0x06011; // Clas 06, Id 01, Ena 1
      } else {
        _printACK_ATT = 0xFF; // need for ACK_NAK, NAV_ATT Supported only on protocol version 19 (only with ADR or UDR products)
      }
    }
    void UBLOX::Dis_NAV_ATT(bool printACK) { // Disable periodic auto update NAV_ATT
      while (12 > _port->availableForWrite()) { }
      _port->write(_Dis_NAV_ATT, sizeof(_Dis_NAV_ATT));
      if ( printACK == true ) {
        Serial.println("Setting Dis_NAV_ATT");
        _printACK_ATT = 0x06010; // Clas 06, Id 01, Dis 0
      } else {
        _printACK_ATT = 0xFF;
      }
    }
    void UBLOX::Ena_NAV_POSLLH(bool printACK) { // Enable periodic auto update NAV_POSLLH
      while (12 > _port->availableForWrite()) { }
      _port->write(_Ena_NAV_POSLLH, sizeof(_Ena_NAV_POSLLH));
      if ( printACK == true ) {
        Serial.println("Setting Ena_NAV_POSLLH");
        _printACK_POSLLH = 0x06011; // Clas 06, Id 01, Ena 1
      }
    }
    void UBLOX::Dis_NAV_POSLLH(bool printACK) { // Disable periodic auto update NAV_POSLLH
      while (12 > _port->availableForWrite()) { }
      _port->write(_Dis_NAV_POSLLH, sizeof(_Dis_NAV_POSLLH));
      if ( printACK == true ) {
        Serial.println("Setting Dis_NAV_POSLLH");
        _printACK_POSLLH = 0x06010; // Clas 06, Id 01, Dis 0
      }
    }
    // Generate the configuration command's syntax
    void UBLOX::Ena_Dis_MON_IO(bool on_off, bool printACK) {
      // UBX-CFG-MSG (0x06 0x01)
      // Set periodic auto update message rate configuration for UBX-MON-IO (0x0A 0x02) I/O Subsystem Status
      if ( printACK == true ) {
        if ( on_off == true ) {
          _print_I_O = 3;
          Serial.print("Setting Ena_MON_IO");
        } else {
          _print_I_O = 2;
          Serial.print("Setting Dis_MON_IO");
        }
        Serial.println(" (0x0A 0x02) I/O Subsystem Status");
      }
      uint8_t CLASS = 0x06, ID = 0x01, Length0 = 0x03, Length1 = 0x00, Identifier = 0x0A, res0 = 0x02;
      uint8_t Par1 = 0, Par2 = 0, Par3 = 0;
      uint32_t P32t0 = 0, P32t1 = 0; // not used for this command
      writeCommand(CLASS, ID, Length0, Length1, Identifier, res0, on_off, Par1, Par2, Par3, P32t0, P32t1);
    }
    // #### u-blox Disable All NMEA Child Messages Command ####
    void UBLOX::Dis_all_NMEA_Child_MSGs(bool printACK) {
      if ( printACK == true ) {
        // Ensure any pending writes are complete before writing new data
        while ( 38 >= _port->availableForWrite() ) {} // make sure we have at least 39 bytes available in TX buffer on start
        /*int wr;
          wr = _port->availableForWrite();
          if (wr <= 39) {
          Serial.print(" Buffer Available For Write only=");
          Serial.println(wr);
          Serial.print(" Please edit Teensyduino Core file serial"), Serial.print(_bus), Serial.println(".c, look for #define SERIAL#_TX_BUFFER_SIZE  40 // number of outgoing bytes to buffer");
          Serial.println(" This Command (Dis_all_NMEA_Child_MSGs(true) - needs 165 bytes of TX BUFFER) recommended TX buffer of 200");
          Serial.println(" I also strongly suggest changing RX buffer to 255 or more,*otherwise you will miss ACKs*on serial monitor");
          }*/
    
        _printACK_Dis_all_NMEA = 0x06010; // print ACK to USB - Clas 06, Id 01, print ACK Dis_all_NMEA 0x0601
        Serial.println("Dis_all_NMEA_Child_MSGs");
        Serial.println("Setting Dis_NMEA_GxDTM");
        Serial.println("Setting Dis_NMEA_GxGBS");
        Serial.println("Setting Dis_NMEA_GxGGA");
        Serial.println("Setting Dis_NMEA_GxGLL");
        Serial.println("Setting Dis_NMEA_GxGNS");
        Serial.println("Setting Dis_NMEA_GxGRS");
        Serial.println("Setting Dis_NMEA_GxGSA");
        Serial.println("Setting Dis_NMEA_GxGST");
        Serial.println("Setting Dis_NMEA_GxGSV");
        Serial.println("Setting Dis_NMEA_GxRMC");
        // Serial.println("Setting Dis_NMEA_GxVLW");   // u-blox6 -- Message Not-Acknowledged
        Serial.println("Setting Dis_NMEA_GxVTG");
        Serial.println("Setting Dis_NMEA_GxZDA");
        Serial.println("Setting Dis_PUBXe_NMEA00");
        // Serial.println("Setting Dis_PUBXe_NMEA01"); // u-blox6 -- Message Not-Acknowledged
        Serial.println("Setting Dis_PUBXe_NMEA03");
        Serial.println("Setting Dis_PUBXe_NMEA04");
        // Serial.println("Setting Dis_PUBXe_NMEA05"); // u-blox6 -- Message Not-Acknowledged
        // Serial.println("Setting Dis_PUBXe_NMEA06"); // u-blox6 -- Message Not-Acknowledged
      } else {
        _printACK_Dis_all_NMEA = 0x00;
      }
      _ACKcount = 15; // Set NMEA ACK counter #
      while (12 > _port->availableForWrite()) {} // check if the hardware serial port has room for outgoing bytes to TX buffer
      _port->write(_Dis_NMEA_GxDTM, sizeof(_Dis_NMEA_GxDTM));             //#1  / NMEA GxDTM (DTM 0xF0 0x0A) Datum Reference)
      while (12 > _port->availableForWrite()) {}
      _port->write(_Dis_NMEA_GxGBS, sizeof(_Dis_NMEA_GxGBS));             //#2  / NMEA GxGBS (GBS 0xF0 0x09) GNSS Satellite Fault Detection)
      while (12 > _port->availableForWrite()) {}
      _port->write(_Dis_NMEA_GxGGA, sizeof(_Dis_NMEA_GxGGA));             //#3  / NMEA GxGGA (GGA 0xF0 0x00) Global positioning system fix data)
      while (12 > _port->availableForWrite()) {}
      _port->write(_Dis_NMEA_GxGLL, sizeof(_Dis_NMEA_GxGLL));             //#4  / NMEA GxGLL (GLL 0xF0 0x01) Latitude and longitude, with time of position fix and status)
      while (12 > _port->availableForWrite()) {}
      _port->write(_Dis_NMEA_GxGNS, sizeof(_Dis_NMEA_GxGNS));             //#5  / NMEA GxGNS (GNS 0xF0 0x0D) GNSS fix data)
      while (12 > _port->availableForWrite()) {}
      _port->write(_Dis_NMEA_GxGRS, sizeof(_Dis_NMEA_GxGRS));             //#6  / NMEA GxGRS (GRS 0xF0 0x06) GNSS Range Residuals)
      while (12 > _port->availableForWrite()) {}
      _port->write(_Dis_NMEA_GxGSA, sizeof(_Dis_NMEA_GxGSA));             //#7  / NMEA GxGSA (GSA 0xF0 0x02) GNSS DOP and Active Satellites)
      while (12 > _port->availableForWrite()) {}
      _port->write(_Dis_NMEA_GxGST, sizeof(_Dis_NMEA_GxGST));             //#8  / NMEA GxGST (GST 0xF0 0x07) GNSS Pseudo Range Error Statistics)
      while (12 > _port->availableForWrite()) {}
      _port->write(_Dis_NMEA_GxGSV, sizeof(_Dis_NMEA_GxGSV));             //#9  / NMEA GxGSV (GSV 0xF0 0x03) GNSS Satellites in View)
      while (12 > _port->availableForWrite()) {}
      _port->write(_Dis_NMEA_GxRMC, sizeof(_Dis_NMEA_GxRMC));             //#10 / NMEA GxRMC (RMC 0xF0 0x04) Recommended Minimum data)
      while (12 > _port->availableForWrite()) {}
      // u-blox6 -- Message Not-Acknowledged
      //_port->write(_Dis_NMEA_GxVLW, sizeof(_Dis_NMEA_GxVLW));           //    / NMEA GxVLW (VLW 0xF0 0x0F) Dual ground/water distance)
      _port->write(_Dis_NMEA_GxVTG, sizeof(_Dis_NMEA_GxVTG));             //#11 / NMEA GxVTG (VTG 0xF0 0x05) Course over ground and Ground speed)
      while (12 > _port->availableForWrite()) {}
      _port->write(_Dis_NMEA_GxZDA, sizeof(_Dis_NMEA_GxZDA));             //#12 / NMEA GxZDA (ZDA 0xF0 0x08) Time and Date)
      while (12 > _port->availableForWrite()) {}
      // #### PUBX- u-blox NMEA extension - Switch OFF ALL ####
      _port->write(_Dis_PUBXe_NMEA00, sizeof(_Dis_PUBXe_NMEA00));         //#13 / PUBX 00 (Position Data)
      while (12 > _port->availableForWrite()) {}
      // u-blox6 -- Message Not-Acknowledged
      //_port->write(_Dis_PUBXe_NMEA01, sizeof(_Dis_PUBXe_NMEA01));       //    / PUBX 01 (UTM Position Data)
      _port->write(_Dis_PUBXe_NMEA03, sizeof(_Dis_PUBXe_NMEA03));         //#14 / PUBX 03 (Satellite Data)
      while (12 > _port->availableForWrite()) {}
      _port->write(_Dis_PUBXe_NMEA04, sizeof(_Dis_PUBXe_NMEA04));         //#15 / PUBX 04 (Time of Day)
      while (12 > _port->availableForWrite()) {}
      // u-blox6 -- Message Not-Acknowledged
      //_port->write(_Dis_PUBXe_NMEA05, sizeof(_Dis_PUBXe_NMEA05));       //    / PUBX 05 (EKF Status)
      //_port->write(_Dis_PUBXe_NMEA06, sizeof(_Dis_PUBXe_NMEA06));       //    / PUBX 06 (GPS-only on EKF products)
    }
    
    /* read the uBlox data */
    bool UBLOX::read(gpsData *gpsData_ptr) {
    
      const double mm2m = 1.0e-3;
      const double en7 = 1.0e-7;
      const double en5 = 1.0e-5;
      const double en2 = 1.0e-2;
    
      union {
        unsigned long val;
        uint8_t b[4];
      } iTOW;
    
      union {
        unsigned short val;
        uint8_t b[2];
      } utcYear;
    
      union {
        unsigned long val;
        uint8_t b[4];
      } tAcc;
    
      union {
        long val;
        uint8_t b[4];
      } utcNano;
    
      union {
        long val; // I4
        uint8_t b[4];
      } lon;
    
      union {
        long val;
        uint8_t b[4];
      } lat;
    
      union {
        long val;
        uint8_t b[4];
      } height;
    
      union {
        long val;
        uint8_t b[4];
      } hMSL;
    
      union {
        unsigned long val;
        uint8_t b[4];
      } hAcc;
    
      union {
        unsigned long val;
        uint8_t b[4];
      } vAcc;
    
      union {
        long val;
        uint8_t b[4];
      } velN;
    
      union {
        long val;
        uint8_t b[4];
      } velE;
    
      union {
        long val;
        uint8_t b[4];
      } velD;
    
      union {
        long val;
        uint8_t b[4];
      } gSpeed;
    
      union {
        long val;
        uint8_t b[4];
      } heading;
    
      union {
        unsigned long val;
        uint8_t b[4];
      } sAcc;
    
      union {
        unsigned long val;
        uint8_t b[4];
      } headingAcc;
    
      union {
        unsigned short val;
        uint8_t b[2];
      } pDOP;
    
      union {
        unsigned long val;
        uint8_t b[4];
      } headVeh;
    
      union {
        signed short val;
        uint8_t b[2];
      } magDec;
    
      union {
        unsigned short val;
        uint8_t b[2];
      } magAcc;
    
      // UBX-NAV-ATT (0x01 0x05) Attitude Solution
      union {
        long val;
        uint8_t b[4];
      } roll;
    
      union {
        long val;
        uint8_t b[4];
      } pitch;
    
      union {
        long val;
        uint8_t b[4];
      } accRoll;
    
      union {
        long val;
        uint8_t b[4];
      } accPitch;
    
      union {
        long val;
        uint8_t b[4];
      } accHeading;
    
      // Current GPS Uart1 Baud
      union {
        uint32_t val;
        uint8_t b[4];
      } GpsUart1Baud;
    
      // parse the uBlox packet
      if (parse()) {
        // uBlox Message (Class, ID) # Description #
        // UBX-NAV-PVT   (0x01 0x07) Navigation Position Velocity Time Solution
        if (_UBX_NAV_PVT_ID == true) {
          iTOW.b[0] = _gpsPayload[4];
          iTOW.b[1] = _gpsPayload[5];
          iTOW.b[2] = _gpsPayload[6];
          iTOW.b[3] = _gpsPayload[7];
          gpsData_ptr->iTOW = iTOW.val;
    
          utcYear.b[0] = _gpsPayload[8];
          utcYear.b[1] = _gpsPayload[9];
          gpsData_ptr->utcYear = utcYear.val;
    
          gpsData_ptr->utcMonth = _gpsPayload[10];
          gpsData_ptr->utcDay = _gpsPayload[11];
          gpsData_ptr->utcHour = _gpsPayload[12];
          gpsData_ptr->utcMin = _gpsPayload[13];
          gpsData_ptr->utcSec = _gpsPayload[14];
          gpsData_ptr->valid = _gpsPayload[15];
    
          tAcc.b[0] = _gpsPayload[16];
          tAcc.b[1] = _gpsPayload[17];
          tAcc.b[2] = _gpsPayload[18];
          tAcc.b[3] = _gpsPayload[19];
          gpsData_ptr->tAcc = tAcc.val;
    
          utcNano.b[0] = _gpsPayload[20];
          utcNano.b[1] = _gpsPayload[21];
          utcNano.b[2] = _gpsPayload[22];
          utcNano.b[3] = _gpsPayload[23];
          gpsData_ptr->utcNano = utcNano.val;
    
          gpsData_ptr->fixType = _gpsPayload[24];
          gpsData_ptr->flags = _gpsPayload[25];
          gpsData_ptr->flags2 = _gpsPayload[26];
          gpsData_ptr->numSV = _gpsPayload[27];
    
          lon.b[0] = _gpsPayload[28];
          lon.b[1] = _gpsPayload[29];
          lon.b[2] = _gpsPayload[30];
          lon.b[3] = _gpsPayload[31];
          gpsData_ptr->lon = lon.val * en7;
    
          lat.b[0] = _gpsPayload[32];
          lat.b[1] = _gpsPayload[33];
          lat.b[2] = _gpsPayload[34];
          lat.b[3] = _gpsPayload[35];
          gpsData_ptr->lat = lat.val * en7;
    
          height.b[0] = _gpsPayload[36];
          height.b[1] = _gpsPayload[37];
          height.b[2] = _gpsPayload[38];
          height.b[3] = _gpsPayload[39];
          gpsData_ptr->height = height.val * mm2m;
    
          hMSL.b[0] = _gpsPayload[40];
          hMSL.b[1] = _gpsPayload[41];
          hMSL.b[2] = _gpsPayload[42];
          hMSL.b[3] = _gpsPayload[43];
          gpsData_ptr->hMSL = hMSL.val * mm2m;
    
          hAcc.b[0] = _gpsPayload[44];
          hAcc.b[1] = _gpsPayload[45];
          hAcc.b[2] = _gpsPayload[46];
          hAcc.b[3] = _gpsPayload[47];
          gpsData_ptr->hAcc = hAcc.val * mm2m;
    
          vAcc.b[0] = _gpsPayload[48];
          vAcc.b[1] = _gpsPayload[49];
          vAcc.b[2] = _gpsPayload[50];
          vAcc.b[3] = _gpsPayload[51];
          gpsData_ptr->vAcc = vAcc.val * mm2m;
    
          velN.b[0] = _gpsPayload[52];
          velN.b[1] = _gpsPayload[53];
          velN.b[2] = _gpsPayload[54];
          velN.b[3] = _gpsPayload[55];
          gpsData_ptr->velN = velN.val * mm2m;
    
          velE.b[0] = _gpsPayload[56];
          velE.b[1] = _gpsPayload[57];
          velE.b[2] = _gpsPayload[58];
          velE.b[3] = _gpsPayload[59];
          gpsData_ptr->velE = velE.val * mm2m;
    
          velD.b[0] = _gpsPayload[60];
          velD.b[1] = _gpsPayload[61];
          velD.b[2] = _gpsPayload[62];
          velD.b[3] = _gpsPayload[63];
          gpsData_ptr->velD = velD.val * mm2m;
    
          gSpeed.b[0] = _gpsPayload[64];
          gSpeed.b[1] = _gpsPayload[65];
          gSpeed.b[2] = _gpsPayload[66];
          gSpeed.b[3] = _gpsPayload[67];
          gpsData_ptr->gSpeed = gSpeed.val * mm2m;
    
          heading.b[0] = _gpsPayload[68];
          heading.b[1] = _gpsPayload[69];
          heading.b[2] = _gpsPayload[70];
          heading.b[3] = _gpsPayload[71];
          gpsData_ptr->heading = heading.val * en5;
    
          sAcc.b[0] = _gpsPayload[72];
          sAcc.b[1] = _gpsPayload[73];
          sAcc.b[2] = _gpsPayload[74];
          sAcc.b[3] = _gpsPayload[75];
          gpsData_ptr->sAcc = sAcc.val  * mm2m;
    
          headingAcc.b[0] = _gpsPayload[76];
          headingAcc.b[1] = _gpsPayload[77];
          headingAcc.b[2] = _gpsPayload[78];
          headingAcc.b[3] = _gpsPayload[79];
          gpsData_ptr->headingAcc = headingAcc.val * en5;
    
          pDOP.b[0] = _gpsPayload[80];
          pDOP.b[1] = _gpsPayload[81];
          gpsData_ptr->pDOP = pDOP.val * 0.01L;
    
          headVeh.b[0] = _gpsPayload[88];
          headVeh.b[1] = _gpsPayload[89];
          headVeh.b[2] = _gpsPayload[90];
          headVeh.b[3] = _gpsPayload[91];
          gpsData_ptr->headVeh = headVeh.val * en5;
    
          // Edit - 2018 2 25 magDec, magAcc
          magDec.b[0] = _gpsPayload[92];
          magDec.b[0] = _gpsPayload[93];
          gpsData_ptr->magDec = magDec.val * en2;
    
          magAcc.b[0] = _gpsPayload[94];
          magAcc.b[0] = _gpsPayload[95];
          gpsData_ptr->magDec = magAcc.val * en2;
          _UBX_NAV_PVT_ID = false;
          // return true on receiving a full packet
          return true;
        }
    
        // UBX-NAV-POSLLH (0x01 0x02) Geodetic Position Solution
        if (_UBX_NAV_POSLLH_ID == true) {
          iTOW.b[0] = _gpsPayload[4]; // GPS time of week of the navigation epoch.
          iTOW.b[1] = _gpsPayload[5];
          iTOW.b[2] = _gpsPayload[6];
          iTOW.b[3] = _gpsPayload[7];
          gpsData_ptr->iTOW = iTOW.val;
    
          lon.b[0] = _gpsPayload[8]; // Longitude
          lon.b[1] = _gpsPayload[9];
          lon.b[2] = _gpsPayload[10];
          lon.b[3] = _gpsPayload[11];
          gpsData_ptr->lon = lon.val * en7;
    
          lat.b[0] = _gpsPayload[12]; // Latitude
          lat.b[1] = _gpsPayload[13];
          lat.b[2] = _gpsPayload[14];
          lat.b[3] = _gpsPayload[15];
          gpsData_ptr->lat = lat.val * en7;
    
          height.b[0] = _gpsPayload[16]; // Height above ellipsoid
          height.b[1] = _gpsPayload[17];
          height.b[2] = _gpsPayload[18];
          height.b[3] = _gpsPayload[19];
          gpsData_ptr->height = height.val * mm2m;
    
          hMSL.b[0] = _gpsPayload[20]; // Height above mean sea level
          hMSL.b[1] = _gpsPayload[21];
          hMSL.b[2] = _gpsPayload[22];
          hMSL.b[3] = _gpsPayload[23];
          gpsData_ptr->hMSL = hMSL.val * mm2m;
    
          hAcc.b[0] = _gpsPayload[24]; // Horizontal accuracy estimate
          hAcc.b[1] = _gpsPayload[25];
          hAcc.b[2] = _gpsPayload[26];
          hAcc.b[3] = _gpsPayload[27];
          gpsData_ptr->hAcc = hAcc.val * mm2m;
    
          _UBX_NAV_POSLLH_ID = false;
          // return true on receiving a full packet
          return true;
        }
    
        // UBX-NAV-ATT (0x01 0x05) Attitude Solution
        if (_UBX_NAV_ATT_ID == true) {
          iTOW.b[0] = _gpsPayload[4]; // GPS time of week of the navigation epoch.
          iTOW.b[1] = _gpsPayload[5];
          iTOW.b[2] = _gpsPayload[6];
          iTOW.b[3] = _gpsPayload[7];
          gpsData_ptr->iTOW = iTOW.val;
    
          gpsData_ptr->version = _gpsPayload[8]; // Message version (0 for this version)
    
          // U1[3] - reserved1 - Reserved
          // gpsData_ptr->reserved1 = _gpsPayload[9];
          // gpsData_ptr->reserved2 = _gpsPayload[10];
          // gpsData_ptr->reserved3 = _gpsPayload[11];
    
          roll.b[0] = _gpsPayload[12]; // Vehicle roll.
          roll.b[1] = _gpsPayload[13];
          roll.b[2] = _gpsPayload[14];
          roll.b[3] = _gpsPayload[15];
          gpsData_ptr->roll = roll.val * en5;
    
          pitch.b[0] = _gpsPayload[16]; // Vehicle pitch.
          pitch.b[1] = _gpsPayload[17];
          pitch.b[2] = _gpsPayload[18];
          pitch.b[3] = _gpsPayload[19];
          gpsData_ptr->pitch = pitch.val * en5;
    
          heading.b[0] = _gpsPayload[20]; // heading
          heading.b[1] = _gpsPayload[21];
          heading.b[2] = _gpsPayload[22];
          heading.b[3] = _gpsPayload[23];
          gpsData_ptr->heading = heading.val * en5;
    
          accRoll.b[0] = _gpsPayload[24]; // Vehicle roll accuracy (if null, roll angle is not available).
          accRoll.b[1] = _gpsPayload[25];
          accRoll.b[2] = _gpsPayload[26];
          accRoll.b[3] = _gpsPayload[27];
          gpsData_ptr->accRoll = accRoll.val * en5;
    
          accPitch.b[0] = _gpsPayload[28]; // Vehicle pitch accuracy (if null, pitch angle is not available).
          accPitch.b[1] = _gpsPayload[29];
          accPitch.b[2] = _gpsPayload[30];
          accPitch.b[3] = _gpsPayload[31];
          gpsData_ptr->accPitch = accPitch.val * en5;
    
          accHeading.b[0] = _gpsPayload[32]; // Vehicle heading accuracy (if null, heading angle is not available).
          accHeading.b[1] = _gpsPayload[33];
          accHeading.b[2] = _gpsPayload[34];
          accHeading.b[3] = _gpsPayload[35];
          gpsData_ptr->accHeading = accHeading.val * en5;
          _UBX_NAV_ATT_ID = false;
          // return true on receiving a full packet
          return true;
        }
    
        if (_UBX_CFG_ID == true) {
          uint16_t MSGpayload = (_gpsPayload[3] << 8) | _gpsPayload[2]; // Combining two uint8_t as uint16_t
          uint32_t DCL0 = 0;
          float DCL1 = 0;
    
          switch (_gpsPayload[1]) {  // UBX-CFG (0x06) Class
            case 0x00: // UBX-CFG-PRT (0x06 0x00) Port Configuration for UART
              switch (MSGpayload) { // 16Bit-Length Field, Decide what to do based on payload size, length in decimal format
                case 1:  // UBX-CFG-PRT 0x06 0x00 Poll Request Polls the configuration for one I/O Port , len[1]
                  Serial.print("Poll Request Polls the configuration for one I/O Port:"), Serial.println(_gpsPayload[4]), Serial.println(); // USB debug print
                  break;
                case 20: // UBX-CFG-PRT 0x06 0x00 Get/Set Port Configuration for UART-USB-SPI-DDC(IC) Port, len[20]
                  // Decide what to do based on _gpsPayload:[4] Payload Content-I/O portID: 0=DDC(I2C), 1=UART1, 2=UART2, 3=USB, 4=SPI, 5=reserved
                  if (_gpsPayload[4] == 1) { // 1=UART1
                    GpsUart1Baud.b[0] = _gpsPayload[12];
                    GpsUart1Baud.b[1] = _gpsPayload[13];
                    GpsUart1Baud.b[2] = _gpsPayload[14];
                    GpsUart1Baud.b[3] = _gpsPayload[15];
                    gpsData_ptr->GpsUart1Baud = GpsUart1Baud.val;
                    // need this for ACK_ACK print
                    _GpsUartBaud = (((_gpsPayload[15] << 24) | (_gpsPayload[14] << 16)) | (_gpsPayload[13] << 8)) | _gpsPayload[12]; // Combining
                  } else {
                    Serial.print("I/O portID: 0=DDC(I2C), 2=UART2, 3=USB, 4=SPI, 5=reserved "), Serial.print(_gpsPayload[4]), Serial.println(" Not implemented"), Serial.println();
                  }
                  break;
                default:
                  // Not Supported
                  Serial.print("Unknown CFG PRT Length Field:"), Serial.println((_MSGpayloadSize) - 4, DEC), Serial.println();
                  break;
              } //end UBX-CFG-PRT (0x06 0x00)
              break;
            case 0x31: // UBX-CFG-TP5 (0x06 0x31) Time Pulse Parameters
              gpsData_ptr->antCableDelay   = (_gpsPayload[9] << 8) | _gpsPayload[8]; // 0~32767, default 50ns
              gpsData_ptr->freqPeriodL = (((_gpsPayload[19] << 24) | (_gpsPayload[18] << 16)) | (_gpsPayload[17] << 8)) | _gpsPayload[16]; // 24000000Hz ~ 1Hz
              DCL0 = (((_gpsPayload[27] << 24) | (_gpsPayload[26] << 16)) | (_gpsPayload[25] << 8)) | _gpsPayload[24]; // (100%=4294967295)
              // re map duty cycle locked %
              // arduino "map" function and large numbers won't fit in a long integer, math overflow
              // rolling my own "map" function
              DCL1 = modifiedMap(DCL0, 0, 4294967295, -0.125, 99.875);
              gpsData_ptr->dutycycleL = DCL1;
              break;
            default:
              // Not Supported
              Serial.print("Unknown CFG Class:0x"), Serial.println((_gpsPayload[1]), HEX), Serial.println();
              break;
          }
          _UBX_CFG_ID = false;
          // return false even though its packets are successfully being received, not a NAV packet
          return false;
        }
    
        if (_UBX_ACK_ACK_ID == true) {
          uint8_t CC = 0; // use only one if statement
          switch (_gpsPayload[4]) { // UBX_ACK_ACK(0x05 0x01) response to Class CFG 0x06
            case 0x06:   // CFG 0x06 Configuration Input Messages: Set Dynamic Model, Set DOP Mask, Set Baud Rate, etc
              switch (_gpsPayload[5]) {    // ### Name ID Description ###  UBX_ACK_ACK response to ID
                case 0x00: // CFG-PRT 0x06 0x00:
                  if ((_pACK2 == 1) || (_printACK == 0x06001)) {
                    Serial.print("UBX_ACK_ACK:"), Serial.print(" Message Acknowledged -");
                    if ((_printACK == 0x06001) && CC == 0) {
                      CC = 1;
                      Serial.print(" Poll_GPSbaud_Port1 baudRate:"), Serial.println(_GpsUartBaud);
                      _printACK = 0x00;
                    }
                    if ((_pACK2 == 1) && CC == 0) {
                      CC = 1;
                      Serial.println(" CFG-PRT (0x06 0x00) Set GPS UART baud");
                      _pACK2 = 0;
                    }
                  } else {
                    if ((_pACK2 == 0) || (_printACK == 0x00)) {
                      // Do Nothing Here
                    } else { // print all Unknown CFG ACK on USB
                      Serial.print("UBX_ACK_ACK: CFG-PRT 0x06 0x00"), Serial.print(" Message Acknowledged "), Serial.println();
                    }
                  }
                  break;
    
                case 0x01:   // CFG-MSG 0x06 0x01: Set Message periodic auto update Rate(s)
                  if ((_printACK_Dis_all_NMEA == 0x06010) && CC == 0) { // Dis_all_NMEA_Child_MSGs 0x06010
                    CC = 1;
                    //Serial.print("UBX_ACK_ACK:"), Serial.print(" Message Acknowledged -");
                    Serial.print("UBX_ACK_ACK:"), Serial.print(" Message Acknowledged - ");
                    Serial.print("Dis_all_NMEA ");
                    Serial.print("-CFG-MSG(0x06 0x01): Set disable All NMEA "), Serial.println(_ACKcount);
                    _ACKcount = _ACKcount - 1;
                    if (_ACKcount == 0) { // NMEA ACK counter
                      _printACK_Dis_all_NMEA = 0x00; // reset
                      //Serial.println();
                    }
                  }
                  if ((_printACK_ATT == 0x06011 || _printACK_ATT == 0x06010) && CC == 0) {
                    CC = 1;
                    Serial.print("UBX_ACK_ACK:"), Serial.print(" Message Acknowledged - ");
                    if (_printACK_ATT == 0x06011) { // Ena_NAV_ATT 0x06011
                      Serial.print("Ena_NAV_ATT "), Serial.println("-Enable periodic auto update");
                      _printACK_ATT = 0x01; // 01
                    }
                    if (_printACK_ATT == 0x06010) { // Dis_NAV_ATT 0x06010
                      Serial.print("Dis_NAV_ATT "), Serial.println("-Disable periodic auto update");
                      _printACK_ATT = 0x02; // 01
                    }
                  }
                  if ((_printACK_PVT == 0x06011 || _printACK_PVT == 0x06010) && CC == 0) {
                    CC = 1;
                    Serial.print("UBX_ACK_ACK:"), Serial.print(" Message Acknowledged - ");
                    if (_printACK_PVT == 0x06011) { // Ena_NAV_PVT 0x06011
                      Serial.print("Ena_NAV_PVT "), Serial.println("-Enable periodic auto update");
                      _printACK_PVT = 0x00;
                    }
                    if (_printACK_PVT == 0x06010) { // Dis_NAV_PVT 0x06010
                      Serial.print("Dis_NAV_PVT "), Serial.println("-Disable periodic auto update");
                      _printACK_PVT = 0x00;
                    }
                  }
                  if ((_printACK_POSLLH == 0x06011 || _printACK_POSLLH == 0x06010) && CC == 0) {
                    CC = 1;
                    Serial.print("UBX_ACK_ACK:"), Serial.print(" Message Acknowledged - ");
                    if (_printACK_POSLLH == 0x06011) { // Ena_NAV_POSLLH 0x06011
                      Serial.print("Ena_NAV_POSLLH "), Serial.println("-Enable periodic auto update");
                      _printACK_POSLLH = 0x00;
                    }
                    if (_printACK_POSLLH == 0x06010) { // Dis_NAV_POSLLH 0x06010
                      Serial.print("Dis_NAV_POSLLH "), Serial.println("-Disable periodic auto update");
                      _printACK_POSLLH = 0x00;
                    }
                  }
                  if ((_print_I_O == 3 || _print_I_O == 2) && CC == 0) {
                    CC = 1;
                    Serial.print("UBX_ACK_ACK:"), Serial.print(" Message Acknowledged - ");
                    if (_print_I_O == 3) {
                      Serial.print("Ena_MON_IO "), Serial.println("-Enable periodic auto update");
                      _print_I_O = 0;
                    }
                    if (_print_I_O == 2) {
                      Serial.print("Dis_MON_IO "), Serial.println("-Disable periodic auto update");
                      _print_I_O = 0;
                    }
                  }
                  break;
                case 0x02:    // CFG-INF 0x06 0x02:       Poll configuration for one protocol or Set Information message configuration
                  Serial.println("-CFG-INF 0x06 0x02"), Serial.println();
                  break;
                case 0x06:    // CFG-DAT 0x06 0x06:       Set User-defined Datum or Get The currently defined Datum
                  Serial.println("-CFG-DAT 0x06 0x06"), Serial.println();
                  break;
                case 0x09:    // CFG-CFG 0x06 0x09:       Command Clear, Save and Load configurations
                  Serial.println("-CFG-CFG 0x06 0x09"), Serial.println();
                  break;
                case 0x08:    // CFG-RATE 0x06 0x08:      Get/Set Navigation/Measurement Rate Settings
                  if (_pACK_RATE == true) {
                    Serial.print("UBX_ACK_ACK:"), Serial.print(" Message Acknowledged ");
                    Serial.println("- CFG-RATE (0x06 0x08) Get/Set Navigation/Measurement Rate Settings");
                    _pACK_RATE = false;
                  }
                  break;
                case 0x11:    // CFG-RXM 0x06 0x11:       Get/Set RXM configuration
                  Serial.println("-CFG-RXM 0x06 0x11"), Serial.println();
                  break;
                case 0x13:    // CFG-ANT 0x06 0x13:       Get/Set Antenna Control Settings
                  Serial.println("-CFG-ANT 0x06 0x13"), Serial.println();
                  break;
                case 0x16:    // CFG-SBAS 0x06 0x16:      Get/Set SBAS Configuration
                  Serial.println("-CFG-SBAS 0x06 0x16"), Serial.println();
                  break;
                case 0x17:    // CFG-NMEA 0x06 0x17:      Get/Set NMEA protocol configuration, V0 (deprecated) or Extended NMEA protocol configuration V1
                  Serial.println("-CFG-NMEA 0x06 0x17"), Serial.println();
                  break;
                case 0x1B:    // CFG-USB 0x06 0x1B:       Get/Set USB Configuration
                  Serial.println("-CFG-USB 0x06 0x1B"), Serial.println();
                  break;
                case 0x1E:    // CFG-ODO 0x06 0x1E:       Get/Set Odometer, Low-speed COG Engine Settings
                  Serial.println("-CFG-ODO 0x06 0x1E"), Serial.println();
                  break;
                case 0x23:    // CFG-NAVX5 0x06 0x23:     Get/Set Navigation Engine Expert Settings
                  Serial.println("-CFG-NAVX5 0x06 0x23"), Serial.println();
                  break;
                case 0x24:    // CFG-NAV5 0x06 0x24:      Get/Set Navigation Engine Settings
                  if (_pACK_NAV5 == true) {
                    Serial.print("UBX_ACK_ACK:"), Serial.print(" Message Acknowledged ");
                    Serial.println("- CFG-NAV5 (0x06 0x24) Set Navigation Engine Settings");
                    _pACK_NAV5 = false;
                  }
                  break;
                case 0x31:    // CFG-TP5 0x06 0x31:       Poll Time Pulse Parameters for Time Pulse 0, Poll Time Pulse Parameters or Get/Set Time Pulse Parameters
                  if (_pACK_TP5 == true) {
                    Serial.print("UBX_ACK_ACK:"), Serial.print(" Message Acknowledged ");
                    Serial.println("- CFG-TP5 (0x06 0x31) Set Time Pulse 0 Parameters");
                    _pACK_TP5 = false;
                  }
                  break;
                case 0x34:    // CFG-RINV 0x06 0x34       Get/Set Contents of Remote Inventory
                  Serial.println("-CFG-RINV 0x06 0x34"), Serial.println();
                  break;
                case 0x39:    // CFG-ITFM 0x06 0x39:      Get/Set Jamming/Interference Monitor configuration
                  Serial.println("-CFG-ITFM 0x06 0x39"), Serial.println();
                  break;
                case 0x3B:    // CFG-PM2 0x06 0x3B:       Get/Set Extended Power Management configuration
                  Serial.println("-CFG-PM2 0x06 0x3B"), Serial.println();
                  break;
                case 0x3D:    // CFG-TMODE2 0x06 0x3D:    Get/Set Time Mode Settings 2
                  Serial.println("-CFG-TMODE2 0x06 0x3D"), Serial.println();
                  break;
                case 0x3E:    // CFG-GNSS 0x06 0x3E:      Get/Set GNSS system configuration
                  Serial.println("-CFG-GNSS 0x06 0x3E"), Serial.println();
                  break;
                case 0x47:    // CFG-LOGFILTER 0x06 0x47: Get/Set Data Logger Configuration
                  Serial.println("-CFG-LOGFILTER 0x06 0x47"), Serial.println();
                  break;
                case 0x53:    // CFG-TXSLOT 0x06 0x53:    Set TX buffer time slots configuration
                  Serial.println("-CFG-TXSLOT 0x06 0x53"), Serial.println();
                  break;
                case 0x57:    // CFG-PWR 0x06 0x57:       Set Put receiver in a defined power state.
                  Serial.println("-CFG-PWR 0x06 0x57"), Serial.println();
                  break;
                case 0x5C:    // CFG-HNR 0x06 0x5C:       Get/Set High Navigation Rate Settings ### u-blox 8 only ###
                  Serial.println("-CFG-HNR 0x06 0x5C"), Serial.println();
                  break;
                case 0x60:    // CFG-ESRC 0x06 0x60:      Get/Set External synchronization source configuration
                  Serial.println("-CFG-ESRC 0x06 0x60"), Serial.println();
                  break;
                case 0x61:    // CFG-DOSC 0x06 0x61:      Get/Set Disciplined oscillator configuration
                  Serial.println("-CFG-DOSC 0x06 0x61"), Serial.println();
                  break;
                case 0x62:    // CFG-SMGR 0x06 0x62:      Get/Set Synchronization manager configuration
                  Serial.println("-CFG-SMGR 0x06 0x62"), Serial.println();
                  break;
                case 0x69:    // CFG-GEOFENCE 0x06 0x69:  Get/Set Geofencing configuration
                  Serial.println("-CFG-GEOFENCE 0x06 0x69"), Serial.println();
                  break;
                case 0x70:    // CFG-DGNSS 0x06 0x70:     Get/Set DGNSS configuration
                  Serial.println("-CFG-DGNSS 0x06 0x70"), Serial.println();
                  break;
                case 0x71:    // CFG-TMODE3 0x06 0x71:    Get/Set Time Mode Settings 3
                  Serial.println("-CFG-TMODE3 0x06 0x71"), Serial.println();
                  break;
                case 0x84:    // CFG-FIXSEED 0x06 0x84:   Set Programming the fixed seed for host...
                  Serial.println("-CFG-FIXSEED 0x06 0x84"), Serial.println();
                  break;
                case 0x85:    // CFG-DYNSEED 0x06 0x85:   Set Programming the dynamic seed for the host...
                  Serial.println("-CFG-DYNSEED 0x06 0x85"), Serial.println();
                  break;
                case 0x86:    // CFG-PMS 0x06 0x86:       Get/Set Power Mode Setup
                  Serial.println("-CFG-PMS 0x06 0x86"), Serial.println();
                  break;
                default:
                  // Not Supported
                  Serial.print("Unknown ACK CFG response to ID:");
                  if (_gpsPayload[5] > 16) {
                    Serial.print("0x");
                  } else {
                    Serial.print("0x0");
                  }
                  Serial.println(_gpsPayload[5], HEX), Serial.println();
                  break;
              }
              break;
            default:
              // Not Supported
              Serial.print("Unknown ACK ACK response to Class:"), Serial.println((_gpsPayload[4]), HEX), Serial.println();
              break;
          }
          _UBX_ACK_ACK_ID = false;
          // return false even though its packets are successfully being received, not a NAV packet
          return false;
        }
    
        // UBX Class MON (0x0A)
        if (_UBX_MON_ID == true) {
          switch (_gpsPayload[1]) {  // UBX MON IDs
            case 0x02:    // UBX-MON-IO (0x0A 0x02) I/O Subsystem Status
              // u-blox 7 = I/O Subsystem Status Payload Length:120
              // 1st 20bytes=I2C _gpsPayload[4], 2nd 20bytes=UART1 _gpsPayload[24], next 20bytes=UART2 Etc.
              gpsData_ptr->rxBytes = (((_gpsPayload[27] << 24) | (_gpsPayload[26] << 16)) | (_gpsPayload[25] << 8)) | _gpsPayload[24]; // Combining
              gpsData_ptr->txBytes = (((_gpsPayload[31] << 24) | (_gpsPayload[30] << 16)) | (_gpsPayload[29] << 8)) | _gpsPayload[28];
              gpsData_ptr->parityErrs = (_gpsPayload[33] << 8) | _gpsPayload[32]; // Combining two uint8_t as uint16_t
              gpsData_ptr->framingErrs = (_gpsPayload[35] << 8) | _gpsPayload[34];
              gpsData_ptr->overrunErrs = (_gpsPayload[37] << 8) | _gpsPayload[36];
              gpsData_ptr->breakCond = (_gpsPayload[39] << 8) | _gpsPayload[38];
              gpsData_ptr->rxBusy = _gpsPayload[40];
              gpsData_ptr->txBusy = _gpsPayload[41];
              break;
            case 0x04:    // UBX-MON-VER (0x0A 0x04) Receiver/Software Version
              // ASCII text to decimal~float converter
              // 0 CH[30] - swVersion - Zero-terminated Software Version String.
              gpsData_ptr->swVersion = i_atoi(_gpsPayload, (4) + 0); // Software Version, e.g. 1.00
              gpsData_ptr->swVersion = gpsData_ptr->swVersion / 100; // Offset
              /// Serial.print(" MON_VER- ");
              /// Serial.print("SW Version "), Serial.print(gpsData_ptr->swVersion), Serial.println();
              // Rev# Software Version, e.g. (59842)
              gpsData_ptr->revVersion = i_atoi(_gpsPayload, (4) + 6); // Rev#
              /// Serial.print("\t REV Version "), Serial.print(gpsData_ptr->revVersion), Serial.println();
    
              // 30 CH[10] - hwVersion - Zero-terminated Hardware Version String
              gpsData_ptr->hwVersion = i_atoi(_gpsPayload, (4) + 30); // Hardware Version, e.g. 7 = u-blox 7
              gpsData_ptr->hwVersion = gpsData_ptr->hwVersion / 10000; // Offset
              /// Serial.print("\t HW Version u-blox:"), Serial.print(gpsData_ptr->hwVersion), Serial.println();
    
              // 40 + 30*N CH[30] - extension - Extended software information strings.
              // 40 Start of repeated block, each extension field is 30 characters long.
              // 1st extension
              if (_gpsPayload[(4 + 40)] == 'P') { // PROTVER 14.00
                gpsData_ptr->extension1 = i_atoi(_gpsPayload, (4) + 48); // Protocol version, e.g. 14.00
                gpsData_ptr->extension1 = gpsData_ptr->extension1 / 100; // Offset
                //Serial.print("\t 1st ext. Protocol "), Serial.print(gpsData_ptr->extension1), Serial.println();
    
                // 2nd extension
              } else if (_gpsPayload[(4 + 70)] == 'P') { // if PROTVER 14.00
                gpsData_ptr->extension1 = i_atoi(_gpsPayload, (4) + 78); //
                gpsData_ptr->extension1 = gpsData_ptr->extension1 / 100; // Offset
                /// Serial.print("\t 2nd ext. Protocol "), Serial.print(gpsData_ptr->extension1), Serial.println();
              }
              break;
            default:
              // Not Supported
              Serial.println(" Unknown Class MON ID:0x0"), Serial.println(_gpsPayload[1], HEX), Serial.println();
              break;
          }
          _UBX_MON_ID = false;
          // return false even though its packets are successfully being received, not a NAV packet
          return false;
        }
    
        // return true on receiving a full packet
        // return true;
        return true;
      } else {
        // return false if a full packet is not received
        return false;
      }
    }
    
    /* parse the uBlox data */
    bool UBLOX::parse() {
      // uBlox UBX header definition
      uint8_t const UBX_HEADER[] = { 0xB5, 0x62 };
    
      //int rd = 0, wr = 0, n = 0;    // # u-center #
      //uint8_t buffer[255];  // # u-center #
    
      // checksum calculation
      static unsigned char checksum[2];
    
      // read a byte from the serial port
      while ( _port->available() ) {
        uint8_t c = _port->read();
    
        /* ///>
          // ############ experimental u-center #########################
          // check if any data has arrived on the USB virtual serial port
          if (_use_ucenter == true) {
          rd = Serial.available();
          if (rd > 0) {
            // compute how much data to move, the smallest
              // of rd, wr and the buffer size
            if (rd > wr) rd = wr;
              if (rd > 80) rd = 80;
          // read data from the USB port
          n = Serial.readBytes((char *)buffer, rd);
          // write it to the hardware serial port
              _port->write(buffer, n);
          }
          // check if the USB virtual serial port is ready to transmit
          wr = Serial.availableForWrite();
          if (wr > 0) {
          Serial.write(c); // fix byte by byte, it's not efficient
          }
          }
          // ############ experimental u-center ##########################
          ///> */
    
        // identify the packet header
        if ( _fpos < 2 ) {
          if ( c == UBX_HEADER[_fpos] ) {
            _fpos++;
          } else {
            _fpos = 0;
          }
        }
        // Identify the packet Class
        else if (_fpos == 2) {
          switch (c) {    // ### Name Class Description ###
            case 0x01:    // NAV 0x01 Navigation Results: Position, Speed, Time, Acc, Heading, DOP, SVs used
              _CurrentClass = c; // Save for Message ID.
              ((unsigned char*)_gpsPayload)[_fpos - 2] = c; // grab the payload, need for checksum
              _fpos++;
              break;
            case 0x02:    // RXM 0x02 Receiver Manager Messages: Satellite Status, RTC Status
              _CurrentClass = c;
              Serial.print("UBX_RXM_CLASS:0x0"), Serial.print(c, HEX), Serial.println(" Not implemented"), Serial.println();
              _fpos = 0;
              break;
            case 0x04:    // INF 0x04 Information Messages: Printf-Style Messages, with IDs such as Error, Warning, Notice
              _CurrentClass = c;
              Serial.print("UBX_INF_CLASS:0x0"), Serial.print(c, HEX), Serial.println(" Not implemented"), Serial.println();
              _fpos = 0;
              break;
            case 0x05:    // ACK 0x05 Ack/Nack Messages: as replies to CFG Input Messages
              _CurrentClass = c;
              ((unsigned char*)_gpsPayload)[_fpos - 2] = c; // grab the payload
              _fpos++;
              break;
            case 0x06:    // CFG 0x06 Configuration Input Messages: Set Dynamic Model, Set DOP Mask, Set Baud Rate, etc.
              _CurrentClass = c; // Save for Message ID.
              ((unsigned char*)_gpsPayload)[_fpos - 2] = c; // grab the payload, need for checksum
              _fpos++;
              break;
            case 0x09:    // UPD 0x09 Firmware Update Messages: Memory/Flash erase/write, Reboot, Flash identification, etc.
              Serial.print("UBX_UPD_CLASS:0x0"), Serial.print(c, HEX), Serial.println(" Not implemented"), Serial.println();
              _CurrentClass = c;
              _fpos = 0;
              break;
            case 0x0A:    // MON 0x0A Monitoring Messages: Comunication Status, CPU Load, Stack Usage, Task Status
              _CurrentClass = c;
              ((unsigned char*)_gpsPayload)[_fpos - 2] = c; // grab the payload
              _fpos++;
              break;
            case 0x0B:    // AID 0x0B AssistNow Aiding Messages: Ephemeris, Almanac, other A-GPS data input
              Serial.print("UBX_AID_CLASS:0x0"), Serial.print(c, HEX), Serial.println(" Not implemented"), Serial.println();
              _CurrentClass = c;
              _fpos = 0;
              break;
            case 0x0D:    // TIM 0x0D Timing Messages: Time Pulse Output, Timemark Results
              Serial.print("UBX_TIM_CLASS:0x0"), Serial.print(c, HEX), Serial.println(" Not implemented"), Serial.println();
              _CurrentClass = c;
              _fpos = 0;
              break;
            // Not Supported
            // case 0x10: ESF 0x10 External Sensor Fusion Messages: External Sensor Measurements and Status Information
            // case 0x13: MGA 0x13 Multiple GNSS Assistance Messages: Assistance data for various GNSS
            case 0x21:    // LOG 0x21 Logging Messages: Log creation, deletion, info and retrieval
              Serial.print("UBX_LOG_CLASS:0x"), Serial.print(c, HEX), Serial.println(" Not implemented"), Serial.println();
              _CurrentClass = c;
              _fpos = 0;
              break;
            // Not Supported
            // case 0x27: SEC 0x27 Security Feature Messages
            // case 0x28: HNR 0x28 High Rate Navigation Results Messages: High rate time, position, speed, heading
            default:
              // Not Supported
              _CurrentClass = c;
              _fpos = 0;
              Serial.print("Unknown packet Class:");
              if (c > 16) {
                Serial.print("0x");
              } else {
                Serial.print("0x0");
              }
              Serial.println(c, HEX), Serial.println();
              break;
          }
        }
        // Identify the packet Message ID
        else if (_fpos == 3 ) {
          switch (_CurrentClass) {
            case 0x01:    // NAV 0x01
              NAV_IDs(c); // Go grab the uBlox NAV - ID
              break;
            case 0x02:    // RXM 0x02
              _fpos = 0;
              break;
            case 0x04:    // INF 0x04
              _fpos = 0;
              break;
            case 0x05:    // ACK 0x05
              ACK_IDs(c); // Go grab the uBlox ACK - ID
              break;
            case 0x06:    // CFG 0x06
              CFG_IDs(c); // Go grab the uBlox CFG - ID
              break;
            case 0x0A:    // MON 0x0A
              switch (c) {  // load only needed Class MON IDs
                case 0x02:    // UBX-MON-IO (0x0A 0x02) I/O Subsystem Status
                  _UBX_MON_ID = true;
                  ((unsigned char*)_gpsPayload)[_fpos - 2] = c;
                  _fpos++;
                  break;
                case 0x04:    // UBX-MON-VER (0x0A 0x04) Receiver/Software Version
                  _UBX_MON_ID = true;
                  ((unsigned char*)_gpsPayload)[_fpos - 2] = c;
                  _fpos++;
                  break;
                default:
                  // Not Supported
                  _fpos = 0;
                  Serial.print(" Not implemented Class MON ID:0x0"), Serial.println(c, HEX), Serial.println();
                  break;
              }
              break;
            case 0x0B:    // AID 0x0B
              _fpos = 0;
              break;
            case 0x0D:    // TIM 0x0D
              _fpos = 0;
              break;
            case 0x21:    // LOG 0x21
              _fpos = 0;
              break;
            default:
              // 0x03 and 0x0E to 0x20 Not Supported
              _fpos = 0;
              Serial.println("Unknown packet ID:"), Serial.println(c, HEX), Serial.println();
              break;
          }
          _CurrentClass = 0x00;
        }
        // Identify the packet 16Bit Message payloadSize
        // It does not include 16Bit-Sync header, 8Bit-Class, 8Bit-ID, 16Bit-Length Field, and 16Bit-CRC fields
        else if (_fpos == 4 ) {
          ((unsigned char*)_gpsPayload)[_fpos - 2] = c; // least significant byte (LSB)
          // prevent _gpsPayload[_payloadSize] array overrun error, currently set @ 150
          if (c < _payloadSize) {
            _fpos++;
          } else {
            Serial.print("_gpsPayload[_payloadSize] array error:"), Serial.println(c);
            _fpos = 0;
          }
        }
        else if (_fpos == 5 ) {
          ((unsigned char*)_gpsPayload)[_fpos - 2] = c; // most significant byte (MSB)
          _MSGpayloadSize = (_gpsPayload[3] << 8) | _gpsPayload[2]; // Combining two uint8_t as uint16_t
          _MSGpayloadSize = _MSGpayloadSize + 4; // Fix for (CRC), Add 8Bit-Class, 8Bit-ID, 16Bit-Length Field
          _fpos++;
        }
        else {
          // grab the payload
          if ( (_fpos - 2) < _MSGpayloadSize )
            ((unsigned char*)_gpsPayload)[_fpos - 2] = c;
          _fpos++;
    
          // The checksum is calculated over the packet, starting and including the
          // CLASS field, ID and payload, up until, but excluding, the Checksum Field
          // (CRC) Compute checksum
          if ( (_fpos - 2) == _MSGpayloadSize ) {
            calcChecksum(checksum, _gpsPayload, _MSGpayloadSize);
          }
          else if ( (_fpos - 2) == (_MSGpayloadSize + 1) ) {
            if ( c != checksum[0] )
              _fpos = 0;
          }
          else if ( (_fpos - 2) == (_MSGpayloadSize + 2) ) {
            _fpos = 0;
            if ( c == checksum[1] ) {
              return true;
            }
          }
          else if ( _fpos > (_MSGpayloadSize + 4) ) {
            _fpos = 0;
          }
        }
      }
      return false;
    }
    
    /* uBlox checksum */
    void UBLOX::calcChecksum(unsigned char* CK, unsigned char* payload, uint8_t length) {
      CK[0] = 0;
      CK[1] = 0;
      for (uint8_t i = 0; i < length; i++) {
        CK[0] += payload[i]; // CK[0] = CK[0] + payload[i];
        CK[1] += CK[0];
      }
    }
    
    /* uBlox CFG - IDs */
    void UBLOX::CFG_IDs(uint8_t SERc) {
      switch (SERc) {       // ### Name CFG-Class & IDs Description, Msg Length ###
        case 0x00:          // UBX-CFG-PRT 0x06 0x00 Poll or Get/Set Request Polls the configuration for one I/O Port , len[1]
          _UBX_CFG_ID = true;
          ((unsigned char*)_gpsPayload)[_fpos - 2] = SERc;
          _fpos++;
          break;
        case 0x01:          // UBX-CFG-MSG 0x06 0x01 Get/Set Set Message Rate(s)
          Serial.print("UBX_CFG_MSG ID:"), Serial.print(SERc, HEX), Serial.println(" Not implemented"), Serial.println();
          _fpos = 0;
          break;
        case 0x31:          // UBX-CFG-TP5 0x06 0x31 Time Pulse Parameters
          _UBX_CFG_ID = true;
          ((unsigned char*)_gpsPayload)[_fpos - 2] = SERc;
          _fpos++;
          break;
        default:
          // Not Supported
          _fpos = 0;
          Serial.print(" Not implemented or Unknown packet Class CFG ID:"), Serial.println(SERc, HEX), Serial.println();
          break;
      }
    }
    
    /* uBlox ACK - IDs */
    // When messages from the class CFG are sent to the receiver, the receiver will send an "acknowledge" (ACK-ACK) or a "not acknowledge" (ACK-NAK) message back to the sender.
    void UBLOX::ACK_IDs(uint8_t SERa) {
      uint8_t CN = 0;
      switch (SERa) {       // ### Name ACK-Class & IDs Description, Msg Length ###
        case 0x00:          //  UBX-ACK-NAK (0x05 0x00) Message Not-Acknowledged, len[2]
          if ((_printACK == 0x06001 || _printACK_ATT == 0x01 || _printACK_ATT == 0x02) && CN == 0) {
            Serial.print("UBX_ACK_NAK:"), Serial.print(" Message Not-Acknowledged ");
            if (_printACK == 0x06001) {
              CN = 1;
              Serial.println("- CFG_PRT baudRate"), Serial.println();
              _printACK = 0x00; // reset
            }
            if ((_printACK_ATT == 0x01) && CN == 0) { // Ena_NAV_ATT 0x06011
              CN = 1;
              Serial.println("- Ena_NAV_ATT, ATT Supported only on protocol v. 19 (only with ADR or UDR products)");
              _printACK_ATT = 0x00; // reset
            }
            if ((_printACK_ATT == 0x02) && CN == 0) { // Dis_NAV_ATT 0x06010
              CN = 1;
              Serial.println("- Dis_NAV_ATT, ATT Supported only on protocol v. 19 (only with ADR or UDR products)");
              _printACK_ATT = 0x00; // reset
            }
          } else {
            if ((_printACK_ATT == 0xFF) && CN == 0) {
              CN = 1;
              _printACK_ATT = 0x00;
            } else {
              Serial.print("UBX_ACK_NAK:"), Serial.println(" Message Not-Acknowledged ");
            }
          }
          // _UBX_ACK_NAK_ID = true; // Not implemented
          ((unsigned char*)_gpsPayload)[_fpos - 2] = SERa;
          _fpos++;
          break;
        case 0x01:          //  UBX-ACK-ACK (0x05 0x01) Message Acknowledged, len[2]
          _UBX_ACK_ACK_ID = true;
          ((unsigned char*)_gpsPayload)[_fpos - 2] = SERa;
          _fpos++;
          break;
        default:
          // Not Supported
          _fpos = 0;
          Serial.println("Unknown packet Class ACK ID:"), Serial.println(SERa, HEX), Serial.println();
          break;
      }
    }
    
    /* uBlox NAV - IDs */
    void UBLOX::NAV_IDs(uint8_t SERn) {
    
      switch (SERn) {    // ### Name NAV-Class & IDs Description, Msg Length ###
        case 0x01:          // NAV-POSECEF 0x01 0x01 Position Solution in ECEF, U-Blox7 = len[20]
          // ####### experimental todo fix
          ((unsigned char*)_gpsPayload)[_fpos - 2] = SERn;
          _fpos++; //### u center test ###
          break;
        case 0x02:          // NAV-POSLLH 0x01 0x02 Geodetic Position Solution, U-Blox7 = len[28]
          _UBX_NAV_POSLLH_ID = true;
          ((unsigned char*)_gpsPayload)[_fpos - 2] = SERn;
          _fpos++;
          break;
        case 0x03:          // NAV-STATUS 0x01 0x03 Receiver Navigation Status, U-Blox7 = len[16]
          // experimental todo fix
          ((unsigned char*)_gpsPayload)[_fpos - 2] = SERn;
          _fpos++; // u center test
          break;
        case 0x04:         // NAV-DOP 0x01 0x04 Dilution of precision, U-Blox7 = len[18]
          // ####### experimental todo fix
          ((unsigned char*)_gpsPayload)[_fpos - 2] = SERn;
          _fpos++; //### u center test ###
          break;
        //                    NAV-ATT (0x01 0x05) Attitude Solution, U-Blox8 = len[32]
        case 0x05:         // #### NOTE: u-blox 8 only from protocol version 19 up to version 23.01 (only with ADR or UDR products) ###
          _UBX_NAV_ATT_ID = true;
          ((unsigned char*)_gpsPayload)[_fpos - 2] = SERn;
          _fpos++;
          break;
        case 0x06:         // NAV-SOL 0x01 0x06 Navigation Solution Information, U-Blox7 = len[52]
          // experimental todo fix
          ((unsigned char*)_gpsPayload)[_fpos - 2] = SERn;
          _fpos++; // u center test
          break;
        case 0x07:        // NAV-PVT 0x01 0x07 Navigation Position Velocity Time Solution, U-Blox7=len[84], U-Blox8=len[92]
          _UBX_NAV_PVT_ID = true;
          ((unsigned char*)_gpsPayload)[_fpos - 2] = SERn;
          _fpos++;
          break;
        //case 0x09: NAV-ODO 0x01 0x09 20 Periodic/Polled Odometer Solution
        //case 0x10: NAV-RESETODO 0x01 0x10 0 Command Reset odometer
        case 0x11:        // NAV-VELECEF 0x01 0x11 Velocity Solution in ECEF, U-Blox7 = len[20]
          // ####### experimental todo fix
          ((unsigned char*)_gpsPayload)[_fpos - 2] = SERn;
          _fpos++; //### u center test ###
          break;
        case 0x12:        // NAV-VELNED 0x01 0x12 Velocity Solution in NED, U-Blox7 = len[36]
          // ####### experimental todo fix
          ((unsigned char*)_gpsPayload)[_fpos - 2] = SERn;
          _fpos++; //### u center test ###
          break;
        //case 0x13: NAV-HPPOSECEF 0x01 0x13 28 Periodic/Polled High Precision Position Solution in ECEF
        //case 0x14: NAV-HPPOSLLH 0x01 0x14 36 Periodic/Polled High Precision Geodetic Position Solution
        case 0x20:        // NAV-TIMEGPS 0x01 0x20 16 Periodic/Polled GPS Time Solution, U-Blox7 = len[16]
          // ####### experimental todo fix
          ((unsigned char*)_gpsPayload)[_fpos - 2] = SERn;
          _fpos++; //### u center test ###
          break;
        case 0x21:        // NAV-TIMEUTC 0x01 0x21 UTC Time Solution, U-Blox7 = len[20]
          // ####### experimental todo fix
          ((unsigned char*)_gpsPayload)[_fpos - 2] = SERn;
          _fpos++; //### u center test ###
          break;
        case 0x22:       // NAV-CLOCK 0x01 0x22 Clock Solution, U-Blox7 = len[20]
          // ####### experimental todo fix
          ((unsigned char*)_gpsPayload)[_fpos - 2] = SERn;
          _fpos++; //### u center test ###
          break;
        //case 0x23: NAV-TIMEGLO 0x01 0x23 20 Periodic/Polled GLO Time Solution
        //case 0x24: NAV-TIMEBDS 0x01 0x24 20 Periodic/Polled BDS Time Solution
        //case 0x25: NAV-TIMEGAL 0x01 0x25 20 Periodic/Polled Galileo Time Solution
        //case 0x26: NAV-TIMELS 0x01 0x26 24 Periodic/Polled Leap second event information
        case 0x30:       // NAV-SVINFO U-Blox7 = len[8 + 12*numCh]
          // ####### experimental todo fix
          ((unsigned char*)_gpsPayload)[_fpos - 2] = SERn;
          _fpos++; //### u center test ###
          break;
        case 0x31:       // NAV-DGPS 0x01 0x31 DGPS Data Used for NAV, U-Blox7 = len[16 + 12*numCh]
          // ####### experimental todo fix
          ((unsigned char*)_gpsPayload)[_fpos - 2] = SERn;
          _fpos++; //### u center test ###
          break;
        case 0x32:       // NAV-SBAS 0x01 0x32 12 + 12*cnt Periodic/Polled SBAS Status Data
          // ####### experimental todo fix
          ((unsigned char*)_gpsPayload)[_fpos - 2] = SERn;
          _fpos++; //### u center test ###
          break;
        //case 0x34: NAV-ORB 0x01 0x34 8 + 6*numSv Periodic/Polled GNSS Orbit Database Info
        //case 0x35: NAV-SAT 0x01 0x35 8 + 12*numSvs Periodic/Polled Satellite Information
        //case 0x39: NAV-GEOFENCE 0x01 0x39 8 + 2*numFen... Periodic/Polled Geofencing status
        //case 0x3B: NAV-SVIN 0x01 0x3B 40 Periodic/Polled Survey-in data
        //case 0x3C: NAV-RELPOSNED 0x01 0x3C 40 Periodic/Polled Relative Positioning Information in NED frame
        case 0x60:       // NAV-AOPSTATUS 0x01 0x60 AssistNow Autonomous Status, U-Blox7 = len[20]
          // ####### experimental todo fix
          ((unsigned char*)_gpsPayload)[_fpos - 2] = SERn;
          _fpos++; //### u center test ###
          break;
        case 0x61:       // NAV-EOE 0x01 0x61 4 Periodic End Of Epoch
          // ####### experimental todo fix
          ((unsigned char*)_gpsPayload)[_fpos - 2] = SERn;
          _fpos++; //### u center test ###
          break;
    
        default:
          // Not Supported
          _fpos = 0;
          Serial.println("Unknown packet Class NAV ID:"), Serial.println(SERn, HEX), Serial.println();
          break;
      }
    }
    
    // String helper
    uint32_t UBLOX::i_atoi(uint8_t s[], uint8_t ptr)
    {
      uint32_t i, n = 0;
      for (i = ptr; (s[i] >= '0' && s[i] <= '9') || (s[i] == '.' || s[i] == '(') ; i++) {
        if (s[i] == '.' || s[i] == '(') {
          // ignore the ASCII . period and ASCII (
        } else {
          n = 10 * n + (s[i] - '0');
        }
      }
      return n;
    }
    
    double UBLOX::modifiedMap(double x, double in_min, double in_max, double out_min, double out_max)
    {
      double temp = (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
      temp = (double) (4 * temp + .5);
      return (double) temp / 4;
    }
    
    void UBLOX::DEBUG() {
      // DEBUG PRINT
      Serial.println("HEADER [0]=HEX B5 DEC *181‬");
      Serial.println("HEADER [1]=HEX 62 DEC ** 98‬‬");
    
      uint16_t MSGpayload = (_gpsPayload[3] << 8) | _gpsPayload[2]; // Combining two uint8_t as uint16_t
      for (int i = 0; i < (MSGpayload) + 4; i++) {
        Serial.print("_gpsPayload["), Serial.print(i, DEC), Serial.print("]=");
        if (_gpsPayload[i] < 16) {
          Serial.print("HEX 0");
          Serial.print(_gpsPayload[i], HEX);
        } else {
          Serial.print("HEX ");
          Serial.print(_gpsPayload[i], HEX);
        }
        if (_gpsPayload[i] < 10) {
          Serial.print(" DEC 0");
          Serial.println(_gpsPayload[i], DEC);
        } else {
          Serial.print(" DEC ");
          Serial.println(_gpsPayload[i], DEC);
        }
      }
    }
    #endif
    v1.0.3
    UBLOX.h
    Code:
    /*
      UBLOX.h
      Brian R Taylor
      brian.taylor@bolderflight.com
      2016-11-03
    
      Copyright (c) 2016 Bolder Flight Systems
    
      Permission is hereby granted, free of charge, to any person obtaining a copy of this software
      and associated documentation files (the "Software"), to deal in the Software without restriction,
      including without limitation the rights to use, copy, modify, merge, publish, distribute,
      sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
      furnished to do so, subject to the following conditions:
    
      The above copyright notice and this permission notice shall be included in all copies or
      substantial portions of the Software.
    
      THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
      BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
      NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
      DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
      OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
    
      v1.0.3
      Chris O.
      2018-03-29
    
      Supported Navigation IDs:
      UBX-NAV-PVT ---- (0x01 0x07) Navigation Position Velocity Time Solution
      UBX-NAV-POSLLH - (0x01 0x02) Geodetic Position Solution
      UBX-NAV-ATT ---- (0x01 0x05) Attitude Solution
    
      /---  High level Commands, for the user ---/
       NOTE: command(bool) == (true)Print Message Acknowledged on USB Serial Monitor
      begin(Baud)                       // Starting communication with the GPS
      end();                            // Disables Teensy serial communication, to re-enable, call begin(Baud)
      Poll_CFG_Port1(bool);             // Polls the configuration for one I/O Port, I/O Target 0x01=UART1
      Poll_NAV_PVT();                   // Polls UBX-NAV-PVT    (0x01 0x07) Navigation Position Velocity Time Solution
      Poll_NAV_POSLLH();                // Polls UBX-NAV-POSLLH (0x01 0x02) Geodetic Position Solution
      Poll_NAV_ATT();                   // Polls UBX-NAV-ATT    (0x01 0x05) Attitude Solution
      Poll_MON_IO(bool);                // Polls UBX-MON-IO (0x0A 0x02) I/O Subsystem Status
      Poll_MON_VER(bool);               // Polls UBX-MON-VER (0x0A 0x04) Receiver/Software Version
      Poll_CFG_TP5(bool);               // Polls CFG-TP5        (0x06 0x31) Poll Time Pulse 0 Parameters
      ### Periodic Auto Update ON,OFF Command ###
      Ena_NAV_PVT(bool);                // Enable periodic auto update NAV_PVT
      Dis_NAV_PVT(bool);                // Disable periodic auto update NAV_PVT
      Ena_NAV_ATT(bool);                // Enable periodic auto update NAV_ATT
      Dis_NAV_ATT(bool);                // Disable periodic auto update NAV_ATT
      Ena_NAV_POSLLH(bool);             // Enable periodic auto update NAV_POSLLH
      Dis_NAV_POSLLH(bool);             // Disable periodic auto update NAV_POSLLH
      ### u-blox Switch off all NMEA MSGs ###
      Dis_all_NMEA_Child_MSGs(bool);    // Disable All NMEA Child Messages Command
      ### High level Command Generator ###
      SetGPSbaud(uint32_t baud, bool)   // Set UBLOX GPS Port Configuration Baud rate
      SetNAV5(uint8_t dynModel, bool)   // Set Dynamic platform model Navigation Engine Settings (0:portable, 3:pedestrian, Etc)
      SetRATE(uint16_t measRate, bool)  // Set Navigation/Measurement Rate Settings (100ms=10.00Hz, 200ms=5.00Hz, 1000ms=1.00Hz, Etc)
      SetCFG_TP5(uint32_t FreqLocked, double DutyLocked, uint16_t antCableDelay, bool printACK); // UBX-CFG-TP5 (0x06 0x31) Time Pulse 0 Parameters
      Ena_Dis_MON_IO(bool En~Di, bool) // UBX-MON-IO (0x0A 0x02) Ena/Dis periodic auto update I/O Subsystem Status, bytes(received, sent), parity , framing , overrun)
    */
    
    //  UBX-NAV-POSLLH -- Geodetic Position Solution
    //  ### UBX Protocol, Class NAV 0x01, ID 0x02 ###
    // UBX-NAV-POSLLH (0x01 0x02) (Payload U-blox-M8&M7&M6=20)
    // iTOW                       ///< [ms], GPS time of the navigation epoch
    // lon                        ///< [deg], Longitude
    // lat                        ///< [deg], Latitude
    // height                     ///< [m], Height above ellipsoid
    // hMSL                       ///< [m], Height above mean sea level
    // hAcc                       ///< [m], Horizontal accuracy estimate
    
    //  UBX-NAV-ATT -- Attitude Solution
    //  ### UBX Protocol, Class NAV 0x01, ID 0x05 ###
    //  ### NOTE: U-blox M8 from protocol version 19 up to version 23.01 (only with ADR or UDR products) ###
    // UBX-NAV-ATT (0x01 0x05)    (Payload U-blox-M8=32)
    // version                    ///< [ND],  Message version (0 for this version)
    // roll                       ///< [deg], Vehicle roll.
    // pitch                      ///< [deg], Vehicle pitch.
    // heading                    ///< [deg], Heading of motion (2-D)
    // accRoll                    ///< [deg], Vehicle roll accuracy (if null, roll angle is not available).
    // accPitch                   ///< [deg], Vehicle pitch accuracy (if null, pitch angle is not available).
    // accHeading                 ///< [deg], Vehicle heading accuracy (if null, heading angle is not available).
    
    //  UBX-NAV-PVT --  Navigation Position Velocity Time Solution
    //  ### UBX Protocol, Class NAV 0x01, ID 0x07 ###
    // UBX-NAV-PVT (0x01 0x07)    (Payload U-blox-M8=92, M7&M6=84)
    // iTOW                       ///< [ms], GPS time of the navigation epoch
    // utcYear                    ///< [year], Year (UTC)
    // utcMonth                   ///< [month], Month, range 1..12 (UTC)
    // utcDay                     ///< [day], Day of month, range 1..31 (UTC)
    // utcHour                    ///< [hour], Hour of day, range 0..23 (UTC)
    // utcMin                     ///< [min], Minute of hour, range 0..59 (UTC)
    // utcSec                     ///< [s], Seconds of minute, range 0..60 (UTC)
    // valid                      ///< [ND], Validity flags
    // tAcc                       ///< [ns], Time accuracy estimate (UTC)
    // utcNano                    ///< [ns], Fraction of second, range -1e9 .. 1e9 (UTC)
    // fixType                    ///< [ND], GNSSfix Type: 0: no fix, 1: dead reckoning only, 2: 2D-fix, 3: 3D-fix, 4: GNSS + dead reckoning combined, 5: time only fix
    // flags                      ///< [ND], Fix status flags
    // flags2                     ///< [ND], Additional flags
    // numSV                      ///< [ND], Number of satellites used in Nav Solution
    // lon                        ///< [deg], Longitude
    // lat                        ///< [deg], Latitude
    // height                     ///< [m], Height above ellipsoid
    // hMSL                       ///< [m], Height above mean sea level
    // hAcc                       ///< [m], Horizontal accuracy estimate
    // vAcc                       ///< [m], Vertical accuracy estimate
    // velN                       ///< [m/s], NED north velocity
    // velE                       ///< [m/s], NED east velocity
    // velD                       ///< [m/s], NED down velocity
    // gSpeed                     ///< [m/s], Ground Speed (2-D)
    // heading                    ///< [deg], Heading of motion (2-D)
    // sAcc                       ///< [m/s], Speed accuracy estimate
    // headingAcc                 ///< [deg], Heading accuracy estimate (both motion and vehicle)
    // pDOP                       ///< [ND], Position DOP
    // headVeh                    ///< [deg], Heading of vehicle (2-D)             #### NOTE: u-blox8 only ####
    // --- magDec, magAcc --- TODO TEST
    // magDec                     ///< [deg], Magnetic declination                 #### NOTE: u-blox8 only ####
    // magAcc                     ///< [deg], Magnetic declination accuracy        #### NOTE: u-blox8 only ####
    
    #ifndef UBLOX_h
    #define UBLOX_h
    
    #include "Arduino.h"
    
    struct gpsData {
      unsigned long   iTOW;        ///< [ms], GPS time of the navigation epoch
      unsigned short  utcYear;     ///< [year], Year (UTC)
      unsigned char   utcMonth;    ///< [month], Month, range 1..12 (UTC)
      unsigned char   utcDay;      ///< [day], Day of month, range 1..31 (UTC)
      unsigned char   utcHour;     ///< [hour], Hour of day, range 0..23 (UTC)
      unsigned char   utcMin;      ///< [min], Minute of hour, range 0..59 (UTC)
      unsigned char   utcSec;      ///< [s], Seconds of minute, range 0..60 (UTC)
      unsigned char   valid;       ///< [ND], Validity flags
      unsigned long   tAcc;        ///< [ns], Time accuracy estimate (UTC)
      long            utcNano;     ///< [ns], Fraction of second, range -1e9 .. 1e9 (UTC)
      unsigned char   fixType;     ///< [ND], GNSSfix Type: 0: no fix, 1: dead reckoning only, 2: 2D-fix, 3: 3D-fix, 4: GNSS + dead reckoning combined, 5: time only fix
      unsigned char   flags;       ///< [ND], Fix status flags
      unsigned char   flags2;      ///< [ND], Additional flags
      unsigned char   numSV;       ///< [ND], Number of satellites used in Nav Solution
      double          lon;         ///< [deg], Longitude
      double          lat;         ///< [deg], Latitude
      double          height;      ///< [m], Height above ellipsoid
      double          hMSL;        ///< [m], Height above mean sea level
      double          hAcc;        ///< [m], Horizontal accuracy estimate
      double          vAcc;        ///< [m], Vertical accuracy estimate
      double          velN;        ///< [m/s], NED north velocity
      double          velE;        ///< [m/s], NED east velocity
      double          velD;        ///< [m/s], NED down velocity
      double          gSpeed;      ///< [m/s], Ground Speed (2-D)
      double          heading;     ///< [deg], Heading of motion (2-D) ***
      double          sAcc;        ///< [m/s], Speed accuracy estimate
      double          headingAcc;  ///< [deg], Heading accuracy estimate (both motion and vehicle)
      double          pDOP;        ///< [ND], Position DOP
      double          headVeh;     ///< [deg], Heading of vehicle (2-D)        #### NOTE: u-blox8 only ####
      double          magDec;      ///< [deg], Magnetic declination            #### NOTE: u-blox8 only ####
      double          magAcc;      ///< [deg], Magnetic declination accuracy   #### NOTE: u-blox8 only ####
      // UBX-NAV-ATT (0x01 0x05) Length (Payload 32)
      // #### NOTE: u-blox 8 from protocol version 19 up to version 23.01 (only with ADR or UDR products) ###
      unsigned char   version;     ///< [ND],  Message version (0 for this version)
      double          roll;        ///< [deg], Vehicle roll.
      double          pitch;       ///< [deg], Vehicle pitch.
      //              heading;     ///< [deg], heading *** see above ^^^^
      double          accRoll;     ///< [deg], Vehicle roll accuracy (if null, roll angle is not available).
      double          accPitch;    ///< [deg], Vehicle pitch accuracy (if null, pitch angle is not available).
      double          accHeading;  ///< [deg], Vehicle heading accuracy (if null, heading angle is not available).
    
      /* Returned after sending a Poll command or periodic auto update */
      uint32_t        GpsUart1Baud; ///< [ND], Current GPS Uart1 baud rate --
      // UBX-MON-IO (0x0A 0x02) I/O Subsystem Status
      uint32_t        rxBytes;       ///< [B], Number of bytes ever received
      uint32_t        txBytes;       ///< [B], Number of bytes ever sent
      uint16_t        parityErrs;    ///< [ms], Number of 100ms timeslots with parity errors
      uint16_t        framingErrs;   ///< [ms], Number of 100ms timeslots with framing errors
      uint16_t        overrunErrs;   ///< [ms], Number of 100ms timeslots with overrun errors
      uint16_t        breakCond;     ///< [ms], Number of 100ms timeslots with break conditions
      uint8_t         rxBusy;        ///< [0~1], Flag is receiver is busy
      uint8_t         txBusy;        ///< [0~1], Flag is transmitter is busy
      // UBX-MON-VER (0x0A 0x04) Receiver/Software Version
      float           swVersion;     ///< Software Version
      uint32_t        revVersion;    ///< Software Version
      uint32_t        hwVersion;     ///< Hardware Version
      float           extension1;    ///< Extended software information, Protocol version e.g. (14.00)
      //              extension2;    ///< strings e.g. the supported major GNSS ~ GPS;SBAS;GLO;QZSS
      // UBX-CFG-TP5 (0x06 0x31) Time Pulse Parameters
      uint16_t        antCableDelay; ///< [ns], Antenna cable delay
      uint32_t        freqPeriodL;   ///< [Hz], Frequency time when locked to GPS time
      double          dutycycleL;    ///< [%], duty cycle locked %
    
      // For more INFO see u-blox # / u-blox M# pdf Manual, Receiver Description Including Protocol Specification.
      // https://www.u-blox.com/sites/default/files/products/documents/u-blox8-M8_ReceiverDescrProtSpec_(UBX-13003221)_Public.pdf
    };
    
    class UBLOX {
      public:
        UBLOX();
        UBLOX(uint8_t bus);
        void configure(uint8_t bus);
        void begin(int baud);
        void end(); // Disables serial communication, to re-enable serial communication, call gps.begin(Baud);
        bool read(gpsData *gpsData_ptr);
    
        /******************************************/
        /**  High level commands, for the user    */
        /******************************************/
        void Poll_GPSbaud_Port1(bool printACK);             // Polls the configuration for one I/O Port, I/O Target 0x01=UART1
        void Poll_CFG_TP5(bool printACK);                   // Polls CFG-TP5 (0x06 0x31) Poll Time Pulse 0 Parameters
        void Poll_MON_IO(bool printACK);                    // Polls MON-I/O Subsystem Status, bytes(received, sent), parity errors, framing errors, overrun errors)
        void Poll_MON_VER(bool printACK);                   // Polls MON-VER (0x0A 0x04) Receiver/Software Version
        void Poll_NAV_PVT();                                // Polls UBX-NAV-PVT    (0x01 0x07) Navigation Position Velocity Time Solution
        void Poll_NAV_POSLLH();                             // Polls UBX-NAV-POSLLH (0x01 0x02) Geodetic Position Solution
        void Poll_NAV_ATT();                                // Polls UBX-NAV-ATT (0x01 0x05) Attitude Solution
        // ### Periodic auto update ON,OFF command ###
        void Ena_NAV_PVT(bool printACK);                    // Enable periodic auto update NAV_PVT
        void Dis_NAV_PVT(bool printACK);                    // Disable periodic auto update NAV_PVT
        void Ena_NAV_ATT(bool printACK);                    // Enable periodic auto update NAV_ATT
        void Dis_NAV_ATT(bool printACK);                    // Disable periodic auto update NAV_ATT
        void Ena_NAV_POSLLH(bool printACK);                 // Enable periodic auto update NAV_POSLLH
        void Dis_NAV_POSLLH(bool printACK);                 // Disable periodic auto update NAV_POSLLH
        // #### u-blox Switch OFF ALL NMEA MSGs ####
        void Dis_all_NMEA_Child_MSGs(bool printACK);        // Disable All NMEA Child Messages Command
        /** High level Command Generator GPS control write operations **/
        void SetGPSbaud(uint32_t GPS32baud, bool printACK); // UBX-CFG-PRT  (0x06 0x00)  Set UBLOX GPS Port Configuration Baud rate
        void SetNAV5(uint8_t dynModel, bool printACK);      // UBX-CFG-NAV5 (0x06 0x24)  Set Dynamic platform model Navigation Engine Settings (0:portable, 3:pedestrian, Etc)
        void SetRATE(uint16_t measRate, bool printACK);     // UBX-CFG-RATE (0x06 0x08)  Set Navigation/Measurement Rate Settings (100ms=10.00Hz, 200ms=5.00Hz, 1000ms=1.00Hz, Etc)
        void Ena_Dis_MON_IO(bool on_off, bool printACK);    // UBX-MON-IO (0x0A 0x02) Ena/Dis periodic auto update I/O Subsystem Status, bytes(received, sent), parity , framing , overrun)
        void SetCFG_TP5(uint32_t FreqLocked, double DutyLocked, uint16_t antCableDelay, bool printACK); // UBX-CFG-TP5 (0x06 0x31) Time Pulse 0 Parameters
    
        // TODO: add pre-processor ifdef
        ///> void USE_Ucenter(bool u_c); // # experimental Bridge between u-center and teensy UBLOX #
    
      private:
        uint8_t _bus;
        uint8_t _fpos;
        uint8_t _CurrentClass;
        uint8_t _portID; // I/O Port
        uint32_t _printACK;
        uint32_t _printACK_Dis_all_NMEA;
        uint32_t _printACK_PVT;
        uint32_t _printACK_ATT;
        uint32_t _printACK_POSLLH;
        uint8_t _ACKcount;
        bool _pACK2 = false;
        bool _pACK_RATE = false;
        bool _pACK_NAV5 = false;
        bool _pACK_TP5 = false;
        bool _UBX_NAV_PVT_ID = false;
        bool _UBX_NAV_POSLLH_ID = false;
        bool _UBX_NAV_ATT_ID = false;
        bool _UBX_ACK_ACK_ID = false;
        bool _UBX_CFG_ID = false;
        bool _UBX_MON_ID = false;
        uint8_t _print_I_O = 0;
        uint16_t _MSGpayloadSize;
        uint32_t _GpsUartBaud;
    
        // # experimental u-center COM #
        ///> bool _use_ucenter = false; // # u-center #
    
        static const uint8_t _payloadSize = 150; // array error: UBX-NAV-SVINFO (0x01 0x30) experimental u-center, needs 230
        uint8_t _gpsPayload[_payloadSize];
        HardwareSerial* _port;
        bool parse();
        void calcChecksum(unsigned char* CK, unsigned char* payload, uint8_t length);
        void ACK_IDs(uint8_t SERa);
        void NAV_IDs(uint8_t SERn);
        void CFG_IDs(uint8_t SERc);
    
        // # experimental u-center COM #
        ///> int rd = 0, wr = 0, n = 0;    // # u-center #
        ///> uint8_t buffer[_payloadSize]; // # u-center #
    
        /** High level Command GPS control write operations **/
        // Generate the uBlox command configuration
        void writeCommand(uint8_t CLASS, uint8_t ID , uint8_t PayloadLength0, uint8_t PayloadLength1, uint8_t Identifier, uint8_t Para0, uint8_t Para1, uint8_t Para2, uint8_t Para3, uint8_t res0, uint32_t P32t0, uint32_t P32t1) const;
    
        // ### Polling Commands Mechanism, MSG REQUEST ###
        // All messages that are output by the receiver in a periodic manner (e.g. messages in classes MON, NAV and RXM) can also be polled.
        uint8_t const _Poll_CFG_PRT[9] =       {0xB5, 0x62, 0x06, 0x00, 0x01, 0x00, 0x01, 0x08, 0x22};  // UBX-CFG-PRT (0x06 0x00) Polls the GPS baud configuration for one I/O Port, (I/O Target # MSG) 0x01=UART1
        uint8_t const _Poll_NAV_PVT[8] =       {0xB5, 0x62, 0x01, 0x07, 0x00, 0x00, 0x08, 0x19};        // UBX-NAV-PVT (0x01 0x07) Navigation Position Velocity Time Solution
        uint8_t const _Poll_NAV_POSLLH[8] =    {0xB5, 0x62, 0x01, 0x02, 0x00, 0x00, 0x03, 0x0A};        // UBX-NAV-POSLLH (0x01 0x02) Geodetic Position Solution
        uint8_t const _Poll_NAV_ATT[8] =       {0xB5, 0x62, 0x01, 0x05, 0x00, 0x00, 0x06, 0x13};        // UBX-NAV-ATT (0x01 0x05) Attitude Solution
        // TODO
        //uint8_t const _Poll_CFG_NAV5[8] =      {0xB5, 0x62, 0x06, 0x24, 0x00, 0x00, 0x2A, 0x84};      // UBX-CFG-NAV5 (0x06 0x24) Navigation Engine Settings
        //uint8_t const _Poll_CFG_RATE[8] =      {0xB5, 0x62, 0x06, 0x08, 0x00, 0x00, 0x0E, 0x30};      // UBX-CFG-RATE (0x06 0x08) Navigation/Measurement Rate Settings
        uint8_t const _Poll_MON_IO[8] =        {0xB5, 0x62, 0x0A, 0x02, 0x00, 0x00, 0x0C, 0x2E};        // UBX-MON-IO (0x0A 0x02) Poll I/O Subsystem Status
        uint8_t const _Poll_MON_VER[8] =       {0xB5, 0x62, 0x0A, 0x04, 0x00, 0x00, 0x0E, 0x34};        // UBX-MON-VER (0x0A 0x04) Poll Receiver/Software Version
        uint8_t const _Poll_CFG_TP5[9] =       {0xB5, 0x62, 0x06, 0x31, 0x01, 0x00, 0x00, 0x38, 0xE5};  // UBX-CFG-TP5 (0x06 0x31) Poll Time Pulse Parameters
    
        // ### Periodic auto update ON,OFF command ###
        // CFG-MSG 0x06 0x01
        uint8_t const _Ena_NAV_PVT[11] =       {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x07, 0x01, 0x13, 0x51}; // Enable periodic auto update NAV_PVT
        uint8_t const _Dis_NAV_PVT[11] =       {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x07, 0x00, 0x12, 0x50}; // Disable periodic auto update NAV_PVT
        uint8_t const _Ena_NAV_ATT[11] =       {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x05, 0x01, 0x11, 0x4D}; // Enable periodic auto update NAV_ATT
        uint8_t const _Dis_NAV_ATT[11] =       {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x05, 0x00, 0x10, 0x4C}; // Disable periodic auto update NAV_ATT
        uint8_t const _Ena_NAV_POSLLH[11] =    {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x02, 0x01, 0x0E, 0x47}; // Enable periodic auto update NAV_POSLLH
        uint8_t const _Dis_NAV_POSLLH[11] =    {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x02, 0x00, 0x0D, 0x46}; // Disable periodic auto update NAV_POSLLH
        //uint8_t const _Ena_MON_IO[11] =      {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x0A, 0x02, 0x01, 0x17, 0x62}; // Enable periodic auto update MON_IO (0x0A 0x02)
        //uint8_t const _Dis_MON_IO[11] =      {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x0A, 0x02, 0x00, 0x16, 0x61}; // Disable periodic auto update MON_IO (0x0A 0x02)
    
        // #### NMEA- u-blox Switch OFF ALL NMEA MSGs ####
        // #### ---------------------------------------------------------------------------------------------------- NMEA--Name - Description -------------------------------------------------------- ####
        uint8_t const _Dis_NMEA_GxDTM[11] =    {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x0A, 0x00, 0x04, 0x23}; // NMEA GxDTM (DTM 0xF0 0x0A) Datum Reference)
        uint8_t const _Dis_NMEA_GxGBS[11] =    {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x09, 0x00, 0x03, 0x21}; // NMEA GxGBS (GBS 0xF0 0x09) GNSS Satellite Fault Detection)
        uint8_t const _Dis_NMEA_GxGGA[11] =    {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x00, 0x00, 0xFA, 0x0F}; // NMEA GxGGA (GGA 0xF0 0x00) Global positioning system fix data)
        uint8_t const _Dis_NMEA_GxGLL[11] =    {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x01, 0x00, 0xFB, 0x11}; // NMEA GxGLL (GLL 0xF0 0x01) Latitude and longitude, with time of position fix and status)
        uint8_t const _Dis_NMEA_GxGNS[11] =    {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x0D, 0x00, 0x07, 0x29}; // NMEA GxGNS (GNS 0xF0 0x0D) GNSS fix data)
        uint8_t const _Dis_NMEA_GxGRS[11] =    {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x06, 0x00, 0x00, 0x1B}; // NMEA GxGRS (GRS 0xF0 0x06) GNSS Range Residuals)
        uint8_t const _Dis_NMEA_GxGSA[11] =    {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x02, 0x00, 0xFC, 0x13}; // NMEA GxGSA (GSA 0xF0 0x02) GNSS DOP and Active Satellites)
        uint8_t const _Dis_NMEA_GxGST[11] =    {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x07, 0x00, 0x01, 0x1D}; // NMEA GxGST (GST 0xF0 0x07) GNSS Pseudo Range Error Statistics)
        uint8_t const _Dis_NMEA_GxGSV[11] =    {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x03, 0x00, 0xFD, 0x15}; // NMEA GxGSV (GSV 0xF0 0x03) GNSS Satellites in View)
        uint8_t const _Dis_NMEA_GxRMC[11] =    {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x04, 0x00, 0xFE, 0x17}; // NMEA GxRMC (RMC 0xF0 0x04) Recommended Minimum data)
        uint8_t const _Dis_NMEA_GxVLW[11] =    {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x0F, 0x00, 0x09, 0x2D}; // NMEA GxVLW (VLW 0xF0 0x0F) Dual ground/water distance)
        uint8_t const _Dis_NMEA_GxVTG[11] =    {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x05, 0x00, 0xFF, 0x19}; // NMEA GxVTG (VTG 0xF0 0x05) Course over ground and Ground speed)
        uint8_t const _Dis_NMEA_GxZDA[11] =    {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x08, 0x00, 0x02, 0x1F}; // NMEA GxZDA (ZDA 0xF0 0x08) Time and Date)
        // #### PUBX- u-blox NMEA extension - Switch OFF ALL ####
        uint8_t const _Dis_PUBXe_NMEA00[11] =  {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF1, 0x00, 0x00, 0xFB, 0x12}; // PUBX 00 (Position Data)
        uint8_t const _Dis_PUBXe_NMEA01[11] =  {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF1, 0x01, 0x00, 0xFC, 0x14}; // PUBX 01 (UTM Position Data)
        uint8_t const _Dis_PUBXe_NMEA03[11] =  {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF1, 0x03, 0x00, 0xFE, 0x18}; // PUBX 03 (Satellite Data)
        uint8_t const _Dis_PUBXe_NMEA04[11] =  {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF1, 0x04, 0x00, 0xFF, 0x1A}; // PUBX 04 (Time of Day)
        uint8_t const _Dis_PUBXe_NMEA05[11] =  {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF1, 0x05, 0x00, 0x00, 0x1C}; // PUBX 05 (EKF Status)
        uint8_t const _Dis_PUBXe_NMEA06[11] =  {0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF1, 0x06, 0x00, 0x01, 0x1E}; // PUBX 06 (GPS-only on EKF products)
    
        uint32_t i_atoi(uint8_t s[], uint8_t ptr); // String helper
        double modifiedMap(double _x, double _in_min, double _in_max, double _out_min, double _out_max);
        void DEBUG(); // debug print helper
    };
    #endif
    v1.0.3
    UBLOX_example3.ino - CFG-TP5 (0x06 0x31) Time Pulse Parameters
    Code:
    /*
      UBLOX_example3.ino - CFG-TP5 (0x06 0x31) Time Pulse Parameters
      Brian R Taylor
      brian.taylor@bolderflight.com
      2016-07-06
    
      Copyright (c) 2016 Bolder Flight Systems
    
      Permission is hereby granted, free of charge, to any person obtaining a copy of this software
      and associated documentation files (the "Software"), to deal in the Software without restriction,
      including without limitation the rights to use, copy, modify, merge, publish, distribute,
      sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
      furnished to do so, subject to the following conditions:
    
      The above copyright notice and this permission notice shall be included in all copies or
      substantial portions of the Software.
    
      THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
      BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
      NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
      DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
      OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
    */
    
    /*
      v1.0.3
      Chris O.
      2018-03-29
      Supported Navigation IDs:
      UBX-NAV-PVT ---- (0x01 0x07) Navigation Position Velocity Time Solution
      UBX-NAV-POSLLH - (0x01 0x02) Geodetic Position Solution
      UBX-NAV-ATT ---- (0x01 0x05) Attitude Solution
    
      /---  High level Commands, for the user ---/
       NOTE: command(bool) == (true)Print Message Acknowledged on USB Serial Monitor
      begin(Baud)                       // Starting communication with the GPS
      end();                            // Disables Teensy serial communication, to re-enable, call begin(Baud)
      Poll_CFG_Port1(bool);             // Polls the configuration for one I/O Port, I/O Target 0x01=UART1
      Poll_NAV_PVT();                   // Polls UBX-NAV-PVT    (0x01 0x07) Navigation Position Velocity Time Solution
      Poll_NAV_POSLLH();                // Polls UBX-NAV-POSLLH (0x01 0x02) Geodetic Position Solution
      Poll_NAV_ATT();                   // Polls UBX-NAV-ATT    (0x01 0x05) Attitude Solution
      Poll_MON_IO(bool);                // Polls UBX-MON-IO (0x0A 0x02) I/O Subsystem Status
      Poll_MON_VER(bool);               // Polls UBX-MON-VER (0x0A 0x04) Receiver/Software Version
      Poll_CFG_TP5(bool);               // Polls CFG-TP5        (0x06 0x31) Poll Time Pulse 0 Parameters
      ### Periodic Auto Update ON,OFF Command ###
      Ena_NAV_PVT(bool);                // Enable periodic auto update NAV_PVT
      Dis_NAV_PVT(bool);                // Disable periodic auto update NAV_PVT
      Ena_NAV_ATT(bool);                // Enable periodic auto update NAV_ATT
      Dis_NAV_ATT(bool);                // Disable periodic auto update NAV_ATT
      Ena_NAV_POSLLH(bool);             // Enable periodic auto update NAV_POSLLH
      Dis_NAV_POSLLH(bool);             // Disable periodic auto update NAV_POSLLH
      ### u-blox Switch off all NMEA MSGs ###
      Dis_all_NMEA_Child_MSGs(bool);    // Disable All NMEA Child Messages Command
      ### High level Command Generator ###
      SetGPSbaud(uint32_t baud, bool)   // Set UBLOX GPS Port Configuration Baud rate
      SetNAV5(uint8_t dynModel, bool)   // Set Dynamic platform model Navigation Engine Settings (0:portable, 3:pedestrian, Etc)
      SetRATE(uint16_t measRate, bool)  // Set Navigation/Measurement Rate Settings (100ms=10.00Hz, 200ms=5.00Hz, 1000ms=1.00Hz, Etc)
      SetCFG_TP5(uint32_t FreqLocked, double DutyLocked, uint16_t antCableDelay, bool printACK); // UBX-CFG-TP5 (0x06 0x31) Time Pulse 0 Parameters
      Ena_Dis_MON_IO(bool En~Di, bool) // UBX-MON-IO (0x0A 0x02) Ena/Dis periodic auto update I/O Subsystem Status, bytes(received, sent), parity , framing , overrun)
    */
    
    #include "UBLOX.h"
    
    // The elapsedMillis feature is built into Teensyduino.
    // For non-Teensy boards, it is available as a library.
    elapsedMillis sinceMSG_poll;
    
    // Set this to the GPS hardware serial port you wish to use
    #define GPShwSERIAL 3 // 1 = Serial1, 2 = Serial2, 3 = Serial3, 4 = Serial4 ....
    uint32_t const BaudDefault = 9600; // default settings
    
    // a uBlox object, which is on Teensy hardware
    // GPS serial port
    UBLOX gps(GPShwSERIAL);
    
    // the uBlox data structure
    gpsData uBloxData;
    
    void setup() {
      // serial to display data
      Serial.begin(1500000); // Teensy Serial object always communicates at 12 Mbit/sec USB speed.
      while ( !Serial && (millis() < 10000)) ; // wait until serial monitor is open or timeout 10 seconds
    
      // -- AutoBauding test --
      // Try communication with the GPS
      // receiver at 9600 baud, default settings
      // then set GPS UART Baud to 460800
      gps.begin(BaudDefault);                   // Enable Teensy serial communication @ 9600 baud, default settings.
      gps.SetGPSbaud(1500000, false);           // Set GPS Port Baud, Possible Baud Rate Configurations 4800~9600~19200~38400~57600~115200~230400~460800-921600
      gps.end();                                // Disables Teensy serial communication, to re-enable serial communication, call gps.begin(Baud, bool);.
      gps.begin(19200);
      gps.SetGPSbaud(1500000, false);
      gps.end();
      gps.begin(38400);
      gps.SetGPSbaud(1500000, false);
      gps.end();
      gps.begin(57600);
      gps.SetGPSbaud(1500000, false);
      gps.end();
      gps.begin(115200);
      gps.SetGPSbaud(1500000, false);
      gps.end();
      gps.begin(230400);
      gps.SetGPSbaud(1500000, false);
      gps.end();
      gps.begin(460800);
      gps.SetGPSbaud(1500000, false);
      gps.end();
      gps.begin(921600);
      gps.SetGPSbaud(1500000, true); // 1843200 // experimental baud rate 1500000
      gps.end();
      // now start communication with the GPS
      // receiver at 1500000 baud,
      gps.begin(1500000);                       // Enable Teensy serial communication @ given baud rate
    
      gps.SetRATE(1000, false);                 // Navigation/Measurement Rate Settings, e.g. 100ms => 10Hz, 200 => 5.00Hz, 1000ms => 1Hz, 10000ms => 0.1Hz
      // Possible Configurations:
      // 60=>16.67Hz, 64=>15.63Hz, 72=>13.89Hz, 80=>12.50Hz, 100=>10.00Hz, 125=>8.00Hz, 200=>5.00Hz, 250=>4.00Hz, 500=>2.00Hz
      // 800=>1.25Hz, 1000=>1.00Hz, 2000=>0.50Hz, 4000=>0.25Hz, 10000=>0.10Hz, 20000=>0.05Hz, 50000=>0.02Hz
    
      // NOTE: Dis_all_NMEA -strongly suggest changing RX buffer to 255 or more,*otherwise you will miss ACKs*on serial monitor
      gps.Dis_all_NMEA_Child_MSGs(false);       // Disable All NMEA Child Messages Command
    
      gps.SetNAV5(3, false);                    // Set Dynamic platform model Navigation Engine Settings (0:portable, 2: stationary, 3:pedestrian, Etc)
      // Possible Configurations
      // 0: portable, 2: stationary, 3: pedestrian, 4: automotive, 5: sea, 6: airborne with <1g, 7: airborne with <2g
      // 8: airborne with <4g, 9: wrist worn watch (not supported in protocol v.less than 18)
    
      // ### Periodic auto update ON,OFF Command ###
      gps.Ena_NAV_PVT(true);                    // Enable periodic auto update NAV_PVT
      //gps.Dis_NAV_PVT(false);                 // Disable periodic auto update NAV_PVT
    
      //gps.Ena_NAV_ATT(true);                  // Enable periodic auto update NAV_ATT ~ U-blox M8 from protocol version 19
      gps.Dis_NAV_ATT(false);                   // Disable periodic auto update NAV_ATT ~ ---^
    
      //gps.Ena_NAV_POSLLH(true);               // Enable periodic auto update NAV_POSLLH
      gps.Dis_NAV_POSLLH(false);                // Disable periodic auto update NAV_POSLLH
    
      gps.Ena_Dis_MON_IO(true, true);           // Ena/Dis periodic auto update I/O Subsystem Status, bytes(received, sent), parity errors, framing errors, overrun errors)
    
      gps.Poll_MON_IO(false);                   // Polls UBX-MON-IO (0x0A 0x02) I/O Subsystem Status
      gps.Poll_MON_VER(false);                  // Polls UBX-MON-VER (0x0A 0x04) Receiver/Software Version
    
      // A UBlox GPS Module Primer for beginners https://forum.pjrc.com/threads/46058-A-UBlox-GPS-Module-Primer-for-beginners
      gps.SetCFG_TP5(2000, 50.0000, 50, true);  // UBX-CFG-TP5 (0x06 0x31) - Set Time Pulse 0 Parameters.
      // Possible Configurations:
      // SetCFG_TP5(FreqLocked- 1Hz ~ 24000000Hz, DutyLocked- 0.000000% ~ 100.000000%, antCableDelay- 0~32767ns, print usb ACK- true or false);
    
      gps.Poll_CFG_TP5(false);                  // Polls CFG-TP5        (0x06 0x31) Poll Time Pulse 0 Parameters
    }
    
    void loop() {
      // put your main code here, to run repeatedly:
      // The elapsedMillis feature is built into Teensyduino.
      // For non-Teensy boards, it is available as a library.
      // #### Polling Mechanism ####
      if (sinceMSG_poll >= 10000) { // SEND poll message request every 10 sec.
        sinceMSG_poll = 0; // reset since
        gps.Poll_NAV_PVT();
        gps.Poll_CFG_TP5(true);                 // Polls CFG-TP5        (0x06 0x31) Poll Time Pulse 0 Parameters
      }
    
      if (gps.read(&uBloxData) ) {
        sinceMSG_poll = 0; // reset since
    
        // UBX-CFG-TP5 (0x06 0x31) - Set Time Pulse 0 Parameters.
        Serial.print(" antCableDelay ns: \t");
        Serial.println(uBloxData.antCableDelay);  // antCableDelay
        Serial.print(" freqPeriodL Hz: \t");
        Serial.println(uBloxData.freqPeriodL);  // freqPeriodL
        Serial.print(" duty cycle % \t");
        Serial.println(uBloxData.dutycycleL, 4);  // duty cycle%
        Serial.println();
    
    
        Serial.print(" MON_IO- RX Bytes: "), Serial.print(uBloxData.rxBytes); ///< [B], Number of bytes ever received
        Serial.print("\t TX Bytes: "), Serial.print(uBloxData.txBytes); ///< [B], Number of bytes ever sent
        Serial.print("\t Overrun Errs: "), Serial.println(uBloxData.overrunErrs); ///< [ms], Number of 100ms timeslots with overrun errors
    
        Serial.print(" MON_VER- SW Version: "), Serial.print(uBloxData.swVersion); ///< Software Version
        Serial.print("\t Rev# "), Serial.print(uBloxData.revVersion); ///< Rev#
        Serial.print("\t HW Version u-blox: "), Serial.print(uBloxData.hwVersion); ///< Hardware Version, 7 = u-blox 7
        Serial.print("\t Protocol: "), Serial.println(uBloxData.extension1); ///< Protocol version, e.g. 14.00
        Serial.println();
    
      } else {
        if (sinceMSG_poll == 5000) {
          sinceMSG_poll = 5001; // reset since
          gps.Poll_MON_IO(false);
        }
      }
    }
    Last edited by Chris O.; 03-30-2018 at 04:40 AM.

  20. #45
    Senior Member+
    Join Date
    Jul 2014
    Location
    New York
    Posts
    3,347
    Thanks Chris.

    Sitting here having coffee and just trying to wake up and just notice that you incorporated a polling mechanism You really expanded the library. Lots of work you put in - nice job. Impressive.

    Mike

    EDIT1:
    Here it is, all together. Not official format for Arduino library but...
    Attached Files Attached Files
    Last edited by mjs513; 03-30-2018 at 12:02 PM.

  21. #46
    Senior Member
    Join Date
    Dec 2013
    Posts
    214
    Thanks for zipping, I've been out with the family for a few days.

  22. #47
    Senior Member
    Join Date
    Oct 2017
    Location
    Houston
    Posts
    414
    Mike,

    Thx, was just starting to look at Chris' mods, and was trying to figure out what to grab. Thx!!
    Don

  23. #48
    Senior Member+
    Join Date
    Jul 2014
    Location
    New York
    Posts
    3,347
    Hi Don

    The zip file has everything in it. Chris has done a great job of documentation so take a look at the .h it has a bunch of comments.

    One thing to remember is that it a drop in replacement for the UBLOX library so everything works the same. Also, the changes you make to the M8N are in effect for the until you remove power from the GPS and plug it back it. When power is applied it starts with the configuration you saved through ucenter.

    Mike

  24. #49
    Senior Member
    Join Date
    Oct 2017
    Location
    Houston
    Posts
    414
    Mike,
    Thx! Am headed to apt, but taking the Example sketch to study. Is there any trick to trying out the PPS feature?

    Don

  25. #50
    Senior Member+
    Join Date
    Jul 2014
    Location
    New York
    Posts
    3,347
    Don

    To be honest haven't looked at that piece of the code yet. I would check out Chris's other thread on the UBLOX.

    Mike

Posting Permissions

  • You may not post new threads
  • You may not post replies
  • You may not post attachments
  • You may not edit your posts
  •