 bool reinit_done = false;
 int reinitcnt = 8;
 int reinitcntr = 0;
- 
+
 void setup()
-{    
-   pinMode(LED,OUTPUT);
-   digitalWrite(LED, LOW);
-   delay(500);
-   digitalWrite(LED, HIGH);
-   delay(500);
-   
-    initialise_interrupt();
-    
-    Serial.begin(9600);
-    
-    //Setup GPS
-    uint8_t setNav[] = {
-      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};
-    
-    uint8_t ecoMode[] = {0xB5, 0x62, 0x06, 0x11, 0x02, 0x00, 0x00, 0x04, 0x1D, 0x85};
-  
-    while(!gps_set_sucess)
-    {
-      sendUBX(setNav, sizeof(setNav)/sizeof(uint8_t));
-      gps_set_sucess=getUBX_ACK(setNav);
-      /*sendUBX(ecoMode, sizeof(ecoMode)/sizeof(uint8_t));
-      gps_set_sucess&=getUBX_ACK(ecoMode);*/
-    }
-    gps_set_sucess=0;
-    
-   digitalWrite(LED, LOW);
-   delay(500);
-   digitalWrite(LED, HIGH); 
-   delay(500);
-   digitalWrite(LED, LOW);
-   delay(500);
-   digitalWrite(LED, HIGH); 
-   delay(500);
-   digitalWrite(LED, LOW); 
-    
-    while(sats < 6 && flat == 0)
-    {
-      while(Serial.available())
-      {
+{
+  pinMode(LED, OUTPUT);
+  digitalWrite(LED, LOW);
+  delay(500);
+  digitalWrite(LED, HIGH);
+  delay(500);
+
+  initialise_interrupt();
+
+  Serial.begin(9600);
+
+  //Setup GPS
+  uint8_t setNav[] = {
+    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
+  };
+
+  uint8_t ecoMode[] =
+      { 0xB5, 0x62, 0x06, 0x11, 0x02, 0x00, 0x00, 0x04, 0x1D, 0x85 };
+
+  while (!gps_set_sucess) {
+    sendUBX(setNav, sizeof(setNav) / sizeof(uint8_t));
+    gps_set_sucess = getUBX_ACK(setNav);
+    /*sendUBX(ecoMode, sizeof(ecoMode)/sizeof(uint8_t));
+       gps_set_sucess&=getUBX_ACK(ecoMode); */
+  }
+  gps_set_sucess = 0;
+
+  digitalWrite(LED, LOW);
+  delay(500);
+  digitalWrite(LED, HIGH);
+  delay(500);
+  digitalWrite(LED, LOW);
+  delay(500);
+  digitalWrite(LED, HIGH);
+  delay(500);
+  digitalWrite(LED, LOW);
+
+  while (sats < 6 && flat == 0) {
+    while (Serial.available()) {
       char c = Serial.read();
       newData = gps.encode(c);
-        if(newData)
-        {
+      if (newData) {
         gps.f_get_position(&flat, &flon, &age);
         sats = gps.satellites();
+      }
+    }
+  }
+
+  setupGPSpower();
+  delay(1000);
+  //Setup RFM22B
+  setupRadio();
+}
+
+void loop()
+{
+  unsigned long chars;
+  unsigned short sentences, failed;
+
+  while (Serial.available()) {
+    char c = Serial.read();
+    if (gps.encode(c))          // Did a new valid sentence come in?
+      newData = true;
+  }
+
+  if (newData) {
+    digitalWrite(LED, HIGH);
+    gps.f_get_position(&flat, &flon, &age);
+    sats = gps.satellites();
+    dtostrf(flat, 10, 6, latbuf);
+    dtostrf(flon, 9, 6, lonbuf);
+    if (lonbuf[0] == ' ') {
+      lonbuf[0] = '+';
+    }
+    if (latbuf[0] == ' ') {
+      latbuf[0] = '+';
+    }
+    ialt = (gps.altitude() / 100);
+    if (ialt >= 0) {
+      itoa(ialt, altbuf, 10);
+    }
+
+    gps.get_datetime(&date, &time, &age);
+    hour = (time / 1000000);
+    minute = ((time - (hour * 1000000)) / 10000);
+    second = ((time - ((hour * 1000000) + (minute * 10000))));
+    second = second / 100;
+
+    sensorValue = analogRead(analogInPin);
+    actualValue = (sensorValue / 1023.00) * divider;
+    v1 = actualValue;
+    v2 = (actualValue - v1) * 100;
+    snprintf(voltage, sizeof(voltage), "%i.%02i", v1, v2);
+
+    cBusy = true;
+    sprintf(datastring, "$$$$%s,%li,%02i:%02i:%02i,%s,%s,%s,%i,%s",
+            callsign, ticks, hour, minute, second, latbuf, lonbuf, altbuf,
+            sats, voltage);
+    unsigned int CHECKSUM = gps_CRC16_checksum(datastring);     // Calculates the checksum for this datastring
+    char checksum_str[7];
+    sprintf(checksum_str, "*%04X\n", CHECKSUM);
+    strcat(datastring, checksum_str);
+    cBusy = false;
+
+    gps.stats(&chars, &sentences, &failed);
+
+  } else {
+    digitalWrite(LED, LOW);
+  }
+  if (reinit == true) {
+    setupRadio();
+    reinit = false;
+  }
+
+}
+
+ISR(TIMER1_COMPA_vect)
+{
+  switch (txstatus) {
+  case 0:                      // This is the optional delay between transmissions.
+    txj++;
+    if (txj > (TXDELAY * RTTY_BAUD)) {
+      txj = 0;
+      txstatus = 1;
+    }
+    break;
+  case 1:                      // Initialise transmission, take a copy of the string so it doesn't change mid transmission.
+    if (reinit == false) {
+      if (cBusy == false) {
+        if (reinitcntr == reinitcnt) {
+          reinit = true;
+          reinitcntr = 0;
+        } else {
+          strcpy(txstring, datastring);
+          txstringlength = strlen(txstring);
+          if (txstringlength != 0)
+            txj = 0;
+          ticks++;
+          txstatus = 2;
+          reinitcntr++;
         }
       }
     }
-  
-    setupGPSpower();
-    delay(1000);
-    //Setup RFM22B
-    setupRadio();
-}
- 
-void loop()
-{
-    unsigned long chars;
-    unsigned short sentences, failed;
-  
-    while(Serial.available())
-    {
-    char c = Serial.read();
-    if (gps.encode(c)) // Did a new valid sentence come in?
-    newData = true;
-    }
-  
-    if (newData)
-    {
-      digitalWrite(LED,HIGH);
-      gps.f_get_position(&flat, &flon, &age);
-      sats = gps.satellites();
-      dtostrf(flat, 10, 6, latbuf);
-      dtostrf(flon, 9, 6, lonbuf);
-      if(lonbuf[0] == ' ')
-            {
-              lonbuf[0] = '+';
-            }
-      if(latbuf[0] == ' ')
-            {
-              latbuf[0] = '+';
-            }
-      ialt = (gps.altitude() / 100);
-      if(ialt >= 0)
-      {
-        itoa(ialt, altbuf, 10);
-      }
-      
-      gps.get_datetime(&date, &time, &age);
-              hour = (time / 1000000);
-              minute = ((time - (hour * 1000000)) / 10000);
-              second = ((time - ((hour * 1000000) + (minute * 10000))));
-              second = second / 100;
-              
-        sensorValue = analogRead(analogInPin);                    
-        actualValue = (sensorValue / 1023.00) * divider;
-        v1 = actualValue;
-        v2 = (actualValue - v1) * 100;
-        snprintf(voltage, sizeof(voltage), "%i.%02i", v1, v2);
-  
-    cBusy = true;
-    sprintf(datastring,"$$$$%s,%li,%02i:%02i:%02i,%s,%s,%s,%i,%s",callsign,ticks,hour,minute,second,latbuf,lonbuf,altbuf,sats,voltage); 
-    unsigned int CHECKSUM = gps_CRC16_checksum(datastring);  // Calculates the checksum for this datastring
-    char checksum_str[7];
-    sprintf(checksum_str, "*%04X\n", CHECKSUM);
-    strcat(datastring,checksum_str);
-    cBusy = false;
-    
-    gps.stats(&chars, &sentences, &failed);
-  
-  }
-  else
-  {
-    digitalWrite(LED,LOW);
-  }
-  if(reinit == true)
-  {
-    setupRadio();
-    reinit = false;
-  }
-  
-}
- 
-ISR(TIMER1_COMPA_vect)
-{
-  switch(txstatus) {
-  case 0: // This is the optional delay between transmissions.
-    txj++;
-    if(txj>(TXDELAY*RTTY_BAUD)) {
-      txj=0;
-      txstatus=1;
-    }
     break;
-  case 1: // Initialise transmission, take a copy of the string so it doesn't change mid transmission.
-  if(reinit == false)
-  {
-    if(cBusy == false)
-    {
-    if(reinitcntr == reinitcnt)
-    {
-      reinit = true;
-      reinitcntr = 0;
-    }
-    else
-    {
-      strcpy(txstring,datastring);
-      txstringlength=strlen(txstring);
-      if(txstringlength != 0)
-      txj=0;
-      ticks++;
-      txstatus=2;
-      reinitcntr++;
-    }
-    }
-   }
-    break;
-  case 2: // Grab a char and lets go transmit it.
-    if ( txj < txstringlength)
-    {
+  case 2:                      // Grab a char and lets go transmit it.
+    if (txj < txstringlength) {
       txc = txstring[txj];
       txj++;
-      txstatus=3;
-      rtty_txbit (0); // Start Bit;
-      txi=0;
-    }
-    else
-    {
-      txstatus=0; // Should be finished
-      txj=0;
+      txstatus = 3;
+      rtty_txbit(0);            // Start Bit;
+      txi = 0;
+    } else {
+      txstatus = 0;             // Should be finished
+      txj = 0;
     }
     break;
   case 3:
-    if(txi<ASCII)
-    {
+    if (txi < ASCII) {
       txi++;
-      if (txc & 1) rtty_txbit(1);
-      else rtty_txbit(0);  
+      if (txc & 1)
+        rtty_txbit(1);
+      else
+        rtty_txbit(0);
       txc = txc >> 1;
       break;
-    }
-    else
-    {
-      rtty_txbit (1); // Stop Bit
-      txstatus=4;
-      txi=0;
+    } else {
+      rtty_txbit(1);            // Stop Bit
+      txstatus = 4;
+      txi = 0;
       break;
     }
   case 4:
-    if(STOPBITS==2)
-    {
-      rtty_txbit (1); // Stop Bit
-      txstatus=2;
+    if (STOPBITS == 2) {
+      rtty_txbit(1);            // Stop Bit
+      txstatus = 2;
       break;
-    }
-    else
-    {
-      txstatus=2;
+    } else {
+      txstatus = 2;
       break;
     }
- 
-  }
-}
- 
-void rtty_txbit (int bit)
-{
-  if (bit)
-  {
-    radio1.write(0x73,0x03); // High
-  }
-  else
-  {
-    radio1.write(0x73,0x00); // Low
-  }
-}
-
-void setupGPSpower() {
+
+  }
+}
+
+void rtty_txbit(int bit)
+{
+  if (bit) {
+    radio1.write(0x73, 0x03);   // High
+  } else {
+    radio1.write(0x73, 0x00);   // Low
+  }
+}
+
+void setupGPSpower()
+{
   //Set GPS ot Power Save Mode
-  uint8_t setPSM[] = { 0xB5, 0x62, 0x06, 0x11, 0x02, 0x00, 0x08, 0x01, 0x22, 0x92 }; // Setup for Power Save Mode (Default Cyclic 1s)
- 
-  sendUBX(setPSM, sizeof(setPSM)/sizeof(uint8_t));
-}
- 
-void setupRadio(){
-  pinMode(RFM22B_SDN, OUTPUT);    // RFM22B SDN is on ARDUINO A3
+  uint8_t setPSM[] = { 0xB5, 0x62, 0x06, 0x11, 0x02, 0x00, 0x08, 0x01, 0x22, 0x92 };    // Setup for Power Save Mode (Default Cyclic 1s)
+
+  sendUBX(setPSM, sizeof(setPSM) / sizeof(uint8_t));
+}
+
+void setupRadio()
+{
+  pinMode(RFM22B_SDN, OUTPUT);  // RFM22B SDN is on ARDUINO A3
   digitalWrite(RFM22B_SDN, LOW);
   delay(1000);
   rfm22::initSPI();
   radio1.init();
-  radio1.write(0x71, 0x00); // unmodulated carrier
+  radio1.write(0x71, 0x00);     // unmodulated carrier
   //This sets up the GPIOs to automatically switch the antenna depending on Tx or Rx state, only needs to be done at start up
-  radio1.write(0x0b,0x12);
-  radio1.write(0x0c,0x15);
+  radio1.write(0x0b, 0x12);
+  radio1.write(0x0c, 0x15);
   radio1.setFrequency(RADIO_FREQUENCY);
-  radio1.write(0x6D, 0x04);// turn tx low power 11db
+  radio1.write(0x6D, 0x04);     // turn tx low power 11db
   radio1.write(0x07, 0x08);
   delay(500);
 }
- 
-uint16_t gps_CRC16_checksum (char *string)
+
+uint16_t gps_CRC16_checksum(char *string)
 {
   size_t i;
   uint16_t crc;
   uint8_t c;
- 
+
   crc = 0xFFFF;
- 
+
   // Calculate checksum ignoring the first four $s
-  for (i = 4; i < strlen(string); i++)
-  {
+  for (i = 4; i < strlen(string); i++) {
     c = string[i];
-    crc = _crc_xmodem_update (crc, c);
-  }
- 
+    crc = _crc_xmodem_update(crc, c);
+  }
+
   return crc;
-}   
+}
+
 void initialise_interrupt()
 {
   // initialize Timer1
-  cli();          // disable global interrupts
-  TCCR1A = 0;     // set entire TCCR1A register to 0
-  TCCR1B = 0;     // same for TCCR1B
-  OCR1A = F_CPU / 1024 / RTTY_BAUD - 1;  // set compare match register to desired timer count:
-  TCCR1B |= (1 << WGM12);   // turn on CTC mode:
+  cli();                        // disable global interrupts
+  TCCR1A = 0;                   // set entire TCCR1A register to 0
+  TCCR1B = 0;                   // same for TCCR1B
+  OCR1A = F_CPU / 1024 / RTTY_BAUD - 1; // set compare match register to desired timer count:
+  TCCR1B |= (1 << WGM12);       // turn on CTC mode:
   // Set CS10 and CS12 bits for:
   TCCR1B |= (1 << CS10);
   TCCR1B |= (1 << CS12);
   // enable timer compare interrupt:
   TIMSK1 |= (1 << OCIE1A);
-  sei();          // enable global interrupts
+  sei();                        // enable global interrupts
 }
 
 // Send a byte array of UBX protocol to the GPS
-void sendUBX(uint8_t *MSG, uint8_t len) {
-  for(int i=0; i<len; i++) {
+void sendUBX(uint8_t * MSG, uint8_t len)
+{
+  for (int i = 0; i < len; i++) {
     Serial.write(MSG[i]);
     //mySerial.print(MSG[i], HEX);
   }

#define RFM22B_PIN 10
#define LED 7

char callsign[8] = "pFALCON"; //Callsign

char datastring[80];
char txstring[80];
volatile int txstatus=1;
volatile int txstringlength=0;
volatile char txc;
volatile int txi;
volatile int txj;
unsigned int count=0;
 
TinyGPS gps;
rfm22 radio1(RFM22B_PIN);


}

// Calculate expected UBX ACK packet and parse UBX response from GPS
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
      }

    }
  }
}
