diff --git a/.vscode/arduino.json b/.vscode/arduino.json
index 09e7173..e8e6b14 100644
--- a/.vscode/arduino.json
+++ b/.vscode/arduino.json
@@ -1,6 +1,6 @@
 {
     "board": "bsfrance:avr:lora32u4",
     "sketch": "crossbow/crossbow.ino",
-    "port": "COM11",
+    "port": "COM7",
     "output": "../build"
 }
\ No newline at end of file
diff --git a/.vscode/settings.json b/.vscode/settings.json
index 77f0cd2..72a79e6 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -1,7 +1,14 @@
 {
     "files.associations": {
         "variables.h": "c",
-        "arduino.h": "c"
+        "arduino.h": "c",
+        "algorithm": "cpp",
+        "random": "cpp",
+        "type_traits": "cpp",
+        "iterator": "cpp",
+        "rope": "c",
+        "__locale": "c",
+        "string": "c"
     },
     "files.exclude": {
         "**/build": true
diff --git a/crossbow/crossbow.ino b/crossbow/crossbow.ino
index 643e353..4b53ad0 100644
--- a/crossbow/crossbow.ino
+++ b/crossbow/crossbow.ino
@@ -9,6 +9,7 @@ Copyright (c) 20xx, MPL Contributor1 contrib1@example.net
 #include "config.h"
 
 #include "lora.h"
+#include "radio_node.h"
 #include "variables.h"
 #include "main_variables.h"
 #include "qsp.h"
@@ -32,6 +33,8 @@ Copyright (c) 20xx, MPL Contributor1 contrib1@example.net
     #error please select hardware
 #endif
 
+RadioNode radioNode;
+
 /*
  * Main defines for device working in TX mode
  */
@@ -83,62 +86,22 @@ Tactile button1(BUTTON_1_PIN);
 QspConfiguration_t qsp = {};
 RxDeviceState_t rxDeviceState = {};
 TxDeviceState_t txDeviceState = {};
-volatile RadioState_t radioState = {};
 
-uint8_t tmpBuffer[MAX_PACKET_SIZE];
-
-uint8_t getRadioRssi(void)
-{
-    return 164 - constrain(LoRa.packetRssi() * -1, 0, 164);
-}
-
-uint8_t getRadioSnr(void)
-{
-    return (uint8_t) constrain(LoRa.packetSnr(), 0, 255);
-}
-
-uint32_t getFrequencyForChannel(uint8_t channel) {
-    return RADIO_FREQUENCY_MIN + (RADIO_CHANNEL_WIDTH * channel);
-}
-
-uint8_t getNextChannel(uint8_t channel) {
-    return (channel + RADIO_HOP_OFFSET) % RADIO_CHANNEL_COUNT;
-}
-
-uint8_t getPrevChannel(uint8_t channel) {
-    return (RADIO_CHANNEL_COUNT + channel - RADIO_HOP_OFFSET) % RADIO_CHANNEL_COUNT;
-}
-
-void hopFrequency(volatile RadioState_t *radioState, bool forward, uint8_t fromChannel, uint32_t timestamp) {
-    radioState->channelEntryMillis = timestamp;
-
-    if (forward) {
-        radioState->channel = getNextChannel(fromChannel);
-    } else {
-        radioState->channel = getPrevChannel(fromChannel);
-    }
-
-    // And set hardware
-    LoRa.sleep();
-    LoRa.setFrequency(
-        getFrequencyForChannel(radioState->channel)
-    );
-    LoRa.idle();
-}
-
-void onQspSuccess(QspConfiguration_t *qsp, TxDeviceState_t *txDeviceState, RxDeviceState_t *rxDeviceState, volatile RadioState_t *radioState) {
+void onQspSuccess(QspConfiguration_t *qsp, TxDeviceState_t *txDeviceState, RxDeviceState_t *rxDeviceState, uint8_t receivedChannel) {
     //If recide received a valid frame, that means it can start to talk
+    radioNode.lastReceivedChannel = receivedChannel;
+    
     qsp->canTransmit = true;
 
-    radioState->rssi = getRadioRssi();
-    radioState->snr = getRadioSnr();
+    radioNode.readRssi();
+    radioNode.readSnr();
 
     /*
      * RX module hops to next channel after frame has been received
      */
 #ifdef DEVICE_MODE_RX
-    hopFrequency(radioState, true, radioState->lastReceivedChannel, millis());
-    radioState->failedDwellsCount = 0; // We received a frame, so we can just reset this counter
+    radioNode.hopFrequency(true, radioNode.lastReceivedChannel, millis());
+    radioNode.failedDwellsCount = 0; // We received a frame, so we can just reset this counter
     LoRa.receive(); //Put radio back into receive mode
 #endif
 
@@ -178,7 +141,7 @@ void onQspSuccess(QspConfiguration_t *qsp, TxDeviceState_t *txDeviceState, RxDev
     qsp->transmitWindowOpen = true;
 }
 
-void onQspFailure(QspConfiguration_t *qsp, TxDeviceState_t *txDeviceState, RxDeviceState_t *rxDeviceState, volatile RadioState_t *radioState) {
+void onQspFailure(QspConfiguration_t *qsp, TxDeviceState_t *txDeviceState, RxDeviceState_t *rxDeviceState) {
 
 }
 
@@ -197,30 +160,7 @@ void setup(void)
     qsp.deviceState = DEVICE_STATE_OK;
 #endif
 
-    /*
-     * Setup hardware
-     */
-    LoRa.setPins(
-        LORA_SS_PIN,
-        LORA_RST_PIN,
-        LORA_DI0_PIN
-    );
-
-    if (!LoRa.begin(getFrequencyForChannel(radioState.channel))) {
-        while (true);
-    }
-
-    //Configure LoRa module
-    LoRa.setSignalBandwidth(radioState.loraBandwidth);
-    LoRa.setSpreadingFactor(radioState.loraSpreadingFactor);
-    LoRa.setCodingRate4(radioState.loraCodingRate);
-    LoRa.setTxPower(radioState.loraTxPower);
-    LoRa.enableCrc();
-
-    //Setup ISR callback and start receiving
-    LoRa.onReceive(onReceive);
-    LoRa.receive();
-    radioState.deviceState = RADIO_STATE_RX;
+    radioNode.init(LORA_SS_PIN, LORA_RST_PIN, LORA_DI0_PIN, onReceive);
 
 #ifdef DEVICE_MODE_RX
     //initiallize default ppm values
@@ -244,7 +184,6 @@ void setup(void)
 #ifdef FEATURE_TX_OLED
     oled.init();
     oled.page(
-        &radioState,
         &rxDeviceState,
         &txDeviceState,
         TX_PAGE_INIT
@@ -346,7 +285,18 @@ void loop(void)
 
     uint32_t currentMillis = millis();
 
-#ifdef DEVICE_MODE_TX
+#ifdef DEVICE_MODE_RX
+    /*
+     * This routine handles resync of TX/RX while hoppping frequencies
+     */
+    radioNode.handleChannelDwell();
+
+    /*
+     * Detect the moment when radio module stopped transmittig and put it
+     * back in to receive state
+     */
+    radioNode.handleTxDoneState(false);
+#else 
 
     //Process buttons
     button0.loop();
@@ -354,7 +304,6 @@ void loop(void)
 
 #ifdef FEATURE_TX_OLED
     oled.loop(
-        &radioState,
         &rxDeviceState,
         &txDeviceState,
         &button0,
@@ -376,67 +325,15 @@ void loop(void)
         txInput.restart();
         serialRestartMillis = currentMillis;
     }
+
+    radioNode.handleTxDoneState(true);
 #endif
 
-    /*
-     * This routine handles resync of TX/RX while hoppping frequencies
-     */
-#ifdef DEVICE_MODE_RX
-
-    //In the beginning just keep jumping forward and try to resync over lost single frames
-    if (radioState.failedDwellsCount < 6 && radioState.channelEntryMillis + RX_CHANNEL_DWELL_TIME < currentMillis) {
-        radioState.failedDwellsCount++;
-        hopFrequency(&radioState, true, radioState.channel, radioState.channelEntryMillis + RX_CHANNEL_DWELL_TIME);
-        LoRa.receive();
-    }
-
-    // If we are loosing more frames, start jumping in the opposite direction since probably we are completely out of sync now
-    if (radioState.failedDwellsCount >= 6 && radioState.channelEntryMillis + (RX_CHANNEL_DWELL_TIME * 5) < currentMillis) {
-        hopFrequency(&radioState, false, radioState.channel, radioState.channelEntryMillis + RX_CHANNEL_DWELL_TIME); //Start jumping in opposite direction to resync
-        LoRa.receive();
-    }
-
-#endif
-
-    /*
-     * Detect the moment when radio module stopped transmittig and put it
-     * back in to receive state
-     */
-    if (
-        currentMillis > radioState.nextTxCheckMillis &&
-        radioState.deviceState == RADIO_STATE_TX &&
-        !LoRa.isTransmitting()
-    ) {
-
-        /*
-         * In case of TX module, hop right now
-         */
-#ifdef DEVICE_MODE_TX
-        hopFrequency(&radioState, true, radioState.channel, millis());
-#endif
-
-        LoRa.receive();
-        radioState.deviceState = RADIO_STATE_RX;
-        radioState.nextTxCheckMillis = currentMillis + 1; //We check of TX done every 1ms
-    }
-
-    /*
-     * There is data to be read from radio!
-     */
-    if (radioState.bytesToRead != NO_DATA_TO_READ) {
-        LoRa.read(tmpBuffer, radioState.bytesToRead);
-
-        for (int i = 0; i < radioState.bytesToRead; i++) {
-            qspDecodeIncomingFrame(&qsp, tmpBuffer[i], &rxDeviceState, &txDeviceState, &radioState);
-        }
-
-        //After reading, flush radio buffer, we have no need for whatever might be over there
-        LoRa.sleep();
-        LoRa.receive();
-        radioState.deviceState = RADIO_STATE_RX;
-
-        radioState.bytesToRead = NO_DATA_TO_READ;
-    }
+    radioNode.readAndDecode(
+        &qsp,
+        &rxDeviceState,
+        &txDeviceState
+    );
 
     bool transmitPayload = false;
 
@@ -455,11 +352,11 @@ void loop(void)
     txInput.loop();
 
     if (
-        radioState.deviceState == RADIO_STATE_RX &&
+        radioNode.deviceState == RADIO_STATE_RX &&
         qsp.protocolState == QSP_STATE_IDLE &&
         qsp.lastTxSlotTimestamp + TX_TRANSMIT_SLOT_RATE < currentMillis
     ) {
-
+        
         int8_t frameToSend = getFrameToTransmit(&qsp);
 
     #ifndef FORCE_TX_WITHOUT_INPUT
@@ -503,7 +400,7 @@ void loop(void)
         lastRxStateTaskTime = currentMillis;
         updateRxDeviceState(&rxDeviceState);
 
-        uint8_t output = constrain(radioState.rssi - 40, 0, 100);
+        uint8_t output = constrain(radioNode.rssi - 40, 0, 100);
 
         rxDeviceState.indicatedRssi = (output * 10) + 1000;
         if (qsp.deviceState == DEVICE_STATE_FAILSAFE) {
@@ -534,7 +431,7 @@ void loop(void)
                     break;
 
                 case QSP_FRAME_RX_HEALTH:
-                    encodeRxHealthPayload(&qsp, &rxDeviceState, &radioState);
+                    encodeRxHealthPayload(&qsp, &rxDeviceState, radioNode.rssi, radioNode.snr);
                     break;
             }
 
@@ -554,7 +451,7 @@ void loop(void)
     if (qsp.lastFrameReceivedAt[QSP_FRAME_RC_DATA] + RX_FAILSAFE_DELAY < currentMillis) {
         qsp.deviceState = DEVICE_STATE_FAILSAFE;
         rxDeviceState.indicatedRssi = 0;
-        radioState.rssi = 0;
+        radioNode.rssi = 0;
     } else {
         qsp.deviceState = DEVICE_STATE_OK;
     }
@@ -563,17 +460,7 @@ void loop(void)
 
     if (qsp.canTransmit && transmitPayload)
     {
-        uint8_t size;
-        LoRa.beginPacket();
-        //Prepare packet
-        qspEncodeFrame(&qsp, &radioState, tmpBuffer, &size);
-        //Sent it to radio in one SPI transaction
-        LoRa.write(tmpBuffer, size);
-        LoRa.endPacketAsync();
-
-        //Set state to be able to detect the moment when TX is done
-        radioState.deviceState = RADIO_STATE_TX;
-
+        radioNode.handleTx(&qsp);
         transmitPayload = false;
     }
 
@@ -616,9 +503,9 @@ void loop(void)
     } else if (txDeviceState.isReceiving && (rxDeviceState.flags & 0x1) == 1) {
         //Failsafe reported by RX module
         buzzerContinousMode(BUZZER_MODE_SLOW_BEEP, &buzzer);
-    } else if (txDeviceState.isReceiving && radioState.rssi < 45) {
+    } else if (txDeviceState.isReceiving && radioNode.rssi < 45) {
         buzzerContinousMode(BUZZER_MODE_DOUBLE_CHIRP, &buzzer); // RSSI below 45dB // Critical state
-    } else if (txDeviceState.isReceiving && radioState.rssi < 55) {
+    } else if (txDeviceState.isReceiving && radioNode.rssi < 55) {
         buzzerContinousMode(BUZZER_MODE_CHIRP, &buzzer); // RSSI below 55dB // Warning state
     } else {
         buzzerContinousMode(BUZZER_MODE_OFF, &buzzer);
@@ -651,18 +538,18 @@ void onReceive(int packetSize)
      * We can start reading only when radio is not reading.
      * If not reading, then we might start
      */
-    if (radioState.bytesToRead == NO_DATA_TO_READ) {
+    if (radioNode.bytesToRead == NO_DATA_TO_READ) {
 
         if (packetSize >= MIN_PACKET_SIZE && packetSize <= MAX_PACKET_SIZE) {
             //We have a packet candidate that might contain a valid QSP packet
-            radioState.bytesToRead = packetSize;
+            radioNode.bytesToRead = packetSize;
         } else {
             /*
             That packet was not very interesting, just flush it, we have no use
             */
             LoRa.sleep();
             LoRa.receive();
-            radioState.deviceState = RADIO_STATE_RX;
+            radioNode.deviceState = RADIO_STATE_RX;
         }
     }
 }
\ No newline at end of file
diff --git a/crossbow/qsp.cpp b/crossbow/qsp.cpp
index 90a8186..fa4dc8d 100644
--- a/crossbow/qsp.cpp
+++ b/crossbow/qsp.cpp
@@ -62,9 +62,9 @@ void qspComputeCrc(QspConfiguration_t *qsp, uint8_t dataByte)
     qsp->crc = crc8_dvb_s2(qsp->crc, dataByte);
 }
 
-void encodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceState, volatile RadioState_t *radioState) {
-    qsp->payload[0] = radioState->rssi;
-    qsp->payload[1] = radioState->snr;
+void encodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceState, uint8_t rssi, uint8_t snr) {
+    qsp->payload[0] = rssi;
+    qsp->payload[1] = snr;
     qsp->payload[2] = rxDeviceState->rxVoltage;
     qsp->payload[3] = rxDeviceState->a1Voltage;
     qsp->payload[4] = rxDeviceState->a2Voltage;
@@ -165,8 +165,7 @@ void qspDecodeIncomingFrame(
     QspConfiguration_t *qsp, 
     uint8_t incomingByte, 
     RxDeviceState_t *rxDeviceState, 
-    TxDeviceState_t *txDeviceState,
-    volatile RadioState_t *radioState
+    TxDeviceState_t *txDeviceState
 ) {
     static uint8_t frameId;
     static uint8_t payloadLength;
@@ -210,10 +209,9 @@ void qspDecodeIncomingFrame(
     {
         if (qsp->crc == incomingByte) {
             //CRC is correct
-            radioState->lastReceivedChannel = receivedChannel;
-            qsp->onSuccessCallback(qsp, txDeviceState, rxDeviceState, radioState);
+            qsp->onSuccessCallback(qsp, txDeviceState, rxDeviceState, receivedChannel);
         } else {
-            qsp->onFailureCallback(qsp, txDeviceState, rxDeviceState, radioState);
+            qsp->onFailureCallback(qsp, txDeviceState, rxDeviceState);
         }
 
         // In both cases switch to listening for next preamble
@@ -224,14 +222,14 @@ void qspDecodeIncomingFrame(
 /**
  * Encode frame is corrent format and write to hardware
  */
-void qspEncodeFrame(QspConfiguration_t *qsp, volatile RadioState_t *radioState, uint8_t buffer[], uint8_t *size) {
+void qspEncodeFrame(QspConfiguration_t *qsp, uint8_t buffer[], uint8_t *size, uint8_t radioChannel) {
     //Salt CRC with bind key
     qspInitCrc(qsp);
 
     //Write frame type and length
     // We are no longer sending payload length, so 4 bits are now free for other usages
     // uint8_t data = qsp->payloadLength & 0x0f;
-    uint8_t data = radioState->channel;
+    uint8_t data = radioChannel;
     data |= (qsp->frameToSend << 4) & 0xf0;
     qspComputeCrc(qsp, data);
     buffer[0] = data;
diff --git a/crossbow/qsp.h b/crossbow/qsp.h
index 60eff2a..8d6d29b 100644
--- a/crossbow/qsp.h
+++ b/crossbow/qsp.h
@@ -1,4 +1,6 @@
 #include "Arduino.h"
+#include "variables.h"
+#include "radio_node.h"
 
 void qspDecodeRcDataFrame(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceSate);
 void decodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceState);
@@ -6,16 +8,15 @@ void decodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceSta
 uint8_t get10bitHighShift(uint8_t channel);
 uint8_t get10bitLowShift(uint8_t channel);
 void qspComputeCrc(QspConfiguration_t *qsp, uint8_t dataByte);
-void encodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceState, volatile RadioState_t *radioState);
+void encodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceState, uint8_t rssi, uint8_t snr);
 void encodeRcDataPayload(QspConfiguration_t *qsp, volatile int16_t channels[], uint8_t noOfChannels);
 void qspDecodeIncomingFrame(
     QspConfiguration_t *qsp, 
     uint8_t incomingByte, 
     RxDeviceState_t *rxDeviceState,
-    TxDeviceState_t *txDeviceState,
-    volatile RadioState_t *radioState
+    TxDeviceState_t *txDeviceState
 );
 void qspClearPayload(QspConfiguration_t *qsp);
-void qspEncodeFrame(QspConfiguration_t *qsp, volatile RadioState_t *radioState,  uint8_t buffer[], uint8_t *size);
+void qspEncodeFrame(QspConfiguration_t *qsp, uint8_t buffer[], uint8_t *size, uint8_t radioChannel);
 
 void encodePingPayload(QspConfiguration_t *qsp, uint32_t currentMicros);
\ No newline at end of file
diff --git a/crossbow/radio_node.cpp b/crossbow/radio_node.cpp
new file mode 100644
index 0000000..772dec8
--- /dev/null
+++ b/crossbow/radio_node.cpp
@@ -0,0 +1,157 @@
+#include "radio_node.h"
+#include "lora.h"
+
+RadioNode::RadioNode(void) {
+
+}
+
+void RadioNode::init(uint8_t ss, uint8_t rst, uint8_t di0, void(*callback)(int)) {
+    /*
+     * Setup hardware
+     */
+    LoRa.setPins(
+        ss,
+        rst,
+        di0
+    );
+
+    if (!LoRa.begin(getFrequencyForChannel(getChannel()))) {
+        while (true);
+    }
+
+    //Configure LoRa module
+    LoRa.setSignalBandwidth(loraBandwidth);
+    LoRa.setSpreadingFactor(loraSpreadingFactor);
+    LoRa.setCodingRate4(loraCodingRate);
+    LoRa.setTxPower(loraTxPower);
+    LoRa.enableCrc();
+
+    //Setup ISR callback and start receiving
+    LoRa.onReceive(callback);
+    LoRa.receive();
+    deviceState = RADIO_STATE_RX;
+}
+
+void RadioNode::readRssi(void)
+{
+    rssi = 164 - constrain(LoRa.packetRssi() * -1, 0, 164);
+}
+
+void RadioNode::readSnr(void)
+{
+    snr = (uint8_t) constrain(LoRa.packetSnr(), 0, 255);
+}
+
+uint8_t RadioNode::getChannel(void) {
+    return _channel;
+}
+
+uint32_t RadioNode::getChannelEntryMillis(void) {
+    return _channelEntryMillis;
+}
+
+uint32_t RadioNode::getFrequencyForChannel(uint8_t channel) {
+    return RADIO_FREQUENCY_MIN + (RADIO_CHANNEL_WIDTH * channel);
+}
+
+uint8_t RadioNode::getNextChannel(uint8_t channel) {
+    return (channel + RADIO_HOP_OFFSET) % RADIO_CHANNEL_COUNT;
+}
+
+uint8_t RadioNode::getPrevChannel(uint8_t channel) {
+    return (RADIO_CHANNEL_COUNT + channel - RADIO_HOP_OFFSET) % RADIO_CHANNEL_COUNT;
+}
+
+void RadioNode::readAndDecode(
+    QspConfiguration_t *qsp,
+    RxDeviceState_t *rxDeviceState,
+    TxDeviceState_t *txDeviceState
+) {
+    uint8_t tmpBuffer[MAX_PACKET_SIZE];
+    /*
+     * There is data to be read from radio!
+     */
+    if (bytesToRead != NO_DATA_TO_READ) {
+        LoRa.read(tmpBuffer, bytesToRead);
+
+        for (int i = 0; i < bytesToRead; i++) {
+            qspDecodeIncomingFrame(qsp, tmpBuffer[i], rxDeviceState, txDeviceState);
+        }
+
+        //After reading, flush radio buffer, we have no need for whatever might be over there
+        LoRa.sleep();
+        LoRa.receive();
+
+        deviceState = RADIO_STATE_RX;
+        bytesToRead = NO_DATA_TO_READ;
+    }
+}
+
+void RadioNode::hopFrequency(bool forward, uint8_t fromChannel, uint32_t timestamp) {
+    _channelEntryMillis = timestamp;
+
+    if (forward) {
+        _channel = RadioNode::getNextChannel(fromChannel);
+    } else {
+        _channel = RadioNode::getPrevChannel(fromChannel);
+    }
+
+    // And set hardware
+    LoRa.sleep();
+    LoRa.setFrequency(
+        RadioNode::getFrequencyForChannel(_channel)
+    );
+    LoRa.idle();
+}
+
+void RadioNode::handleChannelDwell(void) {
+    //In the beginning just keep jumping forward and try to resync over lost single frames
+    if (failedDwellsCount < 6 && getChannelEntryMillis() + RX_CHANNEL_DWELL_TIME < millis()) {
+        failedDwellsCount++;
+        hopFrequency(true, getChannel(), getChannelEntryMillis() + RX_CHANNEL_DWELL_TIME);
+        LoRa.receive();
+    }
+
+    // If we are loosing more frames, start jumping in the opposite direction since probably we are completely out of sync now
+    if (failedDwellsCount >= 6 && getChannelEntryMillis() + (RX_CHANNEL_DWELL_TIME * 5) < millis()) {
+        hopFrequency(false, getChannel(), getChannelEntryMillis() + RX_CHANNEL_DWELL_TIME); //Start jumping in opposite direction to resync
+        LoRa.receive();
+    }
+}
+
+void RadioNode::handleTxDoneState(bool hop) {
+    uint32_t currentMillis = millis();
+    
+    if (
+        currentMillis > nextTxCheckMillis &&
+        deviceState == RADIO_STATE_TX &&
+        !LoRa.isTransmitting()
+    ) {
+
+        /*
+         * In case of TX module, hop right now
+         */
+        if (hop) {
+            hopFrequency(true, getChannel(), currentMillis);
+        }
+
+        LoRa.receive();
+        deviceState = RADIO_STATE_RX;
+        nextTxCheckMillis = currentMillis + 1; //We check of TX done every 1ms
+    }
+}
+
+void RadioNode::handleTx(QspConfiguration_t *qsp) {
+    uint8_t size;
+    uint8_t tmpBuffer[MAX_PACKET_SIZE];
+
+    LoRa.beginPacket();
+    //Prepare packet
+    qspEncodeFrame(qsp, tmpBuffer, &size, getChannel());
+    //Sent it to radio in one SPI transaction
+    LoRa.write(tmpBuffer, size);
+    LoRa.endPacketAsync();
+
+    //Set state to be able to detect the moment when TX is done
+    deviceState = RADIO_STATE_TX;
+}
\ No newline at end of file
diff --git a/crossbow/radio_node.h b/crossbow/radio_node.h
new file mode 100644
index 0000000..e720c5c
--- /dev/null
+++ b/crossbow/radio_node.h
@@ -0,0 +1,54 @@
+#pragma once
+
+#include "Arduino.h"
+#include "qsp.h"
+
+#define RADIO_FREQUENCY_MIN 868000000
+#define RADIO_FREQUENCY_MAX 870000000
+#define RADIO_FREQUENCY_RANGE (RADIO_FREQUENCY_MAX-RADIO_FREQUENCY_MIN)
+#define RADIO_CHANNEL_WIDTH 250000
+#define RADIO_CHANNEL_COUNT 9 // 9 channels in 2MHz range (RADIO_FREQUENCY_RANGE/RADIO_CHANNEL_WIDTH) + 1
+#define RADIO_HOP_OFFSET 5
+
+#ifndef RADIO_NODE_H
+#define RADIO_NODE_H
+
+#include "variables.h"
+
+class RadioNode {
+    public:
+        RadioNode(void);
+        void init(uint8_t ss, uint8_t rst, uint8_t di0, void(*callback)(int));
+        void readRssi(void);
+        void readSnr(void);
+        uint32_t getFrequencyForChannel(uint8_t channel);
+        uint8_t getNextChannel(uint8_t channel);
+        uint8_t getPrevChannel(uint8_t channel);
+        void hopFrequency(bool forward, uint8_t fromChannel, uint32_t timestamp);
+        void readAndDecode(
+            QspConfiguration_t *qsp,
+            RxDeviceState_t *rxDeviceState,
+            TxDeviceState_t *txDeviceState
+        );
+        uint8_t getChannel(void);
+        uint32_t getChannelEntryMillis(void);
+        void handleChannelDwell(void);
+        void handleTxDoneState(bool hop);
+        void handleTx(QspConfiguration_t *qsp);
+        volatile int8_t bytesToRead = -1;
+        volatile uint8_t deviceState = RADIO_STATE_RX;
+        uint8_t rssi = 0;
+        uint8_t snr = 0;
+        uint8_t lastReceivedChannel = 0;
+        uint8_t failedDwellsCount = 0;
+        uint32_t loraBandwidth = 250000;
+        uint8_t loraSpreadingFactor = 7;
+        uint8_t loraCodingRate = 6;
+        uint8_t loraTxPower = 17; // Defines output power of TX, defined in dBm range from 2-17
+    private:
+        uint8_t _channel = 0;
+        uint32_t _channelEntryMillis = 0;
+        uint32_t nextTxCheckMillis = 0;
+};
+
+#endif
\ No newline at end of file
diff --git a/crossbow/tx_oled.cpp b/crossbow/tx_oled.cpp
index cd4e930..35bbd1c 100644
--- a/crossbow/tx_oled.cpp
+++ b/crossbow/tx_oled.cpp
@@ -13,7 +13,6 @@ void TxOled::init() {
 }
 
 void TxOled::loop(
-    volatile RadioState_t *radioState,
     RxDeviceState_t *rxDeviceState,
     TxDeviceState_t *txDeviceState,
     Tactile *button0,
@@ -48,7 +47,6 @@ void TxOled::loop(
 
     if (update) {
         page(
-            radioState,
             rxDeviceState,
             txDeviceState,
             pageSequence[_mainPageSequenceIndex]
@@ -58,26 +56,25 @@ void TxOled::loop(
 }
 
 void TxOled::page(
-    volatile RadioState_t *radioState,
     RxDeviceState_t *rxDeviceState,
     TxDeviceState_t *txDeviceState,
     int page
 ) {
     switch (page) {
         case TX_PAGE_INIT:
-            renderPageInit(radioState, rxDeviceState, txDeviceState);
+            renderPageInit(rxDeviceState, txDeviceState);
             break;
         case TX_PAGE_STATS:
-            renderPageStats(radioState, rxDeviceState, txDeviceState);
+            renderPageStats(rxDeviceState, txDeviceState);
             break;
         case TX_PAGE_PWR:
-            renderPagePwr(radioState, rxDeviceState, txDeviceState);
+            renderPagePwr(rxDeviceState, txDeviceState);
             break;
         case TX_PAGE_BIND:
-            renderPageBind(radioState, rxDeviceState, txDeviceState);
+            renderPageBind(rxDeviceState, txDeviceState);
             break;
         case TX_PAGE_MODE:
-            renderPageMode(radioState, rxDeviceState, txDeviceState);
+            renderPageMode(rxDeviceState, txDeviceState);
             break;
 
     }
@@ -85,7 +82,6 @@ void TxOled::page(
 }
 
 void TxOled::renderPagePwr(
-    volatile RadioState_t *radioState,
     RxDeviceState_t *rxDeviceState,
     TxDeviceState_t *txDeviceState
 ) {
@@ -99,14 +95,13 @@ void TxOled::renderPagePwr(
     //TODO content
     _display.setCursor(0, 25);
     _display.setTextSize(3);
-    _display.print(radioState->loraTxPower);
+    _display.print(radioNode.loraTxPower);
     _display.print("dBm");
 
     _display.display();
 }
 
 void TxOled::renderPageBind(
-    volatile RadioState_t *radioState,
     RxDeviceState_t *rxDeviceState,
     TxDeviceState_t *txDeviceState
 ) {
@@ -123,7 +118,6 @@ void TxOled::renderPageBind(
 }
 
 void TxOled::renderPageMode(
-    volatile RadioState_t *radioState,
     RxDeviceState_t *rxDeviceState,
     TxDeviceState_t *txDeviceState
 ) {
@@ -142,7 +136,6 @@ void TxOled::renderPageMode(
 }
 
 void TxOled::renderPageStats(
-    volatile RadioState_t *radioState,
     RxDeviceState_t *rxDeviceState,
     TxDeviceState_t *txDeviceState
 ) {
@@ -151,11 +144,11 @@ void TxOled::renderPageStats(
     
     _display.setCursor(0, 0);
     _display.setTextSize(3);
-    _display.print(radioState->rssi);
+    _display.print(radioNode.rssi);
 
     _display.setCursor(18, 28);
     _display.setTextSize(2);
-    _display.print(radioState->snr);
+    _display.print(radioNode.snr);
 
     _display.setCursor(74, 0);
     _display.setTextSize(3);
@@ -173,7 +166,6 @@ void TxOled::renderPageStats(
 }
 
 void TxOled::renderPageInit(
-    volatile RadioState_t *radioState,
     RxDeviceState_t *rxDeviceState,
     TxDeviceState_t *txDeviceState
 ) {
@@ -183,22 +175,22 @@ void TxOled::renderPageInit(
 
     _display.setCursor(0, 0);
     _display.print("Rdy ");
-    _display.print(radioState->loraTxPower);
+    _display.print(radioNode.loraTxPower);
     _display.print("dBm");
 
     _display.setTextSize(1);
     _display.setCursor(0, 32);
     _display.print("Bandwitdh: ");
-    _display.print(radioState->loraBandwidth / 1000);
+    _display.print(radioNode.loraBandwidth / 1000);
     _display.print("kHz");
 
     _display.setCursor(0, 42);
     _display.print("SF: ");
-    _display.print(radioState->loraSpreadingFactor);
+    _display.print(radioNode.loraSpreadingFactor);
 
     _display.setCursor(64, 42);
     _display.print("CR: ");
-    _display.print(radioState->loraCodingRate);
+    _display.print(radioNode.loraCodingRate);
 
     _display.setCursor(0, 52);
     _display.print("Rate: ");
diff --git a/crossbow/tx_oled.h b/crossbow/tx_oled.h
index cd43e13..5008b96 100644
--- a/crossbow/tx_oled.h
+++ b/crossbow/tx_oled.h
@@ -7,6 +7,9 @@
 #include "Wire.h"
 #include "variables.h"
 #include "tactile.h"
+#include "radio_node.h"
+
+extern RadioNode radioNode;
 
 enum txOledPages {
     TX_PAGE_NONE,
@@ -32,14 +35,12 @@ class TxOled {
         TxOled(void);
         void init();
         void loop(
-            volatile RadioState_t *radioState,
             RxDeviceState_t *rxDeviceState,
             TxDeviceState_t *txDeviceState,
             Tactile *button0,
             Tactile *button1
         );
         void page(
-            volatile RadioState_t *radioState,
             RxDeviceState_t *rxDeviceState,
             TxDeviceState_t *txDeviceState,
             int page
@@ -47,27 +48,22 @@ class TxOled {
     private:
         Adafruit_SSD1306 _display;
         void renderPageInit(
-            volatile RadioState_t *radioState,
             RxDeviceState_t *rxDeviceState,
             TxDeviceState_t *txDeviceState
         );
         void renderPageStats(
-            volatile RadioState_t *radioState,
             RxDeviceState_t *rxDeviceState,
             TxDeviceState_t *txDeviceState
         );
         void renderPagePwr(
-            volatile RadioState_t *radioState,
             RxDeviceState_t *rxDeviceState,
             TxDeviceState_t *txDeviceState
         );
         void renderPageBind(
-            volatile RadioState_t *radioState,
             RxDeviceState_t *rxDeviceState,
             TxDeviceState_t *txDeviceState
         );
         void renderPageMode(
-            volatile RadioState_t *radioState,
             RxDeviceState_t *rxDeviceState,
             TxDeviceState_t *txDeviceState
         );
diff --git a/crossbow/variables.h b/crossbow/variables.h
index fd75559..38570d5 100644
--- a/crossbow/variables.h
+++ b/crossbow/variables.h
@@ -1,7 +1,19 @@
 #include "Arduino.h"
+#include "radio_node.h"
 
 #pragma once
 
+#ifndef VARIABLES_H
+#define VARIABLES_H
+
+#define RADIO_STATE_TX 1
+#define RADIO_STATE_RX 2
+
+#define TX_TRANSMIT_SLOT_RATE 67 //ms
+#define RX_CHANNEL_DWELL_TIME (TX_TRANSMIT_SLOT_RATE + 10) //Dwell on a channel slightly longer 
+#define RX_FAILSAFE_DELAY (TX_TRANSMIT_SLOT_RATE * 8)
+#define TX_FAILSAFE_DELAY (RX_FAILSAFE_DELAY * 4)
+
 #define OLED_UPDATE_RATE 750
 
 #define SBUS_UPDATE_RATE 15 //ms
@@ -13,11 +25,6 @@
 #define RX_TASK_HEALTH 200 //5Hz should be enough
 #define RSSI_CHANNEL 11
 
-#define TX_TRANSMIT_SLOT_RATE 67 //ms
-#define RX_CHANNEL_DWELL_TIME (TX_TRANSMIT_SLOT_RATE + 10) //Dwell on a channel slightly longer 
-#define RX_FAILSAFE_DELAY (TX_TRANSMIT_SLOT_RATE * 8)
-#define TX_FAILSAFE_DELAY (RX_FAILSAFE_DELAY * 4)
-
 #define CHANNEL_ID 0x01
 #define QSP_PAYLOAD_LENGTH 32
 
@@ -83,34 +90,6 @@ enum debugConfigFlags {
 
 #define NO_DATA_TO_READ -1
 
-#define RADIO_STATE_TX 1
-#define RADIO_STATE_RX 2
-
-#define RADIO_FREQUENCY_MIN 868000000
-#define RADIO_FREQUENCY_MAX 870000000
-#define RADIO_FREQUENCY_RANGE (RADIO_FREQUENCY_MAX-RADIO_FREQUENCY_MIN)
-#define RADIO_CHANNEL_WIDTH 250000
-#define RADIO_CHANNEL_COUNT 9 // 9 channels in 2MHz range (RADIO_FREQUENCY_RANGE/RADIO_CHANNEL_WIDTH) + 1
-#define RADIO_HOP_OFFSET 5
-
-struct RadioState_t {
-    uint32_t loraBandwidth = 250000;
-    uint8_t loraSpreadingFactor = 7;
-    uint8_t loraCodingRate = 6;
-    uint8_t loraTxPower = 17; // Defines output power of TX, defined in dBm range from 2-17
-    int8_t bytesToRead = -1;
-    uint8_t rssi = 0;
-    uint8_t snr = 0;
-    uint8_t deviceState = RADIO_STATE_RX;
-    uint32_t nextTxCheckMillis = 0;
-
-    const uint32_t dwellTime = TX_TRANSMIT_SLOT_RATE * 2; 
-    uint8_t channel = 0;
-    uint8_t lastReceivedChannel = 0;
-    uint32_t channelEntryMillis = 0;
-    uint8_t failedDwellsCount = 0;
-};
-
 struct TxDeviceState_t {
     uint8_t flags = 0;
     uint32_t roundtrip = 0;
@@ -140,8 +119,8 @@ struct QspConfiguration_t {
     uint32_t lastFrameReceivedAt[QSP_FRAME_COUNT] = {0};
     uint32_t anyFrameRecivedAt = 0;
     uint8_t deviceState = DEVICE_STATE_UNDETERMINED;
-    void (* onSuccessCallback)(QspConfiguration_t*, TxDeviceState_t*, RxDeviceState_t*, volatile RadioState_t*);
-    void (* onFailureCallback)(QspConfiguration_t*, TxDeviceState_t*, RxDeviceState_t*, volatile RadioState_t*);    
+    void (* onSuccessCallback)(QspConfiguration_t*, TxDeviceState_t*, RxDeviceState_t*, uint8_t receivedChannel);
+    void (* onFailureCallback)(QspConfiguration_t*, TxDeviceState_t*, RxDeviceState_t*);    
     bool canTransmit = false;
     bool forcePongFrame = false;
     uint8_t debugConfig = 0;
@@ -149,3 +128,5 @@ struct QspConfiguration_t {
     uint32_t lastTxSlotTimestamp = 0;
     bool transmitWindowOpen = false;
 };
+
+#endif
\ No newline at end of file