diff --git a/fun.c b/fun.c
index e3fa2ac..2f7fa05 100644
--- a/fun.c
+++ b/fun.c
@@ -13,44 +13,27 @@ int HexCharToInt(char ch) {
 }
 
 void print(char *s) {
+  #ifdef DEBUG
   while (*s) {
     while (USART_GetFlagStatus(USART3, USART_FLAG_TC) == RESET) {
     }
     USART_SendData(USART3, *(s++));
   }
+  #endif
 }
 
 void send_hex(unsigned char data) {
+  #ifdef DEBUG
   while (USART_GetFlagStatus(USART3, USART_FLAG_TC) == RESET) {
   }
   USART_SendData(USART3, ascii[data >> 4]);
   while (USART_GetFlagStatus(USART3, USART_FLAG_TC) == RESET) {
   }
   USART_SendData(USART3, ascii[data & 0x0f]);
+  #endif
 }
 
 
-unsigned char czytaj_GPS(unsigned char pos, unsigned char len, char *source, char *destination) {
-  char *wyn;
-  char pet = 0;
-  wyn = source;
-  while (pos-- != 0) {
-    while (*(wyn++) != ',');
-  }
-  if (*(wyn) == 0) {
-    return 0;
-  } else {
-    while ((*(wyn) != ',') && (*(wyn) != 0) && (len-- != 0)) {
-      *(destination++) = *(wyn++);
-      pet = 1;
-    }
-    if (pet == 0) {
-      return 0;
-    }
-    *(destination) = 0;
-  }
-  return 1;
-}
 
 uint16_t gps_CRC16_checksum(char *string) {
   uint16_t crc = 0xffff;
@@ -59,7 +42,7 @@ uint16_t gps_CRC16_checksum(char *string) {
     crc = crc ^ (*(string++) << 8);
     for (i = 0; i < 8; i++) {
       if (crc & 0x8000)
-        crc = (crc << 1) ^ 0x1021;
+        crc = (uint16_t) ((crc << 1) ^ 0x1021);
       else
         crc <<= 1;
     }
diff --git a/init.c b/init.c
index 2874c8d..65e1a43 100644
--- a/init.c
+++ b/init.c
@@ -18,9 +18,12 @@ DMA_InitTypeDef DMA_InitStructure;
 
 #define ADC1_DR_Address    ((uint32_t)0x4001244C)
 
-void init_usart_gps(const uint32_t speed) {
+void init_usart_gps(const uint32_t speed, const uint8_t enable_irq) {
   NVIC_DisableIRQ(USART1_IRQn);
-  USART_ITConfig(USART1, USART_IT_RXNE, DISABLE);
+	USART_ITConfig(USART1, USART_IT_RXNE, DISABLE);
+	USART_ClearITPendingBit(USART1, USART_IT_RXNE);
+	USART_ClearITPendingBit(USART1, USART_IT_ORE);
+
   USART_Cmd(USART1, DISABLE);
 
   RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE);// | RCC_APB2Periph_AFIO, ENABLE);
@@ -33,8 +36,10 @@ void init_usart_gps(const uint32_t speed) {
   USART_Init(USART1, &USART_InitStructure);
 
   USART_Cmd(USART1, ENABLE);
-  USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);
-  NVIC_EnableIRQ(USART1_IRQn);
+  if (enable_irq){
+    USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);
+    NVIC_EnableIRQ(USART1_IRQn);
+  }
 }
 
 void init_usart_debug() {
@@ -126,7 +131,7 @@ void init_port()
 	GPIO_Conf.GPIO_Mode = GPIO_Mode_IN_FLOATING;
 	GPIO_Init(GPIOA, &GPIO_Conf);
 
-  init_usart_gps(9600);
+  init_usart_gps(9600, 0);
 
   GPIO_Conf.GPIO_Pin = GPIO_Pin_10;
 	GPIO_Conf.GPIO_Mode = GPIO_Mode_AF_PP;
diff --git a/init.h b/init.h
index ddb136e..4256468 100644
--- a/init.h
+++ b/init.h
@@ -3,4 +3,4 @@ void NVIC_Conf();
 void RCC_Conf();
 void init_port();
 void init_timer(const int rtty_speed);
-void init_usart_gps(const uint32_t speed);
+void init_usart_gps(const uint32_t speed, const uint8_t enable_irq);
diff --git a/main.c b/main.c
index 0ac9413..6179028 100644
--- a/main.c
+++ b/main.c
@@ -32,22 +32,19 @@ unsigned int send_cun;        //frame counter
 char status[2] = {'N'};
 int napiecie;
 
-char flaga = ((((tx_delay / 1000) & 0x0f) << 3) | Smoc);
+volatile char flaga = ((((tx_delay / 1000) & 0x0f) << 3) | Smoc);
 uint16_t CRC_rtty = 0x12ab;  //checksum
 char buf_rtty[200];
 char menu[] = "$$$$$$STM32 RTTY tracker by Blasiu, enjoy and see you on the HUB... \n\r";
 char init_trx[] = "\n\rPowering up TX\n\r";
-unsigned char pun = 0;
-unsigned int cun = 10;
+volatile unsigned char pun = 0;
+volatile unsigned int cun = 10;
 unsigned char dev = 0;
 volatile unsigned char tx_on = 0;
-unsigned int tx_on_delay;
+volatile unsigned int tx_on_delay;
 volatile unsigned char tx_enable = 0;
 rttyStates send_rtty_status = rttyZero;
-unsigned char cun_rtty = 0;
-char *rtty_buf;
-unsigned char GPS_temp;
-char GPS_buf[200];
+volatile char *rtty_buf;
 unsigned char cun_off = 0;
 
 
@@ -57,8 +54,12 @@ void processGPS();
  * GPS data processing
  */
 void USART1_IRQHandler(void) {
-  if ((USART1->SR & USART_FLAG_RXNE) != (u16) RESET) {
+  if (USART_GetITStatus(USART1, USART_IT_RXNE) != RESET) {
     ublox_handle_incoming_byte((uint8_t) USART_ReceiveData(USART1));
+  }else if (USART_GetITStatus(USART1, USART_IT_ORE) != RESET) {
+    USART_ReceiveData(USART1);
+  } else {
+	  USART_ReceiveData(USART1);
   }
 }
 
@@ -67,10 +68,9 @@ void TIM2_IRQHandler(void) {
     TIM_ClearITPendingBit(TIM2, TIM_IT_Update);
   }
   if (tx_on /*&& ++cun_rtty == 17*/) {
-    cun_rtty = 0;
-    send_rtty_status = send_rtty(rtty_buf);
+    send_rtty_status = send_rtty((char *) rtty_buf);
     if (send_rtty_status == rttyEnd) {
-      GPIO_SetBits(GPIOB, RED);
+      GPIO_ResetBits(GPIOB, RED);
       if (*(++rtty_buf) == 0) {
         tx_on = 0;
         tx_on_delay = tx_delay / (1000/RTTY_SPEED);//2500;
@@ -90,7 +90,6 @@ void TIM2_IRQHandler(void) {
     tx_on_delay--;
   }
   if (--cun == 0) {
-    cun_off++;
     if (pun) {
       GPIO_ResetBits(GPIOB, GREEN);
       pun = 0;
@@ -123,14 +122,11 @@ int main(void) {
 
   GPIO_SetBits(GPIOB, RED);
   USART_SendData(USART3, 0xc);
-  print(menu);
   radio_rw_register(0x02, 0xff, 0);
-  //send_hex(temp);
 
   radio_rw_register(0x03, 0xff, 0);
   radio_rw_register(0x04, 0xff, 0);
   radio_soft_reset();
-  print(init_trx);
   // programowanie czestotliwosci nadawania
   radio_set_tx_frequency();
 
@@ -152,7 +148,7 @@ int main(void) {
   tx_enable = 1;
   //tx_enable =0;
   //Button = ADCVal[1];
-
+  radio_enable_tx();
 
   while (1) {
     if (tx_on == 0 && tx_enable) {
@@ -171,11 +167,14 @@ int main(void) {
       uint32_t lat_fl = (uint32_t) abs(abs(gpsData.lat_raw) - lat_d * 10000000) / 100;
       uint8_t lon_d = (uint8_t) abs(gpsData.lon_raw / 10000000);
       uint32_t lon_fl = (uint32_t) abs(abs(gpsData.lon_raw) - lon_d * 10000000) / 100;
-      sprintf(buf_rtty, "$$$$$$$%s,%d,%02u%02u%02u,%s%d.%05ld,%s%d.%05ld,%ld,%d,%d,%d,%02x", callsign, send_cun,
+
+      sprintf(buf_rtty, "$$$$$$$%s,%d,%02u%02u%02u,%s%d.%05ld,%s%d.%05ld,%ld,%d,%d,%d,%d,%d,%02x", callsign, send_cun,
               gpsData.hours, gpsData.minutes, gpsData.seconds,
               gpsData.lat_raw < 0 ? "-" : "", lat_d, lat_fl,
               gpsData.lon_raw < 0 ? "-" : "", lon_d, lon_fl,
-              (gpsData.alt_raw / 1000), temperatura, napiecie, gpsData.sats_raw, flaga);
+              (gpsData.alt_raw / 1000), temperatura, napiecie, gpsData.sats_raw,
+              gpsData.ok_packets, gpsData.bad_packets,
+              flaga);
       CRC_rtty = 0xffff;                                              //napiecie      flaga
       CRC_rtty = gps_CRC16_checksum(buf_rtty + 7);
       sprintf(buf_rtty, "%s*%04X\n", buf_rtty, CRC_rtty & 0xffff);
diff --git a/radio.c b/radio.c
index 6f05a53..87a1791 100644
--- a/radio.c
+++ b/radio.c
@@ -1,5 +1,5 @@
 //
-// Created by Admin on 2016-12-24.
+// Created by SQ5RWU on 2016-12-24.
 //
 
 #include "radio.h"
diff --git a/radio.h b/radio.h
index 5dc74c1..d962141 100644
--- a/radio.h
+++ b/radio.h
@@ -1,5 +1,5 @@
 //
-// Created by Admin on 2016-12-24.
+// Created by SQ5RWU on 2016-12-24.
 //
 
 #ifndef STM32_RTTY_RADIO_H
diff --git a/ublox.c b/ublox.c
index cd24430..460c1ad 100644
--- a/ublox.c
+++ b/ublox.c
@@ -1,5 +1,5 @@
 //
-// Created by Admin on 2016-12-27.
+// Created by SQ5RWU on 2016-12-27.
 //
 
 #include <stm32f10x_usart.h>
@@ -14,6 +14,8 @@ void _sendSerialByte(uint8_t message) {
   while (USART_GetFlagStatus(USART1, USART_FLAG_TC) == RESET) {
   }
   USART_SendData(USART1, message);
+  while (USART_GetFlagStatus(USART1, USART_FLAG_TC) == RESET) {
+  }
 }
 
 void send_ublox(uint8_t msgClass, uint8_t msgId, uint8_t *payload, uint16_t payloadSize) {
@@ -73,11 +75,11 @@ void ublox_init(){
   _delay_ms(800);
 
   uBloxPacket msfcgprt = {.header = {0xb5, 0x62, .messageClass=0x06, .messageId=0x00, .payloadSize=sizeof(uBloxCFGPRTPayload)},
-      .data.cfgprt = {.portID=1, .reserved1=0, .txReady=0, .mode=0b00100011000000, .baudRate=57600,
+      .data.cfgprt = {.portID=1, .reserved1=0, .txReady=0, .mode=0b00100011000000, .baudRate=38400,
           .inProtoMask=1, .outProtoMask=1, .flags=0, .reserved2={0,0}}};
   send_ublox_packet(&msfcgprt);
-  _delay_ms(10);
-  init_usart_gps(57600);
+  init_usart_gps(38400, 1);
+
   _delay_ms(10);
 
   uBloxPacket msgcfgmsg = {.header = {0xb5, 0x62, .messageClass=0x06, .messageId=0x01, .payloadSize=sizeof(uBloxCFGMSGPayload)},
@@ -104,7 +106,7 @@ void ublox_init(){
 void ublox_handle_incoming_byte(uint8_t data){
   static uint8_t sync = 0;
   static uint8_t buffer_pos = 0;
-  volatile static uint8_t  incoming_packet_buffer[sizeof(uBloxPacket) + sizeof(uBloxChecksum)];
+  static uint8_t incoming_packet_buffer[sizeof(uBloxPacket) + sizeof(uBloxChecksum)];
   static uBloxPacket * incoming_packet = (uBloxPacket *) incoming_packet_buffer;
   if (!sync){
     if (!buffer_pos && data == 0xB5){
@@ -137,8 +139,9 @@ void ublox_handle_packet(uBloxPacket *pkt) {
   uBloxChecksum cksum = ublox_calc_checksum(pkt->header.messageClass, pkt->header.messageId, (const uint8_t *) &pkt->data, pkt->header.payloadSize);
   uBloxChecksum *checksum = (uBloxChecksum *)(((uint8_t*)&pkt->data) + pkt->header.payloadSize);
   if (cksum.ck_a != checksum->ck_a || cksum.ck_b != checksum->ck_b) {
-    currentGPSData.fix = 0xf;
+    currentGPSData.bad_packets += 1;
   } else {
+    currentGPSData.ok_packets += 1;
     if (pkt->header.messageClass == 0x01 && pkt->header.messageId == 0x07){
       currentGPSData.fix = pkt->data.navpvt.fixType;
       currentGPSData.lat_raw = pkt->data.navpvt.lat;
diff --git a/ublox.h b/ublox.h
index 6c69ba7..eb90da8 100644
--- a/ublox.h
+++ b/ublox.h
@@ -1,5 +1,5 @@
 //
-// Created by Admin on 2016-12-27.
+// Created by SQ5RWU on 2016-12-27.
 //
 
 #ifndef STM32_RTTY_UBLOX_H
@@ -16,6 +16,8 @@ typedef struct {
   uint8_t minutes;
   uint8_t hours;
   uint8_t fix;
+  uint16_t ok_packets;
+  uint16_t bad_packets;
 } GPSEntry;
 
 typedef struct  __attribute__((packed)){