// GPS initialisation
//

  uint32_t init_speed[5] = {9600,19200,38400,57600,115200};

 #if defined(UBLOX)
   prog_char UBLOX_INIT[] PROGMEM = {                          // PROGMEM array must be outside any function !!!
     0xB5,0x62,0x06,0x01,0x03,0x00,0xF0,0x05,0x00,0xFF,0x19,                            //disable all default NMEA messages
     0xB5,0x62,0x06,0x01,0x03,0x00,0xF0,0x03,0x00,0xFD,0x15,
     0xB5,0x62,0x06,0x01,0x03,0x00,0xF0,0x01,0x00,0xFB,0x11,
     0xB5,0x62,0x06,0x01,0x03,0x00,0xF0,0x00,0x00,0xFA,0x0F,
     0xB5,0x62,0x06,0x01,0x03,0x00,0xF0,0x02,0x00,0xFC,0x13,
     0xB5,0x62,0x06,0x01,0x03,0x00,0xF0,0x04,0x00,0xFE,0x17,
     0xB5,0x62,0x06,0x01,0x03,0x00,0x01,0x02,0x01,0x0E,0x47,                            //set POSLLH MSG rate
     0xB5,0x62,0x06,0x01,0x03,0x00,0x01,0x03,0x01,0x0F,0x49,                            //set STATUS MSG rate
     0xB5,0x62,0x06,0x01,0x03,0x00,0x01,0x06,0x01,0x12,0x4F,                            //set SOL MSG rate
     0xB5,0x62,0x06,0x01,0x03,0x00,0x01,0x12,0x01,0x1E,0x67,                            //set VELNED MSG rate
     0xB5,0x62,0x06,0x16,0x08,0x00,0x03,0x07,0x03,0x00,0x51,0x08,0x00,0x00,0x8A,0x41,   //set WAAS to EGNOS
     0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0xC8, 0x00, 0x01, 0x00, 0x01, 0x00, 0xDE, 0x6A //set rate to 5Hz
   };
 #endif

  void GPS_SerialInit() {
  
  Serial.begin(GPS_SERIAL_SPEED);  
  delay(1000);
  
#if defined(UBLOX)
	//Set speed
      for(uint8_t i=0;i<5;i++){
        Serial.begin(init_speed[i]);          // switch UART speed for sending SET BAUDRATE command (NMEA mode)
        #if (GPS_SERIAL_SPEED==19200)
          Serial.write(PSTR("$PUBX,41,1,0003,0001,19200,0*23\r\n"));     // 19200 baud - minimal speed for 5Hz update rate
        #endif  
        #if (GPS_SERIAL_SPEED==38400)
          Serial.write(PSTR("$PUBX,41,1,0003,0001,38400,0*26\r\n"));     // 38400 baud
        #endif  
        #if (GPS_SERIAL_SPEED==57600)
          Serial.write(PSTR("$PUBX,41,1,0003,0001,57600,0*2D\r\n"));     // 57600 baud
        #endif  
        #if (GPS_SERIAL_SPEED==115200)
          Serial.write(PSTR("$PUBX,41,1,0003,0001,115200,0*1E\r\n"));    // 115200 baud
        #endif  
        delay(300);		//Wait for init 
      }
      delay(200);
      Serial.begin(GPS_SERIAL_SPEED);  
      for(uint8_t i=0; i<sizeof(UBLOX_INIT); i++) {                        // send configuration data in UBX protocol
        Serial.write(pgm_read_byte(UBLOX_INIT+i));
        //delay(5); //simulating a 38400baud pace (or less), otherwise commands are not accepted by the device.
      }

#elif defined(INIT_MTK_GPS)                            // MTK GPS setup
      for(uint8_t i=0;i<5;i++){
        Serial.begin(init_speed[i]);					   // switch UART speed for sending SET BAUDRATE command
        #if (GPS_SERIAL_SPEED==19200)
          Serial.write("$PMTK251,19200*22\r\n");     // 19200 baud - minimal speed for 5Hz update rate
        #endif  
        #if (GPS_SERIAL_SPEED==38400)
          Serial.write("$PMTK251,38400*27\r\n");     // 38400 baud
        #endif  
        #if (GPS_SERIAL_SPEED==57600)
          Serial.write("$PMTK251,57600*2C\r\n");     // 57600 baud
        #endif  
        #if (GPS_SERIAL_SPEED==115200)
          Serial.write("$PMTK251,115200*1F\r\n");    // 115200 baud
        #endif  
        delay(300);
      }
      // at this point we have GPS working at selected (via #define GPS_BAUD) baudrate
      Serial.begin(GPS_SERIAL_SPEED);

	  Serial.write(MTK_NAVTHRES_OFF);
	  delay(100);
      Serial.write(SBAS_ON);
	  delay(100);
      Serial.write(WAAS_ON);
	  delay(100);
      Serial.write(SBAS_TEST_MODE);
	  delay(100);
      Serial.write(MTK_OUTPUT_5HZ);           // 5 Hz update rate
	  delay(100);

      #if defined(NMEA)
        Serial.write(MTK_SET_NMEA_SENTENCES); // only GGA and RMC sentence
      #endif     
	  #if defined(MTK_BINARY19) || defined(MTK_BINARY16)
		Serial.write(MTK_SET_BINARY);
      #endif



#endif     
  }