hab.c

#include <SoftwareSerial.h>



SoftwareSerial myserial(4, 5); // setup software serial for the FTDI module for debugging

byte gps_set_success = 0;

#define uint8_t setNav[] = {   // UBX command to set flight mode
    0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x06, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00,
    0x05, 0x00, 0xFA, 0x00, 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x16, 0xDC };

void setup()
{
    myserial.begin(9600);
    Serial.begin(9600);
    myserial.println("Test Run");
    myserial.println("Initialising...");
    
    while(!gps_set_sucess)
    {
        sendUBX(setNav, sizeof(setNav)/sizeof(uint8_t));
        gps_set_sucess=getUBX_ACK(setNav);
    }
}

gps_set_success = 0;


void loop()
{
  while(1)
  {
    if(Serial.available() > 0) 
    {
      char inByte = Serial.read();
      mySerial.write(inByte);
 
    }
 
  }
}   


void sendUBX(uint8_t *command, uint8_t length) // function to send commands to GPS byte by byte
{
  for(int i=0; i<length; i++) { // for each byte in the byte array
    Serial.write(command[i]); // send the byte to the GPS
    mySerial.print(command[i], HEX);
  }
  Serial.println();
}



void disable_nmea()
{
    // disable automatic NMEA response
    Serial.print("$PUBX,40,GLL,0,0,0,0*5C\r\n");
    Serial.print("$PUBX,40,ZDA,0,0,0,0*44\r\n");
    Serial.print("$PUBX,40,VTG,0,0,0,0*5E\r\n");
    Serial.print("$PUBX,40,GSV,0,0,0,0*59\r\n");
    Serial.print("$PUBX,40,GSA,0,0,0,0*4E\r\n");
    Serial.print("$PUBX,40,RMC,0,0,0,0*47\r\n");
}


boolean getUBX_ACK(uint8_t *MSG)
{
  uint8_t b;
  uint8_t ackByteID = 0;
  uint8_t ackPacket[10];
  unsigned long startTime = millis();
  mySerial.print(" * Reading ACK response: ");
 
  // Construct the expected ACK packet    
  ackPacket[0] = 0xB5;    // header
  ackPacket[1] = 0x62;	// header
  ackPacket[2] = 0x05;	// class
  ackPacket[3] = 0x01;	// id
  ackPacket[4] = 0x02;	// length
  ackPacket[5] = 0x00;
  ackPacket[6] = MSG[2];	// ACK class
  ackPacket[7] = MSG[3];	// ACK id
  ackPacket[8] = 0;		// CK_A
  ackPacket[9] = 0;		// CK_B
 
  // Calculate the checksums
  for (uint8_t i=2; i<8; i++)
  {
    ackPacket[8] = ackPacket[8] + ackPacket[i];
    ackPacket[9] = ackPacket[9] + ackPacket[8];
  }
 
  while (1) 
  {
 
    // Test for success
    if (ackByteID > 9)
    {
      // All packets in order!
      mySerial.println(" (SUCCESS!)");
      return true;
    }
 
    // Timeout if no valid response in 3 seconds
    if (millis() - startTime > 3000) 
    { 
      mySerial.println(" (FAILED!)");
      return false;
    }
 
    // Make sure data is available to read
    if (Serial.available())
    {
      b = Serial.read();
 
      // Check that bytes arrive in sequence as per expected ACK packet
      if (b == ackPacket[ackByteID])
      { 
        ackByteID++;
        mySerial.print(b, HEX);
      } 
      else
      {
        ackByteID = 0;	// Reset and look again, invalid order
      }
    }
  }
}