diff --git a/QAPRSBase.cpp b/QAPRSBase.cpp
index d12c7b6..0d3d576 100644
--- a/QAPRSBase.cpp
+++ b/QAPRSBase.cpp
@@ -65,6 +65,7 @@ uint8_t QAPRSBase::canTransmit(){
  */
 void QAPRSBase::ax25SendHeaderBegin() {
 	this->enableTransmission();
+	timerInterruptHandler();
 	this->ax25CRC = 0xFFFF;
 	this->currentTone = QAPRSMark;
 	//this->currentTone = QAPRSSpace;
@@ -155,20 +156,7 @@ void QAPRSBase::ax25CalcCRC(uint8_t ls_bit) {
  */
 inline void QAPRSBase::toggleTone() {
 	this->currentTone = (this->currentTone == QAPRSSpace) ? QAPRSMark : QAPRSSpace;
-
-  TIM2->CR1 &= (uint16_t)(~((uint16_t)TIM_CR1_CEN));
-
-  uint16_t used = TIM2->CNT;
   this->timer1StartValue = (this->currentTone == QAPRSMark) ? MarkTimerValue : SpaceTimerValue;
-
-  if (used >= this->timer1StartValue){
-    this->timerInterruptHandler();
-  } else {
-    TIM2->ARR = this->timer1StartValue - used;
-    TIM2->CNT = 0;
-  }
-
-  TIM2->CR1 |= TIM_CR1_CEN;
 }
 
 /**
@@ -216,6 +204,7 @@ void QAPRSBase::enableTransmission() {
     #endif
 
   radio_set_tx_frequency(APRS_FREQUENCY);
+  radio_rw_register(0x72, 5, 1);
   GPIO_SetBits(GPIOC, radioNSELpin);
   radio_rw_register(0x71, 0b00010010, 1);
   spi_deinit();
diff --git a/QAPRSBase.h b/QAPRSBase.h
index 1d33cf3..198ef77 100644
--- a/QAPRSBase.h
+++ b/QAPRSBase.h
@@ -58,7 +58,7 @@ private:
 	/**
 	 * @brief ilosć bajtów synchronizacyjnych do nadania przed zawartoscią pakietu
 	 */
-	static const uint8_t ax25HeaderFlagFieldCount1200 = 45;
+	static const uint8_t ax25HeaderFlagFieldCount1200 = 25;
 	/**
 	 * @brief ilosć bajtów synchronizacyjnych do nadania przed zawartoscią pakietu
 	 */
@@ -82,7 +82,7 @@ private:
 #elif F_CPU == 8000000UL
 	static const uint16_t toneSendTime1200 = 785;
 #else
-	static const uint16_t toneSendTime1200 = 717;
+	//static const uint16_t toneSendTime1200 = 1000000/1200;
 #endif
 	/**
 	 * @brief Czas wysyłania podedynczego tonu. W ms.
@@ -182,8 +182,10 @@ public:
 
 private:
   static const uint16_t toneSendTime = 833;
-  static const uint16_t MarkTimerValue = (uint16_t) ((1000000 /  ((1245)*2)) - 1);
-  static const uint16_t SpaceTimerValue = (uint16_t) ((1000000 /  ((2750)*2)) - 1);
+
+	static const uint16_t toneSendTime1200 = 795;
+  static const uint16_t MarkTimerValue = 393;
+  static const uint16_t SpaceTimerValue = 202;
   void togglePin();
   uint8_t pin;
 public:
diff --git a/aprs.cpp b/aprs.cpp
index 0b4701f..f44b9ab 100644
--- a/aprs.cpp
+++ b/aprs.cpp
@@ -39,6 +39,14 @@ void calcDMH(long x, int8_t* degrees, uint8_t* minutes, uint8_t* h_minutes) {
   }
 }
 
+void aprs_test(){
+  char packet_buffer[128];
+  sprintf(packet_buffer,
+          (":TEST1234567890")
+  );
+  qaprs.sendData(packet_buffer);
+}
+
 void aprs_send_position(GPSEntry gpsData, int8_t temperature, uint16_t voltage) {
   char packet_buffer[128];
   int8_t la_degrees, lo_degrees;
diff --git a/aprs.h b/aprs.h
index 15b43a0..49f9752 100644
--- a/aprs.h
+++ b/aprs.h
@@ -17,6 +17,7 @@ extern "C" {
   uint8_t aprs_is_active();
   void aprs_send_position(GPSEntry gpsData, int8_t temperature, uint16_t voltage);
   void aprs_change_tone_time(uint16_t x);
+  void aprs_test();
 #ifdef __cplusplus
 };
 #endif