openXsensor/oXs_gps.cpp

   1 #include "oXs_gps.h"
   2 #include <avr/pgmspace.h>
   3
   4 #ifdef DEBUG
   5 //#define DEBUGGPS
   6 #define DEBUGPARSEGPS
   7 //#define DEBUGSENDGPS
   8 //#define DEBUG_FORWARD_GPS_MSG_TO_PC
   9 #endif
  10
  11 //#define DEBUGSIMULATEGPS
  12
  13 extern unsigned long micros( void ) ;
  14 extern unsigned long millis( void ) ;
  15 extern void delay(unsigned long ms) ;
  16
  17
  18 // **********************
  19 // GPS data being read
  20 // **********************
  21 int32_t GPS_lon;               // longitude in degree with 7 decimals, (neg for S)
  22 bool    GPS_lonAvailable = false ;
  23 int32_t GPS_lat;               // latitude   in degree with 7 decimals, (neg for ?)
  24 bool    GPS_latAvailable = false ;
  25
  26 int32_t GPS_altitude;              // altitude in mm
  27 bool    GPS_altitudeAvailable = false ;
  28 uint16_t GPS_speed_3d;                 // speed in cm/s
  29 bool    GPS_speed_3dAvailable = false;
  30 uint16_t GPS_speed_2d;                 // speed in cm/s
  31 bool    GPS_speed_2dAvailable = false ;
  32 uint32_t GPS_ground_course ;     // degrees with 5 decimals
  33 bool    GPS_ground_courseAvailable = false ;
  34
  35 #if defined(GPS_TRANSMIT_TIME)
  36 int16_t GPS_year;               // year
  37 int8_t GPS_month;               // month
  38 int8_t GPS_day;                 // day
  39 int8_t GPS_hour;                // hours
  40 int8_t GPS_min;                 // minutes
  41 int8_t GPS_sec;                 // seconds
  42 bool    GPS_timeAvailable = false ;
  43 #endif
  44
  45 uint8_t GPS_numSat;
  46 uint8_t GPS_fix_type;
  47 uint16_t GPS_hdop = 9999;           // Compute GPS quality signal
  48 uint16_t GPS_packetCount = 0;
  49 //uint32_t GPS_svInfoReceivedCount = 0; // SV = Space Vehicle, counter increments each time SV info is received.
  50 //uint8_t GPS_update = 0;             // it's a binary toggle to distinct a GPS position update
  51 //uint8_t GPS_numCh;                          // Number of channels
  52 //uint8_t GPS_svinfo_chn[GPS_SV_MAXSATS];     // Channel number
  53 //uint8_t GPS_svinfo_svid[GPS_SV_MAXSATS];    // Satellite ID
  54 //uint8_t GPS_svinfo_quality[GPS_SV_MAXSATS]; // Bitfield Qualtity
  55 //uint8_t GPS_svinfo_cno[GPS_SV_MAXSATS];     // Carrier to Noise Ratio (Signal Strength)
  56
  57 // *********** GPS calculated data
  58 int16_t GPS_distance ;   // distance from home (first location) in m
  59 int16_t GPS_heading ;          // heading from home (in Rad)
  60 int32_t GPS_home_lat ;         // position of home in degre with 7 decimals
  61 int32_t GPS_home_lon ;         // position of home in degre with 7 decimals
  62 float GPS_scale ;     // scaling factor to calculate the distance depending on latitude
  63 int16_t GPS_bearing ;          // bearing from home in degrees
  64 // scaling factor from 1e-7 degrees to meters at equater
  65 // == 1.0e-7 * DEG_TO_RAD * RADIUS_OF_EARTH
  66 #define LOCATION_SCALING_FACTOR 0.011131884502145034f
  67 #define DEG_TO_RAD_FOR_GPS 0.017453292519943295769236907684886f
  68
  69 bool GPS_fix = false ; // true if gps data are available.
  70 static uint8_t _msg_id; //used to identify the message type when reading the gps, is used also when buffer is parsed
  71
  72 #if defined( A_LOCATOR_IS_CONNECTED)  && ( A_LOCATOR_IS_CONNECTED == YES)
  73 uint32_t GPS_last_fix_millis ; // time when last fix has been received (used by lora locator)
  74 int32_t GPS_last_fix_lon ;     // last lon when a fix has been received
  75 int32_t GPS_last_fix_lat ;     // last lat when a fix has been received
  76 #endif
  77
  78 // Receive buffer
  79 static union {
  80     ubx_nav_posllh posllh;
  81     ubx_nav_status status;
  82     ubx_nav_solution solution;
  83     ubx_nav_pvt pvt;
  84     ubx_nav_velned velned;
  85 #if defined(GPS_TRANSMIT_TIME)
  86     ubx_nav_timeutc timeutc;
  87 #endif
  88     ubx_nav_svinfo svinfo;
  89     uint8_t bytes[UBLOX_BUFFER_SIZE];
  90 } _buffer;
  91
  92
  93 #ifdef DEBUG
  94 OXS_GPS::OXS_GPS(HardwareSerial &print)
  95 #else
  96 OXS_GPS::OXS_GPS( uint8_t x )
  97 #endif
  98 {
  99   // constructor
 100 #ifdef DEBUG
 101   printer = &print; //operate on the address of print
 102 #endif
 103 }
 104
 105 // **************** Setup the GPS sensor *********************
 106 #define SOFT_TX_BIT 6  // define which port is used to send the set up to the GPS (here pin arduino 6 = Port D6 is used
 107 #define SOFT_TX_DDR DDRD  // specify the port being used for this pin
 108 #define SOFT_TX_PORT PORTD  // specify the port being used for this pin
 109
 110 #define MICROSECONDS_OVERHEAD_ADJUST 0
 111
 112 #define MICROSECONDS_PER_BIT_9600 ((1000000ul / 9600) - MICROSECONDS_OVERHEAD_ADJUST)
 113 #define MICROSECONDS_PER_BIT_38400 ((1000000ul / 38400ul) - MICROSECONDS_OVERHEAD_ADJUST)
 114
 115 void gps_putchar (char c, uint16_t usPerBit) {
 116   uint8_t  bit_mask;
 117   SOFT_TX_PORT &= ~(1<<SOFT_TX_BIT); // start bit
 118   delayMicroseconds(usPerBit);
 119   // data bits
 120   for (bit_mask=0x01; bit_mask; bit_mask<<=1) {
 121     if (c & bit_mask) {
 122       SOFT_TX_PORT |= (1<<SOFT_TX_BIT);  // set output to 1
 123     }
 124     else {
 125       SOFT_TX_PORT &= ~(1<<SOFT_TX_BIT); // set output to 0
 126     }
 127     delayMicroseconds(usPerBit);
 128   }
 129   SOFT_TX_PORT |= (1<<SOFT_TX_BIT);   // 2 stop bits
 130   delayMicroseconds(usPerBit * 2);
 131   return ;
 132 }
 133
 134
 135 // send config commands to GPS at 9600 bds (default baud rate)
 136 void OXS_GPS::setupGps( ) {
 137 #if defined(GPS_M10)
 138   const static uint8_t initGps1[] PROGMEM = {
 139     0xB5, 0x62, 0x06, 0x8A, 0x0C, 0x00, 0x01, 0x01, 0x00, 0x00, 0x01, 0x00, 0x52, 0x40, 0x00, 0x96, 0x00, 0x00, 0xC7, 0x2B //set uart speed to 38400
 140   };
 141
 142   const static uint8_t initGps2[] PROGMEM = {
 143     0xB5, 0x62, 0x06, 0x8A, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x02, 0x00, 0x74, 0x10, 0x00, 0x21, 0xC0, //disable nmea on uart
 144     0xB5, 0x62, 0x06, 0x8A, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x2A, 0x00, 0x91, 0x20, 0x01, 0x77, 0x00, //NAV_POSLLH on uart1
 145     0xB5, 0x62, 0x06, 0x8A, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x07, 0x00, 0x91, 0x20, 0x01, 0x54, 0x51, //NAV_PVT on uart1
 146     0xB5, 0x62, 0x06, 0x8A, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x6B, 0x00, 0x91, 0x20, 0x00, 0xB7, 0x44, //disable SBAS status on uart1
 147 //    0xB5, 0x62, 0x06, 0x8A, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x6B, 0x00, 0x91, 0x20, 0x01, 0xB8, 0x45, //SBAS on uart1
 148
 149 #if defined(GPS_TRANSMIT_TIME)
 150     0xB5, 0x62, 0x06, 0x8A, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x5C, 0x00, 0x91, 0x20, 0x01, 0xA9, 0xFA, //TIMEUTC on uart1
 151 #endif
 152     0xB5, 0x62, 0x06, 0x8A, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x43, 0x00, 0x91, 0x20, 0x01, 0x90, 0x7D, //NAV_VELNED on uart1
 153     0xB5, 0x62, 0x06, 0x8A, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x02, 0x00, 0x92, 0x20, 0x00, 0x4F, 0x3A, //disable INFMSG ubx on uart1
 154     0xB5, 0x62, 0x06, 0x8A, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x01, 0x00, 0x73, 0x10, 0x01, 0x20, 0xB9, //UBLOX in uart1
 155     0xB5, 0x62, 0x06, 0x8A, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x01, 0x00, 0x74, 0x10, 0x01, 0x21, 0xBC, //UBLOX out uart1
 156
 157     //satellites configuration, first disable unused ones then enable used (otherways it may fail)
 158     0xB5, 0x62, 0x06, 0x8A, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x22, 0x00, 0x31, 0x10, 0x00, 0xFE, 0x97, //diasble baidoo
 159     0xB5, 0x62, 0x06, 0x8A, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x0D, 0x00, 0x31, 0x10, 0x00, 0xE9, 0x2E, //diasble baidoo b1
 160     0xB5, 0x62, 0x06, 0x8A, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x0F, 0x00, 0x31, 0x10, 0x00, 0xEB, 0x38, //diasble baidoo b10
 161     0xB5, 0x62, 0x06, 0x8A, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x24, 0x00, 0x31, 0x10, 0x00, 0x00, 0xA1, //diasble qzss
 162     0xB5, 0x62, 0x06, 0x8A, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x12, 0x00, 0x31, 0x10, 0x00, 0xEE, 0x47, //diasble qzss l1ca
 163     0xB5, 0x62, 0x06, 0x8A, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x14, 0x00, 0x31, 0x10, 0x00, 0xF0, 0x51, //diasble qzss l1s
 164     0xB5, 0x62, 0x06, 0x8A, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x1F, 0x00, 0x31, 0x10, 0x01, 0xFC, 0x89, //enable gps
 165     0xB5, 0x62, 0x06, 0x8A, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x01, 0x00, 0x31, 0x10, 0x01, 0xDE, 0xF3, //enable gps l1ca
 166     0xB5, 0x62, 0x06, 0x8A, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x20, 0x00, 0x31, 0x10, 0x01, 0xFD, 0x8E, //enable sbas
 167     0xB5, 0x62, 0x06, 0x8A, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x05, 0x00, 0x31, 0x10, 0x01, 0xE2, 0x07, //enable sbas l1ca
 168     0xB5, 0x62, 0x06, 0x8A, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x21, 0x00, 0x31, 0x10, 0x01, 0xFE, 0x93, //enable galileo
 169     0xB5, 0x62, 0x06, 0x8A, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x07, 0x00, 0x31, 0x10, 0x01, 0xE4, 0x11, //enable galileo e1
 170     0xB5, 0x62, 0x06, 0x8A, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x25, 0x00, 0x31, 0x10, 0x01, 0x02, 0xA7, //enable glonass
 171     0xB5, 0x62, 0x06, 0x8A, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x18, 0x00, 0x31, 0x10, 0x01, 0xF5, 0x66, //enable glonass l1
 172
 173 #if defined(GPS_REFRESH_RATE) && (GPS_REFRESH_RATE == 1)
 174     0xB5, 0x62, 0x06, 0x8A, 0x0A, 0x00, 0x01, 0x01, 0x00, 0x00, 0x01, 0x00, 0x21, 0x30, 0xE8, 0x03, 0xD9, 0xCE, // 1Hz
 175     0xB5, 0x62, 0x06, 0x8A, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x16, 0x00, 0x91, 0x20, 0x01, 0x63, 0x9C
 176 #if defined(GPS_TRANSMIT_TIME)
 177     , 0xB5, 0x62, 0x06, 0x8A, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x5C, 0x00, 0x91, 0x20, 0x01, 0xA9, 0xFA
 178 #endif
 179
 180 #elif defined(GPS_REFRESH_RATE) && (GPS_REFRESH_RATE == 10)
 181     0xB5, 0x62, 0x06, 0x8A, 0x0A, 0x00, 0x01, 0x01, 0x00, 0x00, 0x01, 0x00, 0x21, 0x30, 0x64, 0x00, 0x52, 0xC3, // 10Hz
 182     0xB5, 0x62, 0x06, 0x8A, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x16, 0x00, 0x91, 0x20, 0x0A, 0x6C, 0xA5
 183 #if defined(GPS_TRANSMIT_TIME)
 184     , 0xB5, 0x62, 0x06, 0x8A, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x5C, 0x00, 0x91, 0x20, 0x0A, 0xB2, 0x03
 185 #endif defined(GPS_TRANSMIT_TIME)
 186
 187 #else
 188     0xB5, 0x62, 0x06, 0x8A, 0x0A, 0x00, 0x01, 0x01, 0x00, 0x00, 0x01, 0x00, 0x21, 0x30, 0xC8, 0x00, 0xB6, 0x8B,  // 5Hz
 189     0xB5, 0x62, 0x06, 0x8A, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x16, 0x00, 0x91, 0x20, 0x05, 0x67, 0xA0
 190 #if defined(GPS_TRANSMIT_TIME)
 191     ,0xB5, 0x62, 0x06, 0x8A, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x5C, 0x00, 0x91, 0x20, 0x05, 0xAD, 0xFE
 192 #endif
 193 #endif
 194   } ;
 195   uint16_t initGpsIdx = 0 ;
 196   SOFT_TX_PORT |= (1<<SOFT_TX_BIT);   // put 1 in output port
 197   SOFT_TX_DDR |= (1<<SOFT_TX_BIT);    // set port as output
 198   delay(3000) ; // wait to be sure that GPS has started and try to change speed to 39400 (m10 should be already there)
 199   while (initGpsIdx < sizeof( initGps1)) {
 200 //    Serial.println( pgm_read_byte_near(initGps1 + initGpsIdx ), HEX) ;
 201     gps_putchar( pgm_read_byte_near(initGps1 + initGpsIdx++ ), MICROSECONDS_PER_BIT_9600) ; // Send speed initialisation command
 202     delay(10) ;
 203   }
 204   delay(1000) ; // wait to be sure that GPS changed speed and send real config settings
 205   initGpsIdx = 0 ;
 206   while (initGpsIdx < sizeof( initGps2)) {
 207 //    Serial.println( pgm_read_byte_near(initGps2 + initGpsIdx ), HEX) ;
 208     gps_putchar( pgm_read_byte_near(initGps2 + initGpsIdx++ ), MICROSECONDS_PER_BIT_38400) ; // Send initialisation command
 209     delay(10) ;
 210   }
 211
 212 #else
 213       const static uint8_t initGps1[] PROGMEM = {
 214 // Here the code to activate galileo sat. (This has not yet been tested and is based on I-NAV code)
 215         0xB5,0x62,0x06,0x3E, 0x3C, 0x00, // GNSS + number of bytes= 60 dec = 003C in HEx
 216         0x00, 0x00, 0x20, 0x07,  // GNSS / min / max / enable
 217         0x00, 0x08, 0x10, 0x00, 0x01, 0x00, 0x01, 0x01, // GPS / 8 / 16 / Y
 218         0x01, 0x01, 0x03, 0x00, 0x01, 0x00, 0x01, 0x01, // SBAS / 1 / 3 / Y
 219         0x02, 0x04, 0x08, 0x00, 0x01, 0x00, 0x01, 0x01, // Galileo / 4 / 8 / Y
 220         0x03, 0x08, 0x10, 0x00, 0x00, 0x00, 0x01, 0x01, // BeiDou / 8 / 16 / N
 221         0x04, 0x00, 0x08, 0x00, 0x00, 0x00, 0x01, 0x01, // IMES / 0 / 8 / N
 222         0x05, 0x00, 0x03, 0x00, 0x00, 0x00, 0x01, 0x01, // QZSS / 0 / 3 / N
 223         0x06, 0x08, 0x0e, 0x00, 0x01, 0x00, 0x01, 0x01, // GLONASS / 8 / 14 / Y
 224         0x30, 0xD8, // checksum
 225
 226 // Here the code to activate SBAS for Europe (This has not yet been tested and is based on I-NAV code)
 227         0xB5,0x62,0x06,0x16, 0x08, 0x00, // SBAS + number of bytes = 8
 228         0x03, 0x03, 0x03, 0x00, // mode = test + enabled, usage=range+diffcorr, max =3, scanmode2=0
 229         0x00, 0x00, 0x08, 0x51, // scanmode1 120,124, 126, 131
 230         0x86, 0x2C, //checksum
 231
 232 // Here other code
 233         0xB5,0x62,0x06,0x01,0x08,0x00,0x01,0x02,0x00,0x01,0x00,0x00,0x00,0x00,0x13,0xBE, // activate NAV-POSLLH message
 234         0xB5,0x62,0x06,0x01,0x08,0x00,0x01,0x06,0x00,0x01,0x00,0x00,0x00,0x00,0x17,0xDA, //        NAV-SOL
 235         0xB5,0x62,0x06,0x01,0x08,0x00,0x01,0x12,0x00,0x01,0x00,0x00,0x00,0x00,0x23,0x2E, //        NAV-VELNED
 236 #if defined(GPS_TRANSMIT_TIME)
 237         0xB5,0x62,0x06,0x01,0x08,0x00,0x01,0x21,0x00,0x01,0x00,0x00,0x00,0x00,0x32,0x97, //........NAV-TIMEUTC
 238 #endif
 239 #if defined(GPS_REFRESH_RATE) && (GPS_REFRESH_RATE == 1)
 240         0xB5,0x62,0x06,0x08,0x06,0x00,0xE8,0x03,0x01,0x00,0x01,0x00,0x01,0x39,  // NAV-RATE for 1 hz
 241 #elif defined(GPS_REFRESH_RATE) && (GPS_REFRESH_RATE == 10)
 242         0xB5,0x62,0x06,0x08,0x06,0x00,0x64,0x00,0x01,0x00,0x01,0x00,0x7A,0x12, // NAV-RATE for 10 hz
 243 #else
 244         0xB5,0x62,0x06,0x08,0x06,0x00,0xC8,0x00,0x01,0x00,0x01,0x00,0xDE,0x6A, // NAV-RATE for 5 hz
 245 #endif
 246         0xB5,0x62,0x06,0x00,0x14,0x00,0x01,0x00,0x00,0x00,0xD0,0x08,0x00,0x00,0x00,0x96, //        CFG-PRT : Set port to output only UBX (so deactivate NMEA msg) and set baud = 38400.
 247                             0x00,0x00,0x07,0x00,0x01,0x00,0x00,0x00,0x00,0x00,0x91,0x84  //                 rest of CFG_PRT command
 248       }  ;
 249
 250       uint8_t initGpsIdx = 0 ;
 251   SOFT_TX_PORT |= (1<<SOFT_TX_BIT);   // put 1 in output port
 252   SOFT_TX_DDR |= (1<<SOFT_TX_BIT);    // set port as output
 253   delay(3000) ; // wait to be sure that GPS has started
 254   while (initGpsIdx < sizeof( initGps1)) {
 255 //    Serial.println( pgm_read_byte_near(initGps1 + initGpsIdx ), HEX) ;
 256     gps_putchar( pgm_read_byte_near(initGps1 + initGpsIdx++ ), MICROSECONDS_PER_BIT_9600) ; // Send initialisation command
 257     delay(10) ;
 258   }
 259 #endif
 260   delay(1000) ;
 261 #ifdef DEBUG
 262   Serial.println(F("End of GPS setup")) ;
 263 #endif
 264 }
 265
 266
 267 // **************** Read the GPS    *********************
 268
 269 void OXS_GPS::readGps() { // read and process GPS data. do not send them.
 270 #ifdef DEBUGSIMULATEGPS
 271       const static uint8_t simulateGpsBuffer[] PROGMEM = { // POS message
 272         0xB5,0x62,0x01,0x02,0x1C,0x00,0x58,0xF2,0x1A,0x14,0xB0,0x4D,0xA1,0xCF,0xAC,0xD5,0x1F,0x1D,0xE0,0x93,0x04,0x00,0xE0,0x93,0x04,0x00,0x06,0x17,0x00,0x00,0x3C,0x24,0x00,0x00,0x2C,0xF5
 273        ,0xB5,0x62,0x01,0x12,0x24,0x00,0x58,0xF2,0x1A,0x14,0xFC,0x06,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFC,0x06,0x00,0x00,0xFC,0x06,0x00,0x00,0x00,0x00,0x00,0x00,0x11,0x01,0x00,0x00,0x00,0x51,0x25,0x02,0x3F,0x00
 274       }  ;
 275       static uint8_t simuIdx= 0 ;
 276 //      printer->print(simuIdx) ;printer->print(",") ;
 277        gpsNewFrameUBLOX( pgm_read_byte_near(simulateGpsBuffer + simuIdx) );
 278 //  printer->print(F("Gps char: "));
 279 //  printer->println( pgm_read_byte_near(simulateGpsBuffer[simuIdx]),hex );
 280         simuIdx++ ;
 281         if ( simuIdx >= sizeof( simulateGpsBuffer) ) simuIdx = 0  ;
 282 #else
 283        static uint8_t gpsChar ;
 284        while  (Serial.available() ) { // read all bytes in Serial Rx buffer.
 285           gpsChar = Serial.read() ;
 286 #ifdef DEBUG_FORWARD_GPS_MSG_TO_PC
 287           if (gpsChar == 0xB5 ) {
 288               Serial.println() ; Serial.print(gpsChar , HEX) ;
 289           } else {
 290               Serial.print(F(",")) ; Serial.print(gpsChar , HEX) ;
 291           }
 292 #endif // end DEBUG_FORWARD_GPS_MSG_TO_PC
 293           // process each incoming byte and so fill GPS data when a frame is received and correct
 294           gpsNewFrameUBLOX( gpsChar );
 295       }
 296 #endif
 297 }
 298
 299 bool OXS_GPS::gpsNewFrameUBLOX(uint8_t data) // handle one byte and if this byte is the last one of a valid frame, parse the whole frame to GPS data
 300 {
 301   static uint16_t gpsDataErrors;
 302
 303   static uint8_t _ck_a;// Packet checksum accumulators
 304   static uint8_t _ck_b;// Packet checksum accumulators
 305   static bool _skip_packet = false ;
 306   static uint8_t _step;
 307   static uint8_t _class;
 308   static uint16_t _payload_length;
 309   static uint16_t _payload_counter;
 310
 311
 312     bool parsed = false;
 313 //    printer->print(_step);  printer->print(" "); printer->println(data , HEX);
 314     switch (_step) {
 315         case 0: // Sync char 1 (0xB5)
 316             if ( 0xB5 == data ) { // UBX sync char 1
 317                 _skip_packet = false;
 318                 _step++;
 319             }
 320             break;
 321         case 1: // Sync char 2 (0x62)
 322             if ( 0x62 != data) { // UBX sync char 1
 323                 _step = 0;
 324                 break;
 325             }
 326             _step++;
 327             break;
 328         case 2: // Class
 329             _step++;
 330             _class = data;  // normally we should check that the class is the expected (otherwise, frame should be skipped)
 331             _ck_b = _ck_a = data;   // reset the checksum accumulators
 332             if ( 0x01 != data ) { // we handle only message type = 0x01 = NAVigation message.
 333                 _skip_packet = true; // when skip packet = true, then the wholepacket will be read but discarded.
 334             }
 335             break;
 336         case 3: // Id
 337             _step++;
 338             _ck_b += (_ck_a += data);       // checksum byte
 339             _msg_id = data;
 340             break;
 341         case 4: // Payload length (part 1)
 342             _step++;
 343             _ck_b += (_ck_a += data);       // checksum byte
 344             _payload_length = data; // payload length low byte
 345             break;
 346         case 5: // Payload length (part 2)
 347             _step++;
 348             _ck_b += (_ck_a += data);       // checksum byte
 349             _payload_length += (uint16_t)(data << 8);
 350             if (_payload_length > UBLOX_PAYLOAD_SIZE) {
 351                 _skip_packet = true; // when skip packet = true, then the wholepacket will be read but discarded.
 352             }
 353             _payload_counter = 0;   // prepare to receive payload
 354             if (_payload_length == 0) {
 355                 _step = 7;
 356             }
 357             break;
 358         case 6:
 359             _ck_b += (_ck_a += data);       // checksum byte
 360             if  (_payload_counter < UBLOX_BUFFER_SIZE)  {
 361                 _buffer.bytes[_payload_counter] = data; // save the content of the payload
 362 //                printer->print(data , HEX);
 363               }
 364             _payload_counter++ ;
 365             if (_payload_counter >= _payload_length) {
 366                 _step++;
 367 //                printer->println(" ");
 368             }
 369             break;
 370         case 7:
 371             _step++;
 372             if (_ck_a != data) {
 373                 _skip_packet = true;          // bad checksum
 374                 gpsDataErrors++;
 375             }
 376             break;
 377         case 8:
 378             _step = 0;
 379             if (_ck_b != data) {
 380                 gpsDataErrors++;
 381                 break;              // bad checksum
 382             }
 383
 384             GPS_packetCount++;
 385 //            printer->print("pac : ");  printer->print(GPS_packetCount); printer->print(",err: "); printer->print(gpsDataErrors); printer->print(",skip: "); printer->println(_skip_packet) ;
 386
 387             if (_skip_packet) {
 388                 break;   // do not parse the packet to be skipped
 389             }
 390                          // if we arive here, it means that a valid frame has been received and that the gpsBuffer contains the data to be parsed
 391             if (UBLOX_parse_gps() && (_class == 0x01) ) {
 392                 parsed = true;
 393             }
 394     }  // end of case
 395     return parsed; // return true if position AND speed have been received (not sure if this is usefull)
 396 }
 397
 398 bool OXS_GPS::UBLOX_parse_gps(void) // move the data from buffer to the different fields
 399 {
 400 // do we have new position information?
 401 static bool _new_position = false ;
 402
 403 // do we have new speed information?
 404 static bool _new_speed = false ;
 405 static bool next_fix = false ;
 406 #if defined(GPS_TRANSMIT_TIME)
 407 // do we have new time information?
 408 static bool _new_time = false ;
 409 #endif
 410     switch (_msg_id) {
 411     case MSG_POSLLH:
 412         //i2c_dataset.time                = _buffer.posllh.time;
 413         GPS_lon = _buffer.posllh.longitude;           // in degree with 7 decimals
 414         GPS_lat = _buffer.posllh.latitude;            // in degree with 7 decimals
 415         GPS_altitude = _buffer.posllh.altitude_msl ;  //alt in mm
 416         if (next_fix) {                               // enable state if a position has been received after a positieve STATUS or SOL
 417             GPS_fix = true ;
 418             if ( GPS_home_lat == 0 ) {
 419               GPS_home_lat = _buffer.posllh.latitude ;  // save home position
 420               GPS_home_lon = _buffer.posllh.longitude ;
 421               GPS_scale = cosf(GPS_home_lat * 1.0e-7f * DEG_TO_RAD_FOR_GPS); // calculate scale factor based on latitude
 422             }
 423             // Calculate distance
 424             float dlat  = (float)(GPS_home_lat - GPS_lat);
 425             float dlong  = ((float)(GPS_home_lon - GPS_lon)) * GPS_scale ;
 426             GPS_distance =  sqrtf( dlat * dlat + dlong * dlong  ) * LOCATION_SCALING_FACTOR;
 427             // calculate bearing
 428             int32_t off_x = GPS_lon - GPS_home_lon ;
 429             int32_t off_y = (GPS_lat - GPS_home_lat) / GPS_scale ;
 430             GPS_bearing = 90 + atan2f(-off_y, off_x) * 57.2957795f;  // in degree
 431             if (GPS_bearing < 0) GPS_bearing += 360;
 432 #if defined( A_LOCATOR_IS_CONNECTED)  && ( A_LOCATOR_IS_CONNECTED == YES)
 433             GPS_last_fix_millis = millis() ;  // used by lora locator
 434             GPS_last_fix_lon = GPS_lon ;      // used by lora locator
 435             GPS_last_fix_lat = GPS_lat ;      // used by lora locator
 436 #endif
 437         } else {
 438             GPS_fix = false;
 439         }
 440         GPS_lonAvailable = GPS_latAvailable = GPS_altitudeAvailable = GPS_fix;
 441         _new_position = true;
 442 #ifdef DEBUGPARSEGPS
 443   printer->print(F("Gps fix: "));
 444   printer->print(GPS_fix);
 445   printer->print(F(" long: "));
 446   printer->print(GPS_lon);
 447   printer->print(F(" lat: "));
 448   printer->print(GPS_lat);
 449   printer->print(F(" alt: "));
 450   printer->println(GPS_altitude);
 451 #endif
 452         break;
 453 //    case MSG_STATUS:                              // !!!!!!!!! I do not see real need of this message because same (and more) data are in SOL, so this message is not activated in init
 454 //        next_fix = (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.status.fix_type == FIX_3D); // si valid position and speed or 3D fix
 455 //        if (!next_fix)
 456 //             GPS_fix = false;
 457 //        break;
 458     case MSG_SOL:                                // !!!! here we could also use vertical speed which is I4 in cm/sec)
 459         next_fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D);
 460         GPS_fix_type = _buffer.solution.fix_type;
 461         if (!next_fix)
 462              GPS_fix = false;
 463         GPS_numSat = _buffer.solution.satellites;
 464         GPS_hdop = _buffer.solution.position_DOP;
 465         break;
 466     case MSG_PVT:                                // this message does not exist in ublox6 (but SOL does not exist in ublox10)
 467         next_fix = (_buffer.pvt.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.pvt.fix_type == FIX_3D);
 468         GPS_fix_type = _buffer.pvt.fix_type; // use to send or not the data in Hott and ELRS
 469         if (!next_fix)
 470              GPS_fix = false;
 471         GPS_numSat = _buffer.pvt.satellites;
 472         if ( _buffer.pvt.fix_status & NAV_STATUS_FIX_VALID) { // PDOP is valid only when bit 0 =1
 473             GPS_hdop = _buffer.pvt.position_DOP;
 474         }
 475         break;
 476     case MSG_VELNED:   // here we should add the 3d speed (if accurate enough
 477         GPS_speed_3d  = _buffer.velned.speed_3d;  // cm/s
 478         GPS_speed_3dAvailable = GPS_fix ;
 479         GPS_speed_2d = _buffer.velned.speed_2d;    // cm/s
 480         GPS_speed_2dAvailable = GPS_fix ;
 481         GPS_ground_course = _buffer.velned.heading_2d ;     // Heading 2D deg with 5 decimals
 482         GPS_ground_courseAvailable = GPS_fix ;
 483         _new_speed = true;
 484 #ifdef DEBUGPARSEGPS
 485   printer->print(F("Gps spd 3d: "));
 486   printer->print(GPS_speed_3d);
 487   printer->print(F(" 2d: "));
 488   printer->print(GPS_speed_2d);
 489   printer->print(F(" course: "));
 490   printer->println(GPS_ground_course);
 491 #endif
 492         break;
 493 #if defined(GPS_TRANSMIT_TIME)
 494     case MSG_TIMEUTC:   // Parse time informations
 495         if (_buffer.timeutc.flag & 0b111) {
 496           GPS_year=_buffer.timeutc.year;
 497           GPS_month=_buffer.timeutc.month;
 498           GPS_day=_buffer.timeutc.day;
 499           GPS_hour=_buffer.timeutc.hour;
 500           GPS_min=_buffer.timeutc.min;
 501           GPS_sec=_buffer.timeutc.sec;
 502           _new_time=true;
 503           GPS_timeAvailable=true;
 504 #ifdef DEBUGPARSEGPS
 505   printer->print(F("Gps time: "));
 506   printer->print(GPS_year);
 507   printer->print(F("-"));
 508   printer->print(GPS_month);
 509   printer->print(F("-"));
 510   printer->print(GPS_day);
 511   printer->print(F(" "));
 512   printer->print(GPS_hour);
 513   printer->print(F(":"));
 514   printer->print(GPS_min);
 515   printer->print(F(":"));
 516   printer->println(GPS_sec);
 517 #endif
 518         } else {
 519           GPS_timeAvailable=false;
 520         }
 521         break;
 522 #endif
 523     default:
 524         return false;
 525     } // end of case
 526
 527     // we only return true when we get new position and speed data
 528     // this ensures we don't use stale data
 529 #if defined(GPS_TRANSMIT_TIME)
 530     if (_new_position && _new_speed && _new_time) {
 531         _new_speed = _new_position = _new_time = false;
 532         return true;
 533     }
 534 #else
 535     if (_new_position && _new_speed) {
 536         _new_speed = _new_position =  false;
 537         return true;
 538     }
 539 #endif
 540     return false;
 541 }