Merge pull request #140 from Genaker/rtl-sdr

Rtl sdr
2026-03-28 17:42:59 +01:00 · 2025-03-02 22:39:43 -08:00
parent 1e958da06d fa9e474741
commit adc751abf0
23 changed files with 2797 additions and 103 deletions
--- a/lib/loraboards/LoRaBoards.cpp
+++ b/lib/loraboards/LoRaBoards.cpp
@@ -111,7 +111,7 @@ HardwareSerial SerialGPS(GPS_RX_PIN, GPS_TX_PIN);
 #include "driver/gpio.h"
 #endif // ARDUINO_ARCH_ESP32

-DISPLAY_MODEL *u8g2 = NULL;
+// DISPLAY_MODEL *u8g2 = NULL;
 static DevInfo_t devInfo;

 #ifdef HAS_GPS
@@ -530,6 +530,7 @@ void loopPMU()

 bool beginDisplay()
 {
+    /**
    Wire.beginTransmission(DISPLAY_ADDR);
    if (Wire.endTransmission() == 0)
    {
@@ -552,7 +553,7 @@ bool beginDisplay()
    }

    Serial.printf("Warning: Failed to find Display at 0x%0X address\n", DISPLAY_ADDR);
-    return false;
+    return false;*/
 }

 bool beginSDCard()
@@ -847,7 +848,7 @@ void printResult(bool radio_online)
    Serial.println((psramFound()) ? "+" : "-");

    Serial.print("Display      : ");
-    Serial.println((u8g2) ? "+" : "-");
+    // Serial.println((u8g2) ? "+" : "-");

 #ifdef HAS_SDCARD
    Serial.print("Sd Card      : ");
@@ -866,7 +867,7 @@ void printResult(bool radio_online)
 #endif
 #endif

-    if (u8g2)
+    /*if (u8g2)
    {

        u8g2->clearBuffer();
@@ -895,6 +896,7 @@ void printResult(bool radio_online)

        delay(2000);
    }
+    */
 #endif
 }

--- a/lib/loraboards/LoRaBoards.h
+++ b/lib/loraboards/LoRaBoards.h
@@ -23,12 +23,12 @@

 #include <Arduino.h>
 #include <SPI.h>
-#include <U8g2lib.h>
+// #include <U8g2lib.h>
 #include <Wire.h>
 #include <XPowersLib.h>

 #ifndef DISPLAY_MODEL
-#define DISPLAY_MODEL U8G2_SSD1306_128X64_NONAME_F_HW_I2C
+// #define DISPLAY_MODEL U8G2_SSD1306_128X64_NONAME_F_HW_I2C
 #endif

 #ifndef OLED_WIRE_PORT
@@ -57,7 +57,7 @@ typedef struct
    uint8_t flashSpeed;
 } DevInfo_t;

-void setupBoards(bool disable_u8g2 = false);
+void setupBoards(bool disable_u8g2 = true);

 bool beginSDCard();

@@ -81,7 +81,7 @@ void loopPMU();
 extern XPowersLibInterface *PMU;
 extern bool pmuInterrupt;
 #endif
-extern DISPLAY_MODEL *u8g2;
+// extern DISPLAY_MODEL *u8g2;

 #define U8G2_HOR_ALIGN_CENTER(t) ((u8g2->getDisplayWidth() - (u8g2->getUTF8Width(t))) / 2)
 #define U8G2_HOR_ALIGN_RIGHT(t) (u8g2->getDisplayWidth() - u8g2->getUTF8Width(t))
--- a/lib/loraboards/utilities.h
+++ b/lib/loraboards/utilities.h
@@ -413,7 +413,7 @@
 #define __HAS_SENSOR__

 #define PMU_WIRE_PORT Wire1
-#define DISPLAY_MODEL U8G2_SH1106_128X64_NONAME_F_HW_I2C
+// #define DISPLAY_MODEL U8G2_SH1106_128X64_NONAME_F_HW_I2C
 #define BOARD_VARIANT_NAME "T-Beam S3"

 #elif defined(T_MOTION_S76G)
@@ -545,7 +545,7 @@
 #define __HAS_SENSOR__

 #define PMU_WIRE_PORT Wire
-#define DISPLAY_MODEL U8G2_SH1106_128X64_NONAME_F_HW_I2C
+// #define DISPLAY_MODEL U8G2_SH1106_128X64_NONAME_F_HW_I2C
 #define BOARD_VARIANT_NAME "T-Beam BPF"

 #else
--- a/lib/radio/radio.h
+++ b/lib/radio/radio.h
@@ -13,7 +13,7 @@ struct RadioModule
    virtual float getRSSI() = 0;
 };

-#ifdef USING_SX1262
+#ifndef USING_SX1262_no
 struct SX1262Module : RadioModule
 {
    SX1262 *_radio;
--- a/lib/scan/scan.cpp
+++ b/lib/scan/scan.cpp
@@ -25,7 +25,7 @@ uint16_t Scan::rssiMethod(float (*getRSSI)(void *), void *param, size_t samples,
            rssi = -65535;

        uint16_t abs_rssi = abs(rssi);
-        if (abs_rssi < max_signal)
+        if (abs_rssi < max_signal && max_signal != 0)
        {
            max_signal = abs_rssi;
        }
--- a/platformio.ini
+++ b/platformio.ini
@@ -20,6 +20,7 @@ default_envs = heltec_wifi_lora_32_V3
 ; src_dir = trans_src
 ; for ELRS-BOARDS
 ; src_dir = elrs-boards
+;src_dir = radio

 [env:heltec_wifi_lora_32_V3]
 platform = espressif32
@@ -250,7 +251,162 @@ build_flags =
 	-DARDUINO_LILYGO_T3_S3_V1_X
 	-DARDUINO_USB_MODE=1

-[env:lilygo-T3S3-v1-2-lr1121-900-compass]
+[env:lilygo-T3S3-v1-2-lr1121-900-radio-tx]
+platform = espressif32
+board = t3_s3_v1_x
+framework = arduino
+upload_speed = 921600
+monitor_speed = 115200
+board_build.f_cpu = 240000000
+board_build.filesystem = littlefs
+src_dir = radio
+lib_deps = 
+	ropg/Heltec_ESP32_LoRa_v3@^0.9.1
+	RadioLib
+	XPowersLib
+	SPI
+	https://github.com/bolderflight/sbus
+	gitlab-airbornemint/Protothreads
+build_flags = 
+ 	-DMAIN_FILE="Tx.cpp"
+	-DLILYGO
+	-DT3_S3_V1_2_LR1121
+	-DARDUINO_LILYGO_T3S3_LR1121
+	-DESP32
+    -DSAMPLES_RSSI=5
+	-DUSING_LR1121
+	-DINIT_FREQ=900
+    -DFREQ_BEGIN=830
+	-DFREQ_END=945
+	-DFREQ_RX=2440
+	-DARDUINO_ARCH_ESP32
+	-DARDUINO_USB_CDC_ON_BOOT=1
+	-DARDUINO_LILYGO_T3_S3_V1_X
+	-DARDUINO_USB_MODE=1
+	-DLORA_RX=0
+	-DLORA_TX=1
+
+[env:lilygo-T3S3-v1-2-lr1121-900-radio-rx]
+platform = espressif32
+board = t3_s3_v1_x
+framework = arduino
+upload_speed = 921600
+monitor_speed = 115200
+board_build.f_cpu = 240000000
+board_build.filesystem = littlefs
+src_dir = radio
+lib_deps = 
+	ropg/Heltec_ESP32_LoRa_v3@^0.9.1
+	RadioLib
+	XPowersLib
+	https://github.com/bolderflight/sbus
+	gitlab-airbornemint/Protothreads
+build_flags = 
+ 	-DMAIN_FILE="Tx.cpp"
+	-DLILYGO
+	-DT3_S3_V1_2_LR1121
+	-DARDUINO_LILYGO_T3S3_LR1121
+	-DESP32
+    -DSAMPLES_RSSI=5
+	-DUSING_LR1121
+	-DINIT_FREQ=900
+    -DFREQ_BEGIN=830
+	-DFREQ_END=945
+	-DFREQ_RX=2440
+	-DARDUINO_ARCH_ESP32
+	-DARDUINO_USB_CDC_ON_BOOT=1
+	-DARDUINO_LILYGO_T3_S3_V1_X
+	-DARDUINO_USB_MODE=1
+	-DLORA_RX=1
+	-DLORA_TX=0
+
+[env:heltec-v3-900-radio-rx]
+platform = espressif32
+board = heltec_wifi_lora_32_V3
+framework = arduino
+upload_speed = 921600
+monitor_speed = 115200
+board_build.f_cpu = 240000000
+src_dir = radio
+lib_deps = 
+	ropg/Heltec_ESP32_LoRa_v3@^0.9.1
+	RadioLib
+	XPowersLib
+	https://github.com/bolderflight/sbus
+	mbed-takuhachisu/CRC8
+build_flags = 
+ 	-DMAIN_FILE="Tx.cpp"
+	-DHELTEC_POWER_BUTTON
+	-DHELTEC
+	-DRADIOLIB_CHECK_PARAMS=0
+	-DESP32
+	-DARDUINO_ARCH_ESP32
+	-DARDUINO_USB_CDC_ON_BOOT=0
+	-DARDUINO_USB_MODE=1
+	-DLORA_RX=1
+	-DLORA_TX=0
+
+[env:heltec-v3-900-radio-tx]
+platform = espressif32
+board = heltec_wifi_lora_32_V3
+framework = arduino
+upload_speed = 921600
+monitor_speed = 115200
+board_build.f_cpu = 240000000
+src_dir = radio
+lib_deps = 
+	ropg/Heltec_ESP32_LoRa_v3@^0.9.1
+	RadioLib
+	XPowersLib
+	https://github.com/bolderflight/sbus
+	mbed-takuhachisu/CRC8
+build_flags = 
+ 	-DMAIN_FILE="Tx.cpp"
+	-DHELTEC_POWER_BUTTON
+	-DHELTEC
+	-DRADIOLIB_CHECK_PARAMS=0
+	-DESP32
+	-DARDUINO_ARCH_ESP32
+	-DARDUINO_USB_CDC_ON_BOOT=0
+	-DARDUINO_USB_MODE=1
+	-DLORA_RX=0
+	-DLORA_TX=1
+	-DSERIAL_INPUT=1
+
+[env:lilygo-T3S3-v1-2-sx1262-900-radio-rx]
+platform = espressif32
+board = t3_s3_v1_x
+framework = arduino
+upload_speed = 921600
+monitor_speed = 115200
+board_build.f_cpu = 240000000
+board_build.filesystem = littlefs
+src_dir = radio
+lib_deps = 
+	ropg/Heltec_ESP32_LoRa_v3@^0.9.1
+	RadioLib
+	XPowersLib
+	duracopter/MAVLink v2 C library@^2.0
+build_flags = 
+ 	-DMAIN_FILE="Tx.cpp"
+	-DLILYGO
+	-DT3_S3_V1_2_SX1262
+	-DARDUINO_LILYGO_T3S3_SX1262
+	-DESP32
+    -DSAMPLES_RSSI=5
+	-DUSING_SX1262
+	-DINIT_FREQ=900
+    -DFREQ_BEGIN=830
+	-DFREQ_END=945
+	-DFREQ_RX=2440
+	-DARDUINO_ARCH_ESP32
+	-DARDUINO_USB_CDC_ON_BOOT=1
+	-DARDUINO_LILYGO_T3_S3_V1_X
+	-DARDUINO_USB_MODE=1
+	-DLORA_RX=1
+	-DLORA_TX=0
+
+[env:lilygo-T3S3-v1-2-lr1121-900-BT]
 platform = espressif32
 board = t3_s3_v1_x
 framework = arduino
@@ -264,16 +420,48 @@ lib_deps =
 	U8g2
 	XPowersLib
 	h2zero/NimBLE-Arduino
-	https://github.com/Genaker/Adafruit_HMC5883_Unified
-build_flags =
-	-lbt
+build_flags = 
 	-DLILYGO
 	-DT3_S3_V1_2_LR1121
 	-DARDUINO_LILYGO_T3S3_LR1121
 	-DESP32
    -DSAMPLES_RSSI=5
 	-DUSING_LR1121
-	-DSCAN_RBW_FACTOR=2
+	-DINIT_FREQ=900
+    -DFREQ_BEGIN=830
+	-DFREQ_END=945
+	-DFREQ_RX=2440
+	-DARDUINO_ARCH_ESP32
+	-DARDUINO_USB_CDC_ON_BOOT=1
+	-DARDUINO_LILYGO_T3_S3_V1_X
+	-DARDUINO_USB_MODE=1
+	-DBT_MOBILE
+
+[env:lilygo-T3S3-v1-2-lr1121-900-compass]
+platform = espressif32
+board = t3_s3_v1_x
+framework = arduino
+upload_speed = 1500000
+monitor_speed = 115200
+board_build.f_cpu = 240000000
+board_build.partitions = min_spiffs.csv
+board_build.filesystem = littlefs
+lib_deps = 
+	ropg/Heltec_ESP32_LoRa_v3@^0.9.1
+	RadioLib
+	XPowersLib
+	h2zero/NimBLE-Arduino
+	https://github.com/Genaker/Adafruit_HMC5883_Unified
+build_flags =
+	-lbt
+	-Og -ggdb3
+	-DLILYGO
+	-DT3_S3_V1_2_LR1121
+	-DARDUINO_LILYGO_T3S3_LR1121
+	-DESP32
+    -DSAMPLES_RSSI=2
+	-DUSING_LR1121
+	-DSCAN_RBW_FACTOR=1
 	-DINIT_FREQ=900
    -DFREQ_BEGIN=800
 	-DFREQ_END=950
@@ -284,9 +472,11 @@ build_flags =
 	-DARDUINO_USB_MODE=1
 	-DCOMPASS_ENABLED
 	-DCOMPASS_FREQ=915
-	-DCOMPASS_DEBUG
+	-DCOMPASS_DEBUG2
 	-DCOMPASS_RSSI
 	-DBT_MOBILE
+	-DBT_RSSI
+	-DBT_NM

 [env:lilygo-T3S3-v1-2-sx1280]
 platform = espressif32
--- a/radio/CRSF.h
+++ b/radio/CRSF.h
@@ -0,0 +1,297 @@
+#pragma once
+
+#include <Arduino.h>
+
+//
+// Standard Crossfire definitions:
+//
+#define CRSF_SYNC_BYTE 0xC8
+#define CRSF_FRAME_TYPE_CHANNELS 0x16
+#define CRSF_MAX_CHANNELS 16
+#define CRSF_BAUDRATE 420000 // standard CRSF baud rate
+
+// Betaflight CRSF docs:
+// https://github.com/betaflight/betaflight/blob/4.5-maintenance/src/main/rx/crsf_protocol.h
+// Crosfire Protocol specs:
+// https://github.com/tbs-fpv/tbs-crsf-spec/blob/main/crsf.md#broadcast-frame
+
+// The CRSF "RC Channels" frame for 16 channels is always 24 bytes total:
+//   Byte[0]   = 0xC8 (address)
+//   Byte[1]   = 22   (length from byte[2] to the last CRC byte)
+//   Byte[2]   = 0x16 (frame type for channels)
+//   Byte[3..22] = 20 bytes of channel data (10 bits × 16 = 160 bits)
+//   Byte[23]  = CRC
+//
+// If type=0x16 and length=22, we interpret it as channel data (16ch, 10 bits each).
+
+class CRSF2
+{
+  public:
+    // Pass in the HardwareSerial, plus the pins to use on ESP32
+    // (rxPin and txPin). If you only do TX, you can set rxPin=-1 or vice versa.
+    CRSF2(HardwareSerial &serialPort, int rxPin, int txPin, long baudRate = CRSF_BAUDRATE)
+        : serialPort(serialPort), rxPin(rxPin), txPin(txPin), baudRate(baudRate)
+    {
+    }
+
+    void begin()
+    {
+        // Initialize the UART. If you want two-way, specify both rxPin, txPin.
+        // Example: 8N1, no inversion, 20s timeout
+        serialPort.begin(baudRate, SERIAL_8N1, rxPin, txPin, false, 20000UL);
+    }
+
+    // -------------------------------
+    // Send a 16-channel RC frame (10 bits per channel).
+    // channelData[i] should be in the range [0..1023].
+    // For typical servo-like values (1000..2000), you must scale them down if needed.
+    // -------------------------------
+    void sendRCFrame(const uint16_t *channelData)
+    {
+        constexpr size_t CHANNEL_BITS = 10;
+        constexpr size_t CHANNEL_COUNT = CRSF_MAX_CHANNELS;
+        constexpr size_t CHANNEL_BYTES =
+            ((CHANNEL_COUNT * CHANNEL_BITS) + 7) / 8; // => 20
+        constexpr size_t FRAME_TYPE_SIZE = 1;         // 1 byte for frame type (0x16)
+        constexpr size_t CRC_SIZE = 1;                // 1 byte for CRC
+        constexpr size_t CRSF_PAYLOAD_LEN =
+            FRAME_TYPE_SIZE + CHANNEL_BYTES + CRC_SIZE;      // => 1 + 20 + 1 = 22
+        constexpr size_t PACKET_SIZE = 2 + CRSF_PAYLOAD_LEN; // => 24 total
+
+        uint8_t packet[PACKET_SIZE];
+
+        // Address (sync) byte
+        packet[0] = CRSF_SYNC_BYTE;
+        // Length (22)
+        packet[1] = CRSF_PAYLOAD_LEN;
+        // Frame Type
+        packet[2] = CRSF_FRAME_TYPE_CHANNELS;
+
+        // Pack channel data (16 × 10 bits = 160 bits => 20 bytes)
+        uint32_t bitBuffer = 0;
+        uint8_t bitCount = 0;
+        size_t byteIndex = 3; // start filling after [0..2]
+
+        for (size_t i = 0; i < CHANNEL_COUNT; i++)
+        {
+            uint16_t val = channelData[i] & 0x03FF; // 10-bit mask
+            bitBuffer |= (static_cast<uint32_t>(val) << bitCount);
+            bitCount += CHANNEL_BITS; // +10
+
+            while (bitCount >= 8)
+            {
+                packet[byteIndex++] = static_cast<uint8_t>(bitBuffer & 0xFF);
+                bitBuffer >>= 8;
+                bitCount -= 8;
+            }
+        }
+
+        // If leftover bits remain, put them into the last byte
+        if (bitCount > 0)
+        {
+            packet[byteIndex++] = static_cast<uint8_t>(bitBuffer & 0xFF);
+        }
+
+        // Compute CRC over [Type + channel bytes] => 21 bytes
+        const size_t CRC_LENGTH = CRSF_PAYLOAD_LEN - 1; // (22 - 1) = 21
+        uint8_t crc = calculateCRC(&packet[2], CRC_LENGTH);
+        // Place CRC at last byte (index 23)
+        packet[PACKET_SIZE - 1] = crc;
+
+        // Send out the 24-byte CRSF RC frame
+        serialPort.write(packet, PACKET_SIZE);
+    }
+
+    // -------------------------------
+    // Poll the serial port for incoming CRSF frames.
+    // If we receive a valid 16-ch "Channel Data" frame (type=0x16),
+    // decode the channels into the provided channelData[] array.
+    // Returns 'true' if a new channel frame was successfully parsed.
+    //
+    // Call this in your loop() or in a periodic task.
+    // -------------------------------
+    bool receiveRCFrame(uint16_t *channelData)
+    {
+        // Run our state machine as long as there's data available
+        while (serialPort.available() > 0)
+        {
+            uint8_t c = static_cast<uint8_t>(serialPort.read());
+
+            switch (rxState)
+            {
+            case WAIT_SYNC:
+                if (c == CRSF_SYNC_BYTE)
+                {
+                    rxPacket[0] = c;
+                    rxIndex = 1;
+                    rxState = WAIT_LEN;
+                }
+                break;
+
+            case WAIT_LEN:
+                // The next byte after sync is "length"
+                rxPacket[rxIndex++] = c;
+                rxLength = c; // length should be the payload from [type..CRC]
+                // Basic sanity check: max 64 or so. CRSF frames can vary, but let's be
+                // safe.
+                if (rxLength < 2 || rxLength > 64)
+                {
+                    // Invalid length, reset parser
+                    rxState = WAIT_SYNC;
+                    rxIndex = 0;
+                }
+                else
+                {
+                    // Next step: read length more bytes
+                    rxState = WAIT_PAYLOAD;
+                }
+                break;
+
+            case WAIT_PAYLOAD:
+                // Store the byte
+                rxPacket[rxIndex++] = c;
+
+                // If we've reached the entire frame, parse
+                if (rxIndex >= (2 + rxLength))
+                {
+                    // We have [0]=0xC8, [1]=length, plus 'length' bytes
+                    // Check CRC, parse if valid
+                    bool ok = parseFrame(channelData);
+                    // Reset to look for the next frame
+                    rxState = WAIT_SYNC;
+                    rxIndex = 0;
+
+                    if (ok)
+                    {
+                        // We got a valid channel frame
+                        return true;
+                    }
+                }
+                break;
+            }
+        }
+
+        return false; // no new channel frame parsed
+    }
+
+  private:
+    HardwareSerial &serialPort;
+    int rxPin;
+    int txPin;
+    long baudRate;
+
+    // ---- Receive State Machine Fields ----
+    enum RxState
+    {
+        WAIT_SYNC,
+        WAIT_LEN,
+        WAIT_PAYLOAD
+    } rxState = WAIT_SYNC;
+
+    uint8_t rxPacket[64]; // buffer for incoming frame
+    size_t rxIndex = 0;   // how many bytes we've stored
+    uint8_t rxLength = 0; // length byte from the packet
+
+    // 8-bit CRC with polynomial 0xD5
+    uint8_t calculateCRC(const uint8_t *data, size_t length)
+    {
+        uint8_t crc = 0;
+        for (size_t i = 0; i < length; i++)
+        {
+            crc ^= data[i];
+            for (uint8_t bit = 0; bit < 8; bit++)
+            {
+                if (crc & 0x80)
+                {
+                    crc <<= 1;
+                    crc ^= 0xD5;
+                }
+                else
+                {
+                    crc <<= 1;
+                }
+            }
+        }
+        return crc;
+    }
+
+    // Attempt to parse the frame in rxPacket[].
+    // If it's a valid 16-ch RC frame (type=0x16) and CRC checks out,
+    // decode into channelData[].
+    // Returns true on success, false otherwise.
+    bool parseFrame(uint16_t *channelData)
+    {
+        // Basic layout:
+        // rxPacket[0] = 0xC8
+        // rxPacket[1] = length (n)
+        // Then we have n bytes: [2..(1+n)]
+        // The last of those n bytes is CRC.
+
+        uint8_t lengthField = rxPacket[1];
+        // The "payload" = lengthField bytes from rxPacket[2]..rxPacket[1 + lengthField]
+        // So the CRC is at rxPacket[1 + lengthField].
+        uint8_t crc = rxPacket[1 + lengthField];
+
+        // Check CRC over the [Frame Type..(payload)] excluding the CRC byte
+        // i.e. from rxPacket[2]..rxPacket[1 + lengthField - 1]
+        size_t dataLen = lengthField - 1; // e.g. if length=22, dataLen=21
+        uint8_t calcCRC = calculateCRC(&rxPacket[2], dataLen);
+
+        if (calcCRC != crc)
+        {
+            // CRC mismatch
+            return false;
+        }
+
+        // Check frame type
+        uint8_t frameType = rxPacket[2];
+        if (frameType != CRSF_FRAME_TYPE_CHANNELS)
+        {
+            // Not a channel frame
+            return false;
+        }
+
+        // For a 16-ch frame, lengthField should be 22
+        // but let's check if we want to be sure
+        if (lengthField != 22)
+        {
+            // not a 16-ch RC frame
+            return false;
+        }
+
+        // If we reach here, we have a valid 16-ch frame
+        // Decode the 20 bytes of channel data
+        // They live at rxPacket[3..22], which is 20 bytes
+        decodeChannels(&rxPacket[3], channelData);
+        return true;
+    }
+
+    // Decode 16 channels of 10 bits from the 20-byte payload
+    // into channelData[16].
+    void decodeChannels(const uint8_t *payload, uint16_t *channelData)
+    {
+        constexpr size_t CHANNEL_COUNT = CRSF_MAX_CHANNELS; // 16
+        constexpr size_t CHANNEL_BITS = 10;
+
+        uint32_t bitBuffer = 0;
+        uint8_t bitCount = 0;
+        size_t bytePos = 0;
+
+        for (size_t i = 0; i < CHANNEL_COUNT; i++)
+        {
+            // accumulate bits until we have at least 10
+            while (bitCount < CHANNEL_BITS)
+            {
+                bitBuffer |= (static_cast<uint32_t>(payload[bytePos++]) << bitCount);
+                bitCount += 8;
+            }
+            // extract 10 bits
+            uint16_t val = static_cast<uint16_t>(bitBuffer & 0x3FF); // 10-bit mask
+            bitBuffer >>= CHANNEL_BITS;
+            bitCount -= CHANNEL_BITS;
+
+            // store
+            channelData[i] = val;
+        }
+    }
+};
--- a/radio/FHSS.cpp
+++ b/radio/FHSS.cpp
@@ -0,0 +1,94 @@
+#include "FHSS.h"
+
+float hopTable[MAX_HOP_CHANNELS];
+uint64_t packetNumber = 0;
+long int receivedPacketCounter = 0;
+
+uint32_t syncWord = 98754386857476; // Example sync word
+// uint32_t syncWord = 0x1A2B3C4D; // Example sync word (can be any 32-bit value)
+
+float maxWidthMHz = 5.0;                        // Max hopping width of 20 MHz
+float startFreq = LORA_BASE_FREQ - maxWidthMHz; // Start at 900 MHz
+float stepKHz = 10.0;                           // 10 kHz step size
+
+int numChannels = 0;
+
+// Get the next frequency from the hopping table
+int hopIndex = 0;
+unsigned long lastHopTime = 0;
+unsigned long dwellTime = 500; // 500ms dwell time
+float currentFreq = 999;
+
+// Function to generate a frequency hopping table, adapting if channels are fewer
+int generateFrequencies(uint32_t syncWord, float startFreq, float stepKHz,
+                        float maxWidthMHz)
+{
+    float stepMHz = stepKHz / 1000.0; // Convert kHz to MHz
+    numChannels = int((maxWidthMHz * 1e3) /
+                      stepKHz); // Calculate number of channels within max width
+
+    // If fewer channels are available, adjust dynamically
+    if (numChannels < 10)
+    { // Less than 10 channels is not good for FHSS
+        Serial.println("Warning: Too few channels! FHSS may not work well.");
+        numChannels = 10; // Ensure a minimum of 10 channels
+    }
+
+    if (numChannels > MAX_HOP_CHANNELS)
+    {
+        Serial.println("Warning: Reducing channels to MAX_HOP_CHANNELS.");
+        numChannels = MAX_HOP_CHANNELS; // Prevent overflow
+    }
+
+    // Generate sequential frequencies within max width
+    for (int i = 0; i < numChannels; i++)
+    {
+        hopTable[i] = startFreq + (i * stepMHz);
+    }
+
+    // Shuffle using sync word (randomize the order)
+    for (int i = 0; i < numChannels; i++)
+    {
+        syncWord = (syncWord * 1103515245 + 12345) & 0x7FFFFFFF;
+        int swapIndex = syncWord % numChannels;
+
+        // Swap values
+        float temp = hopTable[i];
+        hopTable[i] = hopTable[swapIndex];
+        hopTable[swapIndex] = temp;
+    }
+
+    return numChannels; // Return actual number of channels generated
+}
+
+// Function to print the generated table (for debugging)
+void printHopTable(int numChannels)
+{
+    delay(100);
+    Serial.println("------");
+    Serial.println("Generated Frequency Hopping Table [" + String(numChannels) + "]:");
+    /*for (int i = 0; i < numChannels; i++)
+    {
+        Serial.println(String(i) + ": " + hopTable[i] + " MHz\n");
+    }*/
+    delay(1000);
+}
+
+void updateFrequency()
+{
+    unsigned long currentTime = millis();
+
+    if (currentTime - lastHopTime >= dwellTime)
+    {
+        if (hopIndex == numChannels)
+        {
+            hopIndex = 0;
+        }
+        hopIndex = hopIndex + 1;
+        // packetNumber = hopIndex;
+        currentFreq = hopTable[hopIndex];
+        radio.setFrequency(hopTable[hopIndex]);
+
+        lastHopTime = currentTime;
+    }
+}
--- a/radio/FHSS.h
+++ b/radio/FHSS.h
@@ -0,0 +1,36 @@
+#include "config.h"
+#include <Arduino.h>
+#include <LiLyGo.h>
+#include <LoRaBoards.h>
+
+#define SYNC_FREQUENCY 915.000
+
+#define MAX_HOP_CHANNELS 5000              // 20 MHz range with 10 kHz step
+#define PACKET_SEND_DURATION 1 * 60 * 1000 // 1 minutes in milliseconds
+
+extern float hopTable[MAX_HOP_CHANNELS];
+extern uint64_t packetNumber;
+extern long int receivedPacketCounter;
+
+extern uint32_t syncWord; // Example sync word (can be any 32-bit value)
+extern int numChannels;
+
+// Get the next frequency from the hopping table
+extern int hopIndex;
+extern unsigned long lastHopTime;
+extern unsigned long dwellTime; // 500ms dwell time
+extern float currentFreq;
+
+extern uint32_t syncWord; // Example sync word
+extern float maxWidthMHz; // Max hopping width of 20 MHz
+extern float startFreq;   // Start at 900 MHz
+extern float stepKHz;     // 10 kHz step size
+
+// Function to generate a frequency hopping table, adapting if channels are fewer
+int generateFrequencies(uint32_t syncWord, float startFreq, float stepKHz,
+                        float maxWidthMHz);
+
+// Function to print the generated table (for debugging)
+void printHopTable(int numChannels);
+
+void updateFrequency();
--- a/radio/MavLink.h
+++ b/radio/MavLink.h
@@ -0,0 +1,125 @@
+#include <Arduino.h>
+
+// Include MAVLink headers.
+// Adjust the include path to match your local mavlink library structure.
+#include "mavlink.h"
+
+// Which UART on ESP32? For example, Serial2 can be mapped to pins:
+constexpr int RX_PIN = 16;           // Flight Controller TX -> ESP32 RX
+constexpr int TX_PIN = 17;           // Flight Controller RX <- ESP32 TX
+constexpr long MAVLINK_BAUD = 57600; // Common default for MAVLink
+
+// Identify our local system/component, and the target FC system
+// (ArduPilot often uses sysid=1, compid=1, but it can vary).
+constexpr uint8_t MAV_COMP_ID_ONBOARD = 1;  // or 190 for companion computer
+constexpr uint8_t MAV_SYS_ID_ONBOARD = 255; // e.g. 255 for companion
+constexpr uint8_t TARGET_SYS_ID = 1;        // autopilot system ID
+constexpr uint8_t TARGET_COMP_ID = 1;       // autopilot component ID
+
+// We'll send 8 channels. MAVLink 2 can handle up to 18 channels in
+// set_rc_channels_override, but typical autopilots read at least 8 or 14 channels.
+static uint16_t rcChannels[8] = {
+    1500, // Channel 1
+    1500, // Channel 2
+    1500, // Channel 3
+    1500, // Channel 4
+    1000, // Channel 5
+    1000, // Channel 6
+    1000, // Channel 7
+    1000  // Channel 8
+};
+
+void setup()
+{
+    Serial.begin(115200);
+    delay(2000);
+    Serial.println("MAVLink RC Override Example - ESP32");
+
+    // Initialize Serial2 on given pins
+    Serial2.begin(MAVLINK_BAUD, SERIAL_8N1, RX_PIN, TX_PIN);
+
+    // (Optional) Wait a bit for the autopilot to boot, if needed
+    delay(2000);
+}
+
+void loop()
+{
+    // For demonstration, let's do a small "animation" on channel 1
+    // to show that the flight controller is indeed receiving changes.
+    // We'll move channel 1 from 1000 to 2000 in steps.
+    static int ch1Value = 1000;
+    static int increment = 10;
+
+    ch1Value += increment;
+    if (ch1Value > 2000)
+    {
+        ch1Value = 2000;
+        increment = -10;
+    }
+    else if (ch1Value < 1000)
+    {
+        ch1Value = 1000;
+        increment = 10;
+    }
+
+    // Update channel 1 in our array
+    rcChannels[0] = ch1Value;
+
+    // Send the MAVLink Set RC Channels Override message
+    sendRCChannelsOverride();
+
+    // Send ~20 times per second
+    delay(50);
+}
+
+void sendRCChannelsOverride()
+{
+    mavlink_message_t msg;
+    uint8_t buf[MAVLINK_MAX_PACKET_LEN];
+
+    // Fill out the message:
+    // mavlink_msg_set_rc_channels_override_pack(
+    //   uint8_t system_id,
+    //   uint8_t component_id,
+    //   mavlink_message_t* msg,
+    //   uint8_t target_system,
+    //   uint8_t target_component,
+    //   uint16_t chan1_raw,
+    //   uint16_t chan2_raw,
+    //   ...
+    //   uint16_t chan8_raw,
+    //   uint16_t chan9_raw,
+    //   ...
+    //   up to chan18_raw
+    // )
+    // For 8 channels, pass 0 for unused channels > 8.
+
+    mavlink_msg_set_rc_channels_override_pack(
+        MAV_SYS_ID_ONBOARD, MAV_COMP_ID_ONBOARD, &msg, TARGET_SYS_ID, TARGET_COMP_ID,
+        rcChannels[0],         // chan1
+        rcChannels[1],         // chan2
+        rcChannels[2],         // chan3
+        rcChannels[3],         // chan4
+        rcChannels[4],         // chan5
+        rcChannels[5],         // chan6
+        rcChannels[6],         // chan7
+        rcChannels[7],         // chan8
+        0, 0, 0, 0, 0, 0, 0, 0 // for chan9..chan16 or up to chan18
+    );
+
+    // Encode the message into the send buffer
+    uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
+
+    // Write it out the serial port to the FC
+    Serial2.write(buf, len);
+
+    // For debugging
+    Serial.print("Sent RC override: [");
+    for (int i = 0; i < 8; i++)
+    {
+        Serial.print(rcChannels[i]);
+        if (i < 7)
+            Serial.print(", ");
+    }
+    Serial.println("]");
+}
--- a/radio/RadioCommands.h
+++ b/radio/RadioCommands.h
@@ -0,0 +1,67 @@
+
+#pragma once
+
+#include <Arduino.h>
+#include <map>
+#include <unordered_map>
+#include <vector>
+
+uint16_t testChannels[16] = {1500, 2000, 1350, 1400, 1505, 1506, 1507, 1508,
+                             1509, 1510, 1511, 1512, 1513, 1514, 1515, 1516};
+
+/*
+commands      sending message              comments
+-----------------------------------------------------
+roll	      rc 1 <value>	        // 	move left or right
+pitch         rc 2 <value>          // move forward or backwards
+yaw	          rc 4 <value>	        // turn left or right
+throttle      rc 3 <value>  	    // move up or down
+*/
+enum Command
+{
+    HEART_BEAT = 0, // Corresponds to rc 0
+    ROLL = 1,       // Corresponds to rc 1
+    PITCH = 2,      // Corresponds to rc 2
+    THROTTLE = 3,   // Corresponds to rc 3
+    YAW = 4,        // Corresponds to rc 4
+    ////  ----- Not Assigned Yet -----
+    AUX1 = 5, // Corresponds to rc 5
+    AUX2 = 6, // Corresponds to rc 6
+    AUX3 = 7, // Corresponds to rc 7
+    AUX4 = 8, // Corresponds to rc 8
+    AUX5 = 9, // Corresponds to rc 9
+    AUX6 = 10 // Corresponds to rc 10
+};
+
+// Create a map from Command to string
+std::unordered_map<Command, String> commandToStringMap = {{HEART_BEAT, "HEART_BEAT"},
+                                                          {ROLL, "ROLL"},
+                                                          {PITCH, "PITCH"},
+                                                          {YAW, "YAW"},
+                                                          {THROTTLE, "THROTTLE"}};
+
+// Define the mapping table
+std::vector<std::pair<uint8_t, uint16_t>> channelValueMappingTable = {
+    {0, 1300},  {1, 1325},  {2, 1350},  {3, 1375}, {4, 1400},  {5, 1425},
+    {6, 1450},  {7, 1475},  {8, 1500},  {9, 1525}, {10, 1550}, {11, 1575},
+    {12, 1600}, {13, 1625}, {14, 1650}, {15, 1675}};
+// 0 - 1300 0
+// 1300 - 1325 1
+// 1325 - 1350 2
+// 1350 - 1375 3
+// 1375 - 1400 4
+// 1400 - 1425 5
+// 1425 - 1450 6
+// 1450 - 1475 7
+// 1475 - 1500 8
+// 1500 - 1525 9
+// 1525 - 1550 10
+// 1550 - 1575 11
+// 1575 - 1600 12
+// 1600 - 1625 13
+// 1625 - 1650 14
+// 1650 - 1675 15
+
+#define INIT_SBUS_ARRAY                                                                  \
+    {1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500,                                     \
+     1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500}
--- a/radio/Readme.txt
+++ b/radio/Readme.txt
@@ -0,0 +1,28 @@
+1. Wiring:
+Connect the SBUS output from your receiver to the appropriate UART (Universal Asynchronous Receiver-Transmitter) port on your flight controller. This is usually labeled as RX or SBUS on the flight controller.
+2. Betaflight Configuration:
+Connect to Betaflight Configurator: Use the Betaflight Configurator software to connect to your flight controller via USB.
+Ports Tab: In the Configurator, navigate to the "Ports" tab. Enable the UART port where your SBUS receiver is connected. Set the port to "Serial RX".
+Configuration Tab: Go to the "Configuration" tab. Under "Receiver", select "Serial-based receiver" and then choose "SBUS" from the protocol dropdown menu.
+Save and Reboot: After making these changes, save the configuration and reboot the flight controller.
+3. Testing:
+After configuration, test the setup by moving the sticks on your transmitter and observing the response in the Betaflight Configurator's "Receiver" tab. The channels should move according to your stick inputs.
+
+
+Steps to Connect to Betaflight via Browser
+1. Download Betaflight Configurator:
+Visit the Betaflight Configurator releases page on GitHub. 
+https://github.com/betaflight/betaflight-configurator/releases
+Download the appropriate version for your operating system (Windows, macOS, or Linux).
+
+go to Port -> select port -> check Serial RX
+
+Go to 
+
+# SBUS tested
+
+ESP32 39 -> SBUS(R2) Any of them
+GND -> G
+Article explains : https://speedybee.zendesk.com/hc/en-us/articles/19968381088795-How-to-set-up-your-SBUS-receiver-in-Betaflight-configurator-on-SpeedyBee-F405MINI-flight-controller
+
+
--- a/radio/SBUS.h
+++ b/radio/SBUS.h
@@ -0,0 +1,63 @@
+#include "config.h"
+#include <Arduino.h>
+
+#define SBUS_CHANNELS 16
+#define SBUS_PACKET_SIZE 25
+
+class SBUS2
+{
+  public:
+    SBUS2(HardwareSerial &serialPort) : sbusSerial(serialPort)
+    {
+        sbusSerial.begin(100000, SERIAL_8E2, -1, TXD1, true); // Inverted SBUS signal
+    }
+
+    void send(uint16_t channels[SBUS_CHANNELS])
+    {
+        uint8_t sbusPacket[SBUS_PACKET_SIZE] = {0};
+
+        sbusPacket[0] = 0x0F; // SBUS Start Byte
+
+        // Correctly pack 16 channels (11-bit each) into 22 bytes
+        uint16_t packedData[SBUS_CHANNELS] = {0};
+        for (int i = 0; i < SBUS_CHANNELS; i++)
+        {
+            packedData[i] = channels[i] & 0x07FF; // Ensure 11-bit limit
+        }
+
+        sbusPacket[1] = (packedData[0] & 0xFF);
+        sbusPacket[2] = ((packedData[0] >> 8) | (packedData[1] << 3)) & 0xFF;
+        sbusPacket[3] = ((packedData[1] >> 5) | (packedData[2] << 6)) & 0xFF;
+        sbusPacket[4] = (packedData[2] >> 2) & 0xFF;
+        sbusPacket[5] = ((packedData[2] >> 10) | (packedData[3] << 1)) & 0xFF;
+        sbusPacket[6] = ((packedData[3] >> 7) | (packedData[4] << 4)) & 0xFF;
+        sbusPacket[7] = ((packedData[4] >> 4) | (packedData[5] << 7)) & 0xFF;
+        sbusPacket[8] = (packedData[5] >> 1) & 0xFF;
+        sbusPacket[9] = ((packedData[5] >> 9) | (packedData[6] << 2)) & 0xFF;
+        sbusPacket[10] = ((packedData[6] >> 6) | (packedData[7] << 5)) & 0xFF;
+        sbusPacket[11] = (packedData[7] >> 3) & 0xFF;
+        sbusPacket[12] = (packedData[8] & 0xFF);
+        sbusPacket[13] = ((packedData[8] >> 8) | (packedData[9] << 3)) & 0xFF;
+        sbusPacket[14] = ((packedData[9] >> 5) | (packedData[10] << 6)) & 0xFF;
+        sbusPacket[15] = (packedData[10] >> 2) & 0xFF;
+        sbusPacket[16] = ((packedData[10] >> 10) | (packedData[11] << 1)) & 0xFF;
+        sbusPacket[17] = ((packedData[11] >> 7) | (packedData[12] << 4)) & 0xFF;
+        sbusPacket[18] = ((packedData[12] >> 4) | (packedData[13] << 7)) & 0xFF;
+        sbusPacket[19] = (packedData[13] >> 1) & 0xFF;
+        sbusPacket[20] = ((packedData[13] >> 9) | (packedData[14] << 2)) & 0xFF;
+        sbusPacket[21] = ((packedData[14] >> 6) | (packedData[15] << 5)) & 0xFF;
+        sbusPacket[22] = (packedData[15] >> 3) & 0xFF;
+
+        // Flags: Failsafe & Frame Lost
+        sbusPacket[23] = 0x00; // Set failsafe/lost frame bits here if needed
+
+        // End byte
+        sbusPacket[24] = 0x00; // Must be 0x00 or 0x04 if failsafe active
+
+        // Send SBUS packet
+        sbusSerial.write(sbusPacket, SBUS_PACKET_SIZE);
+    }
+
+  private:
+    HardwareSerial &sbusSerial;
+};
--- a/radio/Tx.cpp
+++ b/radio/Tx.cpp
@@ -0,0 +1,381 @@
+#include "USB-Serial.h"
+#include "config.h"
+#include "radio-protocol.h"
+#include "utility.h"
+#include <Arduino.h>
+#include <FreeRTOS.h>
+#include <cmath>
+#include <esp_system.h>
+#include <map>
+#include <stdexcept>
+#include <unordered_map>
+#include <vector>
+
+// Support heltec boards
+#ifndef LILYGO
+#include <heltec_unofficial.h>
+#endif // end ifndef LILYGO
+
+#if defined(LILYGO)
+// LiLyGO device does not support the auto download mode, you need to get into the
+// download mode manually. To do so, press and hold the BOOT button and then press the
+// RESET button once. After that release the BOOT button. Or OFF->ON together with BOOT
+
+// Default LilyGO code
+#include <LoRaBoards.h>
+
+// #include "utilities.h"
+//  Our Code
+#include <LiLyGo.h>
+#endif // end LILYGO
+
+#define RADIOLIB_GODMODE (1)
+#define RADIOLIB_CHECK_PARAMS (0)
+
+#include <RadioLib.h>
+
+// Define the UART ports and pins
+#define TXD1 39 // Transmit pin for Serial1
+#define RXD2 40 // Receive pin for Serial2
+
+long int lastWriteTime = 0;
+
+#if PROTOCOL == IBUS
+#include "IBUS.h"
+// Create an instance of the Ibus class
+Ibus ibus;
+// Test data list with all control values set to 1700
+uint8_t testControlValues[IBUS_CHANNELS_COUNT * 2];
+#endif
+
+#if PROTOCOL == SBUS
+#include "SBUS.h"
+SBUS2 sbus(Serial1); // Use Serial1 for SBUS output
+#endif
+
+#include "FHSS.h"
+
+#if PROTOCOL == CRSF
+#include "CRSF.h"
+// CRSF crsf(Serial1, TXD1, TXD1, 420000); // Use Serial1, TX_PIN, RX_PIN, BAUD_RATE
+CRSF2 crsf(Serial1, -1, TXD1);
+#endif
+// Example usage
+
+#include "radio.h"
+
+#if RUN_TESTS
+void testMap11BitTo4Bit();
+void testMap4BitTo11Bit();
+#endif
+
+long int startTime = 0;
+void setup()
+{
+
+    Serial.begin(115200);
+
+    // Initialize Serial1 for iBUS communication with a custom TX pin
+#if PROTOCOL == IBUS
+    ibus.begin(Serial1, TXD1);
+    ibus.enable();
+#endif
+
+#if PROTOCOL == SBUS
+    // clearSbusData();
+#if LORA_RX
+    // sbusWrite.Begin();
+#endif
+#endif
+
+#if PROTOCOL == CRSF
+    crsf.begin();
+#endif // end CRSF
+
+#if RUN_TESTS
+    testMap11BitTo4Bit();
+    testMap4BitTo11Bit();
+#endif
+    // testMap11BitTo4Bit();
+    //  Initialize SBUS communication
+
+#if LORA_TX
+    // sbusRead.Begin();
+#endif
+    Serial.println("SBUS write and read are ready");
+
+    heltec_setup();
+    startTime = millis();
+
+    numChannels = generateFrequencies(syncWord, startFreq, stepKHz, maxWidthMHz);
+
+    delay(100);
+    Serial.println("------");
+    Serial.println("Generated Frequency Hopping Table [" + String(numChannels) + "]:");
+    for (int i = 0; i < numChannels; i++)
+    {
+        // Serial.println(String(i) + ": " + hopTable[i] + " MHz\n");
+    }
+    delay(1000);
+
+    printHopTable(numChannels); // Print the generated table
+
+#ifdef LILYGO
+    setupBoards(); // true for disable U8g2 display library
+    delay(200);
+    Serial.println("Setup LiLyGO board is done");
+    display.println("Setup LiLyGO board is done");
+#endif
+    /// beginGFSK
+    if (radio.begin() == RADIOLIB_ERR_NONE)
+    {
+        Serial.println("LoRa Initialized");
+    }
+    else
+    {
+        Serial.println("LoRa Initialization Failed!");
+        while (true)
+            ;
+    }
+
+    // Setup Radio
+    setupRadio();
+}
+
+String toBinary(int num, int bitSize = 4);
+std::map<int, int> lastSerialCommands;
+void loop()
+{
+// Read serial input
+#if LORA_TX && SERIAL_INPUT
+    Serial.println("Waiting for Serial Commands");
+    while (true)
+    {
+        std::map<int, int> serialCommands = readSerialInput();
+        if (serialCommands.empty())
+        {
+            // Serial.println("Waiting for Serial Commands");
+            delay(20);
+        }
+        else
+        {
+            lastSerialCommands = serialCommands;
+            break;
+        }
+    }
+#endif
+
+#if PROTOCOL == IBUS
+    uint32_t seed = esp_random() ^ millis();
+    randomSeed(seed);
+    String str = "";
+    // Set all control values to 1700
+    for (int i = 0; i < IBUS_CHANNELS_COUNT; i++)
+    {
+        uint16_t randomValue =
+            random(1200, 1900); // Generate random values between 1200 and 1900
+        str += String(randomValue) + ",";
+        testControlValues[i * 2] = randomValue & 0xFF;            // Low byte
+        testControlValues[i * 2 + 1] = (randomValue >> 8) & 0xFF; // High byte
+    }
+    Serial.println("I-BUS:" + str);
+    display.println("I-BUS:" + str);
+
+    ibus.setControlValuesList(testControlValues);
+    ibus.sendPacket();
+    delay(20);
+#endif // end IBUS
+
+#if PROTOCOL == SBUS
+    uint32_t seed = esp_random() ^ millis();
+
+    randomSeed(seed);
+    //  Set all control values to 1700
+
+    uint16_t sbusSend[16] = INIT_SBUS_ARRAY;
+
+    for (int i = 0; i < 12; i++)
+    {
+        uint16_t randomValue = random(1200, 1900);
+        sbusSend[i] = randomValue; // map4BitTo11Bit(randomValue);
+    }
+    // writeSbusData(sbusSend);
+    sbus.send(sbusSend);
+    display.println("SBUS");
+    Serial.println("SBUS: " + String(sbusSend[0]) + "," + String(sbusSend[1]) + "," +
+                   String(sbusSend[2]) + "," + String(sbusSend[3]) + "," +
+                   String(sbusSend[4]) + "," + String(sbusSend[5]) + "," +
+                   String(sbusSend[6]) + "," + String(sbusSend[7]) + "," +
+                   String(sbusSend[8]) + "," + String(sbusSend[9]) + "," +
+                   String(sbusSend[10]) + "," + String(sbusSend[11]));
+    delay(30);
+    //   Read data for test purpose
+    //  readSbusData();
+#endif // end SBUS
+
+#if PROTOCOL == CRSF
+    display.println("CRSF");
+    uint32_t seed = esp_random() ^ millis();
+
+    randomSeed(seed);
+    //  Set all control values to 1700
+
+    uint16_t sbusSend[16] = INIT_SBUS_ARRAY;
+    // Example: Set channel values
+    uint16_t channels[CRSF_MAX_CHANNELS] = {random(1200, 1900),
+                                            random(1200, 1900),
+                                            random(1200, 1900),
+                                            random(1200, 1900),
+                                            random(1200, 1900),
+                                            random(1200, 1900),
+                                            random(1200, 1900),
+                                            1435,
+                                            1450,
+                                            1800,
+                                            1300,
+                                            1000,
+                                            1500,
+                                            2000,
+                                            1200,
+                                            1300};
+
+    crsf.sendRCFrame(channels);
+    Serial.println("CRSF: " + String(channels[0]) + "," + String(channels[1]) + "," +
+                   String(channels[2]) + "," + String(channels[3]));
+    delay(50);
+#endif // end CRSF
+
+    uint8_t cmd1 = 0;  // Example command 1
+    uint8_t val1 = 5;  // Example value 1
+    uint8_t cmd2 = 1;  // Example command 2
+    uint8_t val2 = 10; // Example value 2
+    uint8_t val3 = 0, val4 = 0;
+    uint8_t cmd3 = 2, cmd4 = 3;
+    long int start = millis();
+#if LORA_FHSS
+    updateFrequency();
+#endif
+#if LORA_TX
+    // listen to the Sbus commands form the Ground station or RC
+    // readSbusData();
+
+    val1 = convertTo4Bit(testChannels[4]);
+    val2 = convertTo4Bit(testChannels[1]);
+    // 4 byte packet
+    val3 = convertTo4Bit(testChannels[2]);
+    val4 = convertTo4Bit(testChannels[3]);
+    if (val3 != 8 && val4 != 8)
+    {
+        radio.setBandwidth(62.5);
+        // RC 1 1500
+        sendLoRaRCPacket(cmd1, val1, cmd2, val2, cmd3, val3, cmd4, val4);
+        delay(500);
+        radio.setBandwidth(500);
+        sendLoRaRCPacket(cmd1, val1, cmd2, val2, cmd3, val3, cmd4, val4);
+    }
+    else
+    {
+        radio.setBandwidth(62.5);
+        sendLoRaRCPacket(cmd1, val1, cmd2, val2);
+        delay(500);
+        radio.setBandwidth(500);
+        sendLoRaRCPacket(cmd1, val1, cmd2, val2);
+    }
+
+    packetNumber++;
+
+    if (packetNumber % 10 == 0)
+    {
+        // 8 B
+        uint8_t data8[8];
+        data8[0] = (packetNumber >> 56) & 0xFF;
+        data8[1] = (packetNumber >> 48) & 0xFF;
+        data8[2] = (packetNumber >> 40) & 0xFF;
+        data8[3] = (packetNumber >> 32) & 0xFF;
+        data8[4] = (packetNumber >> 24) & 0xFF;
+        data8[5] = (packetNumber >> 16) & 0xFF;
+        data8[6] = (packetNumber >> 8) & 0xFF;
+        data8[7] = packetNumber & 0xFF;
+        packetNumber++;
+
+        Serial.println("Sending 8 byte packet Number(" + String(sizeof(data8)) + ")");
+        sendLoRaPacket(data8, 8);
+        heltec_delay(50);
+    }
+
+#endif
+#if LORA_RX
+    if (packetReceived)
+    {
+        packetReceived = false;
+        onReceive();
+    }
+    /*if (millis() - lastPacketTime > 15000)
+    { // No packet for 15s? Reset.
+        Serial.println("[LoRa] No packets received for a while, restarting...");
+        forceRestartLoRa();
+    }*/
+    // Serial.print("[LoRa] Current Status: ");
+#endif
+
+    long int end = millis();
+
+#if LORA_TX && LORA_FHSS
+    long int currentTime = millis();
+    if (currentTime - startTime < PACKET_SEND_DURATION)
+    {                                    // Check if within first 5 minutes
+        char packetData[LORA_DATA_BYTE]; // 2-byte array
+
+        // Store packet number into 2 bytes (big-endian format)
+        packetData[0] = (packetNumber >> 8) & 0xFF; // High byte
+        packetData[1] = packetNumber & 0xFF;        // Low byte
+        radio.setSpreadingFactor(5);
+        radio.setBandwidth(125.0);
+
+        radio.transmit((uint8_t *)packetData, 2); // Send exactly 2 bytes
+        Serial.printf("Sent: %s on %.3f MHz\n", packetData, hopTable[hopIndex]);
+    }
+    radio.setSpreadingFactor(LORA_SF);
+    radio.setBandwidth(LORA_BW);
+#endif
+
+#if LORA_TX
+
+#if LORA_FHSS
+    Serial.printf("Hopping [%s] to: %.3f MHz\n", String(packetNumber), currentFreq);
+    display.printf("FHSS: %.3fMHz\n", currentFreq);
+#endif
+    Serial.println("Packet: " + String(packetSave));
+
+    display.println("Time in the Air: " + String((end - start)));
+    Serial.println("Time in the Air: " + String((end - start)));
+
+    display.println("P:" + String(cmd1) + ":" + String(val1) + ":" + String(cmd2) + ":" +
+                    String(val2));
+    display.println("BP:" + toBinary(cmd1) + ":" + toBinary(val1) + ":" + toBinary(cmd2) +
+                    ":" + toBinary(val2));
+
+    Serial.println("P:" + String(cmd1) + ":" + String(val1) + ":" + String(cmd2) + ":" +
+                   String(val2));
+    Serial.println("BP:" + toBinary(cmd1) + ":" + toBinary(val1) + ":" + toBinary(cmd2) +
+                   ":" + toBinary(val2));
+
+    if (val3 != 8 && val4 != 8)
+    {
+        Serial.println("P2:" + String(cmd3) + ":" + String(val3) + ":" + String(cmd4) +
+                       ":" + String(val4));
+        Serial.println("BP2:" + toBinary(cmd3) + ":" + toBinary(val3) + ":" +
+                       toBinary(cmd4) + ":" + toBinary(val4));
+    }
+    // display.println("Packet Sent");
+    // delay(1000);
+#endif
+}
+
+// this function is called when a complete packet
+// is received by the module
+// IMPORTANT: this function MUST be 'void' type
+//            and MUST NOT have any arguments!
+#if defined(ESP8266) || defined(ESP32)
+ICACHE_RAM_ATTR
+#endif
--- a/radio/USB-Serial.h
+++ b/radio/USB-Serial.h
@@ -0,0 +1,96 @@
+#include <Arduino.h>
+#include <map>
+
+std::map<int, int> readSerialInput();
+void dumpCommand(const std::map<int, int> &map);
+std::map<int, int> processSerialCommand(const String &input);
+
+std::map<int, int> readSerialInput()
+{
+    std::map<int, int> result;
+    String input = "";
+    if (Serial.available() > 0)
+    {
+        input = Serial.readStringUntil(
+            '\n');    // Read the incoming data until a newline character
+        input.trim(); // Remove any leading or trailing whitespace
+    }
+
+    if (!input.isEmpty())
+    {
+        Serial.println("SERIAL Data: " + input);
+        delay(1000);
+        result = processSerialCommand(input);
+    }
+
+    if (!result.empty())
+    {
+        Serial.println("The map contains data: ");
+        dumpCommand(result);
+        delay(1000);
+    }
+
+    return result; // Return an empty string if no data is available
+}
+
+void dumpCommand(const std::map<int, int> &map)
+{
+    for (const auto &pair : map)
+    {
+        Serial.println("Key: " + String(pair.first) + ", Value: " + String(pair.second));
+    }
+}
+
+std::map<int, int> processSerialCommand(const String &input)
+{
+    const int MAX_KEYS = 6;
+
+    std::map<int, int> keyValuePairs;
+    if (input.startsWith("RC "))
+    {
+        int key1 = -1, value1 = -1;
+        int key2 = -1, value2 = -1;
+        int key3 = -1, value3 = -1;
+        int key4 = -1, value4 = -1;
+        int key5 = -1, value5 = -1;
+        int key6 = -1, value6 = -1;
+
+        char command[3];
+        int parsed = sscanf(input.c_str(), "%s %d %d %d %d %d %d %d %d %d %d %d %d",
+                            command, &key1, &value1, &key2, &value2, &key3, &value3,
+                            &key4, &value4, &key5, &value5, &key6, &value6);
+
+        // Create arrays to hold keys and values
+        int keys[MAX_KEYS] = {key1, key2, key3, key4, key5, key6};
+        int values[MAX_KEYS] = {value1, value2, value3, value4, value5, value6};
+
+        if (parsed >= 3)
+        {
+            Serial.print("Received command: ");
+            Serial.print(command);
+            Serial.print(" ");
+            for (int i = 0; i < parsed - 2; i++)
+            {
+                if (values[i] != -1)
+                {
+                    keyValuePairs[keys[i]] = values[i];
+                }
+                Serial.print("Key[" + String(i) + "]: ");
+                Serial.print(keys[i]);
+                Serial.print(" Value[" + String(i) + "]: ");
+                Serial.print(String(values[i]) + ", ");
+            }
+            Serial.println(" ");
+            Serial.println(">--------------------------------<");
+        }
+        else
+        {
+            Serial.println("Invalid command format or out of range values.");
+        }
+    }
+    else
+    {
+        Serial.println("Invalid command prefix.");
+    }
+    return keyValuePairs;
+}
--- a/radio/config.h
+++ b/radio/config.h
@@ -0,0 +1,75 @@
+#pragma once
+
+#define RUN_TESTS 0
+
+#define TXD1 39
+#define RXD2 40
+// SBUS packet structure
+#define SBUS_PACKET_SIZE 25
+
+#ifndef LORA_SF
+// Sets LoRa spreading factor. Allowed values range from 5 to 12.
+#define LORA_SF 8 // For retranmission 6.
+#endif
+
+#ifndef LORA_CR
+#define LORA_CR 8
+#endif
+
+#ifndef DEBUG_RX
+#define DEBUG_RX 0
+#endif
+
+#ifndef LORA_HEADER
+#define LORA_HEADER 0
+#endif
+
+#ifndef LORA_FHSS
+#define LORA_FHSS 0
+#endif
+
+#ifndef LORA_RX
+#define LORA_RX 0
+#endif
+
+#ifndef LORA_TX
+#define LORA_TX 1
+#endif
+
+#ifndef LORA_BASE_FREQ
+// Sets LoRa coding rate denominator. Allowed values range from 5 to 8.
+#define LORA_BASE_FREQ 915
+#endif
+
+#ifndef LORA_BW
+// Sets LoRa bandwidth. Allowed values are 62.5, 125.0, 250.0 and 500.0 kHz. (default,
+// high = false)
+#define LORA_BW 62.5 // 31.25 // 125.0 // 62.5 // 31.25 // 500
+#endif
+
+#ifndef LORA_DATA_BYTE
+#define LORA_DATA_BYTE 2
+#endif
+
+#define SBUS 1
+#define IBUS 2
+#define CRSF 3 // Doesn't work
+
+#ifndef PROTOCOL
+#define PROTOCOL CRSF // CRSF // IBUS // SBUS
+#endif
+
+#ifndef SERIAL_INPUT
+#define SERIAL_INPUT 0
+#endif
+
+#ifndef LORA_PREAMBLE
+// 8 is default
+#if LORA_SF == 6 || LORA_SF == 5
+#define LORA_PREAMBLE 8
+#elif LORA_SF == 7
+#define LORA_PREAMBLE 8
+#else
+#define LORA_PREAMBLE 10
+#endif
+#endif
--- a/radio/iBUS.h
+++ b/radio/iBUS.h
@@ -0,0 +1,161 @@
+#include <Arduino.h>
+#include <HardwareSerial.h>
+// Define constants for iBUS
+#define IBUS_BAUD_RATE 115200
+#define IBUS_SEND_INTERVAL_MS 20
+#define IBUS_CHANNELS_COUNT 14
+#define IBUS_PACKET_BYTES_COUNT ((IBUS_CHANNELS_COUNT * 2) + 4)
+
+// Define the custom TX pin
+#define CUSTOM_TX_PIN 17 // Change this to your desired TX pin
+
+// Define the Ibus class
+class Ibus
+{
+  public:
+    void begin(HardwareSerial &serial, int txPin);
+    void loop();
+    void enable();
+    void disable();
+    void readLoop();
+    void sendPacket();
+    bool unpackIbusData(uint8_t *packet);
+    void setControlValue(uint8_t channel, uint8_t value);
+    void setControlValuesList(uint8_t list[IBUS_CHANNELS_COUNT * 2]);
+
+  private:
+    HardwareSerial *serial;
+    uint8_t controlValuesList[IBUS_CHANNELS_COUNT * 2] = {0};
+    bool isEnabled = false;
+    unsigned long previousMillis = 0;
+    unsigned long currentMillis = 0;
+
+    uint8_t *createPacket();
+};
+
+// Implement the Ibus methods
+void Ibus::begin(HardwareSerial &serial, int txPin)
+{
+    this->serial = &serial;
+    this->serial->begin(IBUS_BAUD_RATE, SERIAL_8N1, -1, txPin); // Set custom TX pin
+}
+
+void Ibus::loop()
+{
+    if (this->isEnabled)
+    {
+        this->sendPacket();
+        // this->readLoop();
+    }
+}
+
+uint8_t *Ibus::createPacket()
+{
+    static uint8_t packetBytesList[IBUS_PACKET_BYTES_COUNT];
+    packetBytesList[0] = 0x20;
+    packetBytesList[1] = 0x40;
+    uint_fast16_t checksum = 0xFFFF - 0x20 - 0x40;
+    for (size_t i = 2; i < (IBUS_CHANNELS_COUNT * 2) + 2; i++)
+    {
+        packetBytesList[i] = this->controlValuesList[i - 2];
+        checksum -= packetBytesList[i];
+    }
+    packetBytesList[IBUS_PACKET_BYTES_COUNT - 2] = lowByte(checksum);
+    packetBytesList[IBUS_PACKET_BYTES_COUNT - 1] = highByte(checksum);
+    return packetBytesList;
+}
+
+void Ibus::sendPacket()
+{
+    if (this->isEnabled)
+    {
+        uint8_t *packetBytesList = this->createPacket();
+        for (size_t i = 0; i < IBUS_PACKET_BYTES_COUNT; i++)
+        {
+            this->serial->write(packetBytesList[i]);
+        }
+    }
+}
+
+void Ibus::enable() { this->isEnabled = true; }
+
+void Ibus::disable() { this->isEnabled = false; }
+
+void Ibus::setControlValuesList(uint8_t list[IBUS_CHANNELS_COUNT * 2])
+{
+    for (size_t i = 0; i < (IBUS_CHANNELS_COUNT * 2); i++)
+    {
+        this->controlValuesList[i] = list[i];
+    }
+}
+
+void Ibus::setControlValue(uint8_t channel, uint8_t value)
+{
+    this->controlValuesList[channel] = value;
+}
+
+bool Ibus::unpackIbusData(uint8_t *packet)
+{
+    // Verify start and length bytes
+    if (packet[0] != 0x20 || packet[1] != 0x40)
+    {
+        return false; // Invalid packet
+    }
+
+    // Calculate checksum
+    uint_fast16_t checksum = 0xFFFF;
+    for (int i = 0; i < IBUS_PACKET_BYTES_COUNT - 2; i++)
+    {
+        checksum -= packet[i];
+    }
+
+    // Verify checksum
+    uint_fast16_t receivedChecksum =
+        packet[IBUS_PACKET_BYTES_COUNT - 2] | (packet[IBUS_PACKET_BYTES_COUNT - 1] << 8);
+    if (checksum != receivedChecksum)
+    {
+        return false; // Checksum mismatch
+    }
+
+    // Extract channel values
+    for (int i = 0; i < IBUS_CHANNELS_COUNT; i++)
+    {
+        this->controlValuesList[i] = packet[2 + i * 2] | (packet[3 + i * 2] << 8);
+    }
+
+    return true; // Successfully unpacked
+}
+
+void Ibus::readLoop()
+{
+    uint8_t ibusPacket[IBUS_PACKET_BYTES_COUNT];
+    int packetIndex = 0;
+    while (this->serial->available())
+    {
+        uint8_t byte = this->serial->read();
+
+        // Store byte in packet buffer
+        ibusPacket[packetIndex++] = byte;
+
+        // Check if we have a full packet
+        if (packetIndex == IBUS_PACKET_BYTES_COUNT)
+        {
+            if (unpackIbusData(ibusPacket))
+            {
+                Serial.println("iBUS Data Unpacked:");
+                for (int i = 0; i < IBUS_CHANNELS_COUNT; i++)
+                {
+                    Serial.print("Channel ");
+                    Serial.print(i + 1);
+                    Serial.print(": ");
+                    Serial.println(this->controlValuesList[i]);
+                }
+            }
+            else
+            {
+                Serial.println("Failed to unpack iBUS data.");
+            }
+            packetIndex = 0; // Reset for next packet
+        }
+    }
+}
--- a/radio/radio-protocol.h
+++ b/radio/radio-protocol.h
@@ -0,0 +1,153 @@
+#include <Arduino.h>
+#include <RadioCommands.h>
+
+uint8_t convertTo4Bit(uint16_t value11Bit);
+int16_t map4BitTo11Bit(uint8_t value4Bit);
+uint8_t map11BitTo4Bit(uint16_t value11Bit);
+
+uint8_t convertTo4Bit(uint16_t value11Bit) { return map11BitTo4Bit(value11Bit); }
+
+uint8_t map11BitTo4Bit(uint16_t value11Bit)
+{
+    // Initialize variables to track the closest match
+    uint8_t closest4BitValue = 0;
+    uint16_t smallestDifference = UINT16_MAX;
+
+    const auto &lastEntry = channelValueMappingTable.back();
+    // Find the closest match in the table
+    for (const auto &entry : channelValueMappingTable)
+    {
+
+        uint8_t key = entry.first;     // Access the key
+        uint16_t value = entry.second; // Access the value
+        if (value11Bit >= lastEntry.second)
+        {
+            closest4BitValue = lastEntry.first;
+            break;
+        }
+        else if (value11Bit <= value)
+        {
+            closest4BitValue = key;
+            break;
+        }
+    }
+
+    return closest4BitValue;
+}
+
+int16_t map4BitTo11Bit(uint8_t value4Bit)
+{
+    // Iterate through the mapping table to find the corresponding 11-bit value
+    for (const auto &entry : channelValueMappingTable)
+    {
+        uint8_t key = entry.first;     // Access the 4-bit key
+        uint16_t value = entry.second; // Access the 11-bit value
+
+        if (key == value4Bit)
+        {
+            return value; // Return the 11-bit value if the key matches
+        }
+    }
+
+    // If no match is found, return a default value or handle the error
+    // For example, return 0 - 1500 or throw an exception
+    return 1500; // Or handle the error as needed
+}
+
+// Test function
+void testMap11BitTo4Bit()
+{
+    Serial.println("Test Mapping 11-bit to 4-bit");
+
+    // Test cases: {input, expected_output}
+    std::vector<std::pair<uint16_t, uint8_t>> testCases = {
+        {1290, 0}, {1300, 0},  {1310, 1},  {1325, 1},  {1340, 2},
+        {1500, 8}, {1600, 12}, {1700, 15}, {1675, 15}, {1800, 15}};
+    // 0 - 1300 0
+    // 1300 - 1325 1
+    // 1325 - 1350 2
+    // 1350 - 1375 3
+    // 1375 - 1400 4
+    // 1400 - 1425 5
+    // 1425 - 1450 6
+    // 1450 - 1475 7
+    // 1475 - 1500 8
+    // 1500 - 1525 9
+    // 1525 - 1550 10
+    // 1550 - 1575 11
+    // 1575 - 1600 12
+    // 1600 - 1625 13
+    // 1625 - 1650 14
+    // 1650 - 1675 15
+    bool failed = false;
+
+    for (const auto &testCase : testCases)
+    {
+        uint16_t input = testCase.first;
+        uint8_t expectedOutput = testCase.second;
+        uint8_t actualOutput = map11BitTo4Bit(input);
+        bool assert = actualOutput == expectedOutput;
+        if (!assert)
+        {
+            Serial.print("Test Failed->");
+            failed = true;
+        }
+        Serial.println("Test input " + String(input) + ": expected " +
+                       String(expectedOutput) + ", got " + String(actualOutput));
+        delay(100);
+    }
+    if (failed)
+    {
+        Serial.println("Test Failed");
+        delay(500);
+    }
+}
+
+// Test function
+void testMap4BitTo11Bit()
+{
+    Serial.println("Testing map4BitTo11Bit");
+    // Test cases: {input, expected_output}
+    std::vector<std::pair<uint8_t, uint16_t>> testCases = {
+        {0, 1300}, {0, 1300},  {1, 1325},  {1, 1325},  {2, 1350},
+        {8, 1500}, {12, 1600}, {15, 1675}, {14, 1650}, {18, 1500}};
+
+    // 1300 0
+    // 1325 1
+    // 1350 2
+    // 1375 3
+    // 1400 4
+    // 1425 5
+    // 1450 6
+    // 1475 7
+    // 1500 8
+    // 1525 9
+    // 1550 10
+    // 1575 11
+    // 1600 12
+    // 1625 13
+    // 1650 14
+    // 1675 15
+    bool failed = false;
+
+    for (const auto &testCase : testCases)
+    {
+        uint16_t input = testCase.first;
+        uint16_t expectedOutput = testCase.second;
+        uint16_t actualOutput = map4BitTo11Bit(input);
+        bool assert = actualOutput == expectedOutput;
+        if (!assert)
+        {
+            Serial.print("Test Failed->");
+            failed = true;
+        }
+        Serial.println("Test input " + String(input) + ": expected " +
+                       String(expectedOutput) + ", got " + String(actualOutput));
+        delay(100);
+    }
+    if (failed)
+    {
+        Serial.println("Test Failed");
+        delay(500);
+    }
+}
--- a/radio/radio.h
+++ b/radio/radio.h
@@ -0,0 +1,378 @@
+#include "SSD1306Wire.h"
+#include <LiLyGo.h>
+#include <LoRaBoards.h>
+#include <RadioLib.h>
+
+void onReceive();
+void onReceiveFlag(void);
+void forceRestartLoRa();
+
+bool radioIsRX = false;
+unsigned long lastPacketTime = 0;
+long int packetN = 0;
+int packetSave = 0;
+bool packetReceived = false;
+// Function to send 2-byte LoRa packet OR 4-byte LoRa packet
+void sendLoRaRCPacket(uint8_t cmd1, uint8_t val1, uint8_t cmd2, uint8_t val2,
+                      uint8_t cmd3 = 0, uint8_t val3 = 0, uint8_t cmd4 = 0,
+                      uint8_t val4 = 0)
+{
+    cmd1 &= 0x0F;
+    val1 &= 0x0F;
+    cmd2 &= 0x0F;
+    val2 &= 0x0F;
+    int packetSize = LORA_DATA_BYTE;
+    if (cmd3 != 0 && val3 != 0)
+    {
+        cmd3 &= 0x0F;
+        val3 &= 0x0F;
+        cmd4 &= 0x0F;
+        val4 &= 0x0F;
+        packetSize = 4;
+    }
+
+    Serial.printf("Sending LoRa Packet: CMD1=%d, VAL1=%d, CMD2=%d, VAL2=%d, ", cmd1, val1,
+                  cmd2, val2);
+    if (packetSize == 4)
+    {
+        Serial.printf("CMD3=%d, VAL3=%d, CMD4=%d, VAL4=%d, ", cmd3, val3, cmd4, val4);
+    }
+    Serial.println();
+
+    uint8_t paddedData[packetSize] = {0}; // Initialize with zeros
+    // P:2:5:4:10
+    // 0010:0101:0100:1010
+    // RX 11001110
+    // 9546
+    // 0010:0101:0100:1010
+    uint16_t packet = (cmd1 << 12) | (val1 << 8) | (cmd2 << 4) | val2;
+    uint16_t packet2 = 0;
+
+    if (packetSize == 4)
+    {
+        packet2 = (cmd3 << 12) | (val3 << 8) | (cmd4 << 4) | val4;
+    }
+
+    packetSave = packet;
+    if (packet2 != 0)
+    {
+        packetSave = (static_cast<uint32_t>(packet) << 16) | packet2;
+    }
+
+    uint8_t data[packetSize] = {0};
+    data[0] = (packet >> 8) & 0xFF; // High byte
+    data[1] = packet & 0xFF;        // Low byte
+
+    if (packetSize == 4)
+    {
+        data[2] = (packet2 >> 8) & 0xFF; // High byte
+        data[3] = packet2 & 0xFF;        // Low byte
+    }
+
+    int len = sizeof(data);
+
+    Serial.println("Packet  length: " + String(len));
+    Serial.print("Sending Packet: ");
+    Serial.print(data[0], BIN);
+    Serial.print(" ");
+    Serial.print(data[1], BIN);
+    if (packetSize == 4)
+    {
+        Serial.print(" ");
+        Serial.print(data[2], BIN);
+        Serial.print(" ");
+        Serial.print(data[3], BIN);
+    }
+    Serial.println();
+
+    // size_t dataSize = sizeof(data) / sizeof(data[0]);
+    // memcpy(paddedData, data, min(dataSize, (size_t)LORA_DATA_BYTE));
+
+    int status = radio.transmit(data, sizeof(data));
+    if (status == RADIOLIB_ERR_NONE)
+    {
+        Serial.println("LoRa Packet Sent!");
+    }
+    else
+    {
+        Serial.println("LoRa Transmission Failed.");
+    }
+}
+
+void sendLoRaPacket(uint8_t *packetData, int length)
+{
+    // Size of pointer here not an array count.
+    // int length = sizeof(packetData);
+    String packetStr = "";
+
+    Serial.println("Packet  length: " + String(length));
+
+    Serial.print("Sending Packet: ");
+    for (size_t i = 0; i < length; i++)
+    {
+        Serial.print(packetData[i], BIN);
+        Serial.print(" ");
+        packetStr += String(packetData[i]);
+    }
+    Serial.println();
+    display.println("P[" + String(length) + "]:" + packetStr);
+    Serial.println("P[" + String(length) + "]:" + packetStr);
+
+    int status = radio.transmit(packetData, length);
+    if (status == RADIOLIB_ERR_NONE)
+    {
+        Serial.println("LoRa Packet Sent!");
+    }
+    else
+    {
+        Serial.println("LoRa Transmission Failed.");
+    }
+}
+
+void onReceiveFlag(void) { packetReceived = true; }
+
+void onReceive(void)
+{
+#if LORA_RX
+    receivedPacketCounter++;
+    size_t len = radio.getPacketLength(true);
+    uint8_t data[len] = {0};
+    Serial.println("[LoRa] onReceive(" + String(len) + ")");
+
+#if DEBUG_RX
+    Serial.println("[LoRa] onReceive");
+    if (len > LORA_DATA_BYTE)
+    {
+        Serial.println("WARNING: Packet size is too large:" + String(len));
+    }
+
+    Serial.println("[LoRa] Length: " + String(len));
+#endif
+    int state = radio.readData(data, len);
+
+    if (state == RADIOLIB_ERR_NONE)
+    {
+#if DEBUG_RX
+        if (sizeof(data) != LORA_DATA_BYTE)
+        {
+            Serial.println("[LoRa] ERROR: Packet Length Mismatch!");
+        }
+#endif
+        // Packet size 4 processing
+        if (len == 8)
+        {
+            uint64_t seqNum = (static_cast<uint64_t>(data[0]) << 56) |
+                              (static_cast<uint64_t>(data[1]) << 48) |
+                              (static_cast<uint64_t>(data[2]) << 40) |
+                              (static_cast<uint64_t>(data[3]) << 32) |
+                              (static_cast<uint64_t>(data[4]) << 24) |
+                              (static_cast<uint64_t>(data[5]) << 16) |
+                              (static_cast<uint64_t>(data[6]) << 8) |
+                              static_cast<uint64_t>(data[7]);
+#if DEBUG
+
+            Serial.println("Lost:" + String(seqNum) + "/" +
+                           String(receivedPacketCounter) + ":" +
+                           String(seqNum - receivedPacketCounter));
+#endif
+            int lost = seqNum - receivedPacketCounter;
+            display.println("Lost:" + String(seqNum) + "/" +
+                            String(receivedPacketCounter) + ":" + String(lost));
+        }
+
+        // Check if data is actually zero
+        else if (data[0] == 0 && data[1] == 0)
+        {
+            int length = len;
+#if DEBUG
+            Serial.println("[LoRa] WARNING: Received 0 : 0. I don't know why");
+            Serial.print("[LoRa Receiver] Packet length: ");
+
+            Serial.println(String(length));
+
+            // return; // Ignore this packet
+            Serial.print("[LoRa Receiver] RSSI: ");
+
+            Serial.print(radio.getRSSI());
+            // display.println("0-0");
+
+            Serial.print("[LoRa Receiver] SNR: ");
+            Serial.print(radio.getSNR());
+            Serial.println(" dB");
+#endif
+        }
+        else if (len == 2 || len == 4) //** ToDo: process length 4 */)
+        {
+            int length = len;
+#if DEBUG
+            Serial.println("[LoRa Receiver] Packet Received!");
+            Serial.print("[LoRa Receiver] Packet length: ");
+            Serial.println(String(length));
+            Serial.println("[LoRa Receiver] DATA: " + String(data[0]) + ":" +
+                           String(data[1]));
+#endif
+            display.println("DATA[" + String(packetN) + "]: " + String(data[0]) + ":" +
+                            String(data[1]));
+            if (len == 4)
+            {
+                display.println("DATA-4[" + String(packetN) + "]: " + String(data[2]) +
+                                ":" + String(data[3]));
+            }
+            packetN++;
+            // Decode received data
+            uint16_t receivedPacket = (data[0] << 8) | data[1];
+            uint8_t cmd1 = (receivedPacket >> 12) & 0x0F;
+            uint8_t val1 = (receivedPacket >> 8) & 0x0F;
+            uint8_t cmd2 = (receivedPacket >> 4) & 0x0F;
+            uint8_t val2 = receivedPacket & 0x0F;
+
+            display.println("Data:" + String(cmd1) + ":" + String(val1) + ":" +
+                            String(cmd2) + ":" + (val2));
+
+            // If size is 4, decode another 16 bits
+            uint8_t cmd3 = 0, val3 = 0, cmd4 = 0, val4 = 0;
+
+            if (len == 4)
+            {
+                uint16_t receivedPacket2 = (data[2] << 8) | data[3];
+                uint8_t cmd3 = (receivedPacket2 >> 12) & 0x0F;
+                uint8_t val3 = (receivedPacket2 >> 8) & 0x0F;
+                uint8_t cmd4 = (receivedPacket2 >> 4) & 0x0F;
+                uint8_t val4 = receivedPacket2 & 0x0F;
+
+                display.println("Data:" + String(cmd3) + ":" + String(val3) + ":" +
+                                String(cmd4) + ":" + (val4));
+            }
+
+#if DEBUG
+            // Print received data
+            Serial.print("[LoRa Receiver] Data: ");
+            Serial.print("CMD1=");
+            Serial.print(cmd1);
+            Serial.print(", VAL1=");
+            Serial.print(val1);
+            Serial.print(", CMD2=");
+            Serial.print(cmd2);
+            Serial.print(", VAL2=");
+            Serial.println(val2);
+            Serial.print("[LoRa Receiver] Data: ");
+
+            // Print RSSI (Signal Strength)
+            Serial.print("[LoRa Receiver] RSSI: ");
+            Serial.print(radio.getRSSI());
+            Serial.println(" dBm");
+
+            // Print SNR (Signal-to-Noise Ratio)
+            Serial.print("[LoRa Receiver] SNR: ");
+            Serial.print(radio.getSNR());
+            Serial.println(" dB");
+#endif
+            display.print("RSSI: " + String(radio.getRSSI()));
+            display.println(" SNR: " + String(radio.getSNR()));
+        }
+    }
+    else if (state == RADIOLIB_ERR_RX_TIMEOUT)
+    {
+        //  No packet received
+        Serial.println("[LoRa Receiver] No packet received.");
+    }
+    else if (state == RADIOLIB_ERR_CRC_MISMATCH)
+    {
+        // Packet received but corrupted
+        Serial.println("[LoRa Receiver] Packet received but CRC mismatch!");
+    }
+    else
+    {
+        // Other error
+        Serial.print("[LoRa Receiver] Receive failed, error code: ");
+        Serial.println(state);
+    }
+    // Restart LoRa receiver
+    // radio.implicitHeader(LORA_DATA_BYTE);
+    radio.startReceive();
+#endif
+}
+
+void forceRestartLoRa()
+{
+    Serial.println("[LoRa] Forcing Restart...");
+    radio.standby();
+    radio.reset();
+    delay(100);
+    radio.begin();
+    radio.startReceive();
+    lastPacketTime = millis(); // Reset timeout
+}
+
+void initRadioSwitchTable()
+{
+#ifdef USING_LR1121 // USING_SX1262
+    // LR1121
+    // set RF switch configuration for Wio WM1110
+    // Wio WM1110 uses DIO5 and DIO6 for RF switching
+    static const uint32_t rfswitch_dio_pins[] = {RADIOLIB_LR11X0_DIO5,
+                                                 RADIOLIB_LR11X0_DIO6, RADIOLIB_NC,
+                                                 RADIOLIB_NC, RADIOLIB_NC};
+
+    static const Module::RfSwitchMode_t rfswitch_table[] = {
+        // mode                  DIO5  DIO6
+        {LR11x0::MODE_STBY, {LOW, LOW}},  {LR11x0::MODE_RX, {HIGH, LOW}},
+        {LR11x0::MODE_TX, {LOW, HIGH}},   {LR11x0::MODE_TX_HP, {LOW, HIGH}},
+        {LR11x0::MODE_TX_HF, {LOW, LOW}}, {LR11x0::MODE_GNSS, {LOW, LOW}},
+        {LR11x0::MODE_WIFI, {LOW, LOW}},  END_OF_MODE_TABLE,
+    };
+    radio.setRfSwitchTable(rfswitch_dio_pins, rfswitch_table);
+
+    // LR1121 TCXO Voltage 2.85~3.15V
+    radio.setTCXO(3.0);
+    heltec_delay(500);
+#endif
+}
+
+void setupRadio()
+{
+    radio.setFrequency(LORA_BASE_FREQ);
+    radio.setBandwidth(LORA_BW);
+    radio.setSpreadingFactor(LORA_SF);
+#if LORA_HEADER
+    // Some issue it receives 0 0
+    radio.implicitHeader(LORA_DATA_BYTE);
+#else
+    radio.explicitHeader();
+#endif
+    radio.setCodingRate(LORA_CR);
+    radio.setPreambleLength(LORA_PREAMBLE);
+    radio.forceLDRO(true);
+    radio.setCRC(2);
+#if LORA_TX
+    radio.setOutputPower(22);
+    display.println("Turn ON RX to pair you have 30 seconds");
+    for (int t = 0; t < 30; t++)
+    {
+        display.print(".");
+        delay(50);
+    }
+    display.println();
+#endif
+
+    initRadioSwitchTable();
+
+#if LORA_RX
+    radio.setPacketReceivedAction(onReceiveFlag);
+    int state = radio.startReceive();
+    if (state == RADIOLIB_ERR_NONE)
+    {
+        radioIsRX = true;
+        Serial.println("Listening for LoRa Packets...");
+    }
+    else
+    {
+        Serial.print("Receiver failed to start, code: ");
+        Serial.println(state);
+        while (true)
+        {
+            delay(5);
+        }
+    }
+#endif
+}
--- a/radio/utility.h
+++ b/radio/utility.h
@@ -0,0 +1,22 @@
+#include <Arduino.h>
+
+String toBinary(int num, int bitSize)
+{
+    if (num == 0)
+        return "0";
+
+    String binary = "";
+    while (num > 0)
+    {
+        binary = String(num % 2) + binary;
+        num /= 2;
+    }
+
+    // Pad with leading zeros to match `bitSize`
+    while (binary.length() < bitSize)
+    {
+        binary = "0" + binary;
+    }
+
+    return binary;
+}
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -23,64 +23,6 @@

 //  #define HELTEC_NO_DISPLAY

-#ifdef BT_MOBILE
-#include <NimBLEDevice.h>
-
-#define SERVICE_UUID "00001234-0000-1000-8000-00805f9b34fb"
-#define CHARACTERISTIC_UUID "00001234-0000-1000-8000-00805f9b34ac"
-
-NimBLEServer *pServer = nullptr;
-NimBLECharacteristic *pCharacteristic = nullptr;
-NimBLEAdvertising *pAdvertising = nullptr;
-
-void initBT()
-{
-    // Initialize BLE device
-    NimBLEDevice::init("RSSI_Radar");
-
-    // Get and print the MAC address
-    String macAddress = NimBLEDevice::getAddress().toString().c_str();
-    Serial.println("Bluetooth MAC Address: " + macAddress);
-
-    // Create BLE server
-    pServer = NimBLEDevice::createServer();
-
-    // Create a BLE service
-    NimBLEService *pService = pServer->createService(SERVICE_UUID);
-
-    // Create a BLE characteristic
-    pCharacteristic = pService->createCharacteristic(
-        CHARACTERISTIC_UUID, NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY);
-
-    // Start the service
-    pService->start();
-
-    // Start advertising
-
-    pAdvertising = NimBLEDevice::getAdvertising();
-    pAdvertising->addServiceUUID(SERVICE_UUID);
-    pAdvertising->setName("ESP32_RSSI_Radar"); // Set the device name
-    pAdvertising->setMinInterval(300);
-    pAdvertising->setMaxInterval(350);
-    // pAdvertising->setScanResponse(true); // Allow scan responses
-
-    pAdvertising->start();
-
-    Serial.println("BLE server started.");
-}
-
-// Function to send RSSI and Heading Data
-void sendBTData(float heading, float rssi)
-{
-    String data =
-        "RSSI_HEADING: '{H:" + String(heading) + ",RSSI:-" + String(rssi) + "}'";
-    Serial.println("Sending data: " + data);
-    pCharacteristic->setValue(data.c_str()); // Set BLE characteristic value
-    pCharacteristic->notify();               // Notify connected client
-}
-
-#endif
-
 #include "FS.h"
 #include <Arduino.h>
 #ifdef WEB_SERVER
@@ -120,6 +62,223 @@ void sendBTData(float heading, float rssi)
 #include <scan.h>
 #include <stdlib.h>

+#ifdef BT_MOBILE
+
+bool deviceConnected = false;
+#define SERVICE_UUID "00001234-0000-1000-8000-00805f9b34fb"
+#define CHARACTERISTIC_UUID "00001234-0000-1000-8000-00805f9b34ac"
+
+#ifndef BT_NM
+#include <BLE2902.h>
+#include <BLEDevice.h>
+#include <BLEServer.h>
+#include <BLEUtils.h>
+
+BLEServer *pServer = NULL;
+BLECharacteristic *pCharacteristic = NULL;
+BLEAdvertising *pAdvertising = NULL;
+
+class MyServerCallbacks : public BLEServerCallbacks
+{
+    void onConnect(BLEServer *pServer) { deviceConnected = true; };
+
+    void onDisconnect(BLEServer *pServer)
+    {
+        deviceConnected = false;
+        BLEDevice::startAdvertising(); // Restart advertising after disconnect
+    }
+};
+
+#else
+#include <NimBLEDevice.h>
+
+NimBLEServer *pServer = nullptr;
+NimBLECharacteristic *pCharacteristic = nullptr;
+NimBLEAdvertising *pAdvertising = nullptr;
+
+class BTServerCallbacks : public NimBLEServerCallbacks
+{
+  public:
+    void onConnect(NimBLEServer *pServer, NimBLEConnInfo &connInfo) override
+    {
+        deviceConnected = true;
+        Serial.printf("Device Connected | Free Heap: %d kByte\n",
+                      ESP.getFreeHeap() / 1000);
+        Serial.printf("Client address: %s\n", connInfo.getAddress().toString().c_str());
+
+        pServer->updateConnParams(connInfo.getConnHandle(), 24, 48, 0, 180);
+    }
+
+    void onDisconnect(NimBLEServer *pServer, NimBLEConnInfo &connInfo,
+                      int reason) override
+    {
+        deviceConnected = false;
+        Serial.println("Device Disconnected");
+        NimBLEDevice::startAdvertising(); // Restart advertising
+    }
+
+    void onMTUChange(uint16_t MTU, NimBLEConnInfo &connInfo) override
+    {
+        Serial.printf("MTU updated: %u for connection ID: %u\n", MTU,
+                      connInfo.getConnHandle());
+    }
+} BTServerCallbacks;
+
+class CharacteristicCallbacks : public NimBLECharacteristicCallbacks
+{
+    void onRead(NimBLECharacteristic *pCharacteristic, NimBLEConnInfo &connInfo) override
+    {
+        Serial.printf("%s : onRead(), value: %s\n",
+                      pCharacteristic->getUUID().toString().c_str(),
+                      pCharacteristic->getValue().c_str());
+    }
+
+    void onWrite(NimBLECharacteristic *pCharacteristic, NimBLEConnInfo &connInfo) override
+    {
+        Serial.printf("%s : onWrite(), value: %s\n",
+                      pCharacteristic->getUUID().toString().c_str(),
+                      pCharacteristic->getValue().c_str());
+    }
+
+    void onStatus(NimBLECharacteristic *pCharacteristic, int code) override
+    {
+#ifdef COMPASS_DEBUG
+        Serial.printf("Notification/Indication return code: %d, %s\n", code,
+                      NimBLEUtils::returnCodeToString(code));
+#endif
+    }
+
+    void onSubscribe(NimBLECharacteristic *pCharacteristic, NimBLEConnInfo &connInfo,
+                     uint16_t subValue) override
+    {
+        std::string str = "Client ID: ";
+        str += connInfo.getConnHandle();
+        str += " Address: ";
+        str += connInfo.getAddress().toString();
+        if (subValue == 0)
+        {
+            str += " Unsubscribed to ";
+        }
+        else if (subValue == 1)
+        {
+            str += " Subscribed to notifications for ";
+        }
+        else if (subValue == 2)
+        {
+            str += " Subscribed to indications for ";
+        }
+        else if (subValue == 3)
+        {
+            str += " Subscribed to notifications and indications for ";
+        }
+        str += std::string(pCharacteristic->getUUID());
+
+        Serial.printf("%s\n", str.c_str());
+    }
+} chrCallbacks;
+
+class DescriptorCallbacks : public NimBLEDescriptorCallbacks
+{
+    void onWrite(NimBLEDescriptor *pDescriptor, NimBLEConnInfo &connInfo) override
+    {
+        std::string dscVal = pDescriptor->getValue();
+        Serial.printf("Descriptor written value: %s\n", dscVal.c_str());
+    }
+
+    void onRead(NimBLEDescriptor *pDescriptor, NimBLEConnInfo &connInfo) override
+    {
+        Serial.printf("%s Descriptor read\n", pDescriptor->getUUID().toString().c_str());
+    }
+} dscCallbacks;
+
+#endif
+
+void initBT()
+{
+#ifdef BT_NM
+    // Initialize BLE device
+    NimBLEDevice::init("RSSI_Radar");
+
+    // Get and print the MAC address
+    String macAddress = NimBLEDevice::getAddress().toString().c_str();
+    Serial.println("Bluetooth MAC Address: " + macAddress);
+
+    // Create BLE server
+    pServer = NimBLEDevice::createServer();
+
+    if (!pServer)
+    {
+        Serial.println("Failed to create BLE Server");
+    }
+
+    pServer->setCallbacks(&BTServerCallbacks);
+
+    // Create a BLE service
+    NimBLEService *pService = pServer->createService(SERVICE_UUID);
+
+    // Create a BLE characteristic
+    pCharacteristic = pService->createCharacteristic(
+        CHARACTERISTIC_UUID, NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY);
+    pCharacteristic->setCallbacks(&chrCallbacks);
+
+    // Start the service
+    pService->start();
+
+    // esp_task_wdt_init(20, true); // Increase timeout to 10 seconds
+    // esp_task_wdt_add(NULL);
+
+    // Start advertising
+    pAdvertising = NimBLEDevice::getAdvertising();
+    pAdvertising->addServiceUUID(SERVICE_UUID);
+    pAdvertising->setName("ESP32_RSSI_Radar"); // Set the device name
+    // pAdvertising->setMinInterval(300);
+    // pAdvertising->setMaxInterval(350);
+    pAdvertising->enableScanResponse(true);
+    // pAdvertising->setScanResponse(true); // Allow scan responses
+
+    pAdvertising->start();
+
+    Serial.println("BLE server started.");
+#else
+    BLEDevice::init("ESP32_RADAR");
+    pServer = BLEDevice::createServer();
+    pServer->setCallbacks(new MyServerCallbacks());
+
+    BLEService *pService = pServer->createService(SERVICE_UUID);
+
+    pCharacteristic = pService->createCharacteristic(
+        CHARACTERISTIC_UUID, BLECharacteristic::PROPERTY_READ |
+                                 BLECharacteristic::PROPERTY_WRITE |
+                                 BLECharacteristic::PROPERTY_NOTIFY);
+
+    pCharacteristic->setValue("Hello from ESP32");
+    pService->start();
+
+    pAdvertising = BLEDevice::getAdvertising();
+    pAdvertising->addServiceUUID(SERVICE_UUID);
+    pAdvertising->setScanResponse(true);
+    pAdvertising->setMinInterval(300);
+    pAdvertising->setMaxInterval(350);
+    BLEDevice::startAdvertising();
+
+    Serial.println("BLE server is ready!");
+#endif
+}
+
+// Function to send RSSI and Heading Data
+void sendBTData(float heading, float rssi)
+{
+    String data =
+        "RSSI_HEADING: '{H:" + String(heading) + ",RSSI:-" + String(rssi) + "}'";
+
+#ifdef COMPASS_DEBUG
+    Serial.println("Sending data: " + data);
+#endif
+    pCharacteristic->setValue(data.c_str()); // Set BLE characteristic value
+    pCharacteristic->notify();               // Notify connected client
+}
+#endif
+
 #ifndef LILYGO
 #include <heltec_unofficial.h>
 // This file contains a binary patch for the SX1262
@@ -258,7 +417,7 @@ void displaySensorDetails(void)
    Serial.println(" uT");
    Serial.println("------------------------------------");
    Serial.println("");
-    delay(500);
+    heltec_delay(500);
 }

 // Variables for dynamic calibration
@@ -802,7 +961,7 @@ int16_t initForScan(float freq)

    // LR1121 TCXO Voltage 2.85~3.15V
    radio.setTCXO(3.0);
-    delay(1000);
+    heltec_delay(1000);
 #else
    state = radio.beginFSK(freq);
 #endif
@@ -824,7 +983,7 @@ A:
        Serial.print(F("Failed to start receive mode, error code: "));
        display.drawString(0, 64 - 10, "E:startReceive");
        display.display();
-        delay(2000);
+        heltec_delay(2000);
        Serial.println(state);
        gotoAcounter++;
        if (gotoAcounter < 5)
@@ -876,7 +1035,7 @@ bool setFrequency(float curr_freq)
        Serial.println("E(" + String(state) +
                       "):setFrequency:" + String(r.current_frequency));
        display.display();
-        delay(2);
+        // delay(2);
        return false;
    }

@@ -949,6 +1108,7 @@ void init_radio()
 #ifdef USING_SX1262
    if (config.radio2.enabled && config.radio2.module.equalsIgnoreCase("SX1262"))
    {
+
        radio2 = new SX1262Module(config.radio2);
        state = radio2->beginScan(CONF_FREQ_BEGIN, BANDWIDTH, RADIOLIB_SHAPING_NONE);
        if (state == RADIOLIB_ERR_NONE)
@@ -1165,7 +1325,6 @@ void draw360Scale(int start = 0, int end = 360, int width = 128, int height = 64
    for (int x = 0; x <= width; x += stepPixel)
    {
        // int x = map(i, start, end, 0, scaleLength);
-        Serial.println("Tick: " + String(x));
        if (x == 128)
        {
            x = x - 1;
@@ -2223,15 +2382,21 @@ int max_rssi_x = 999;
 void doScan();

 void reportScan();
+long calStart = 0;

 #ifdef COMPASS_ENABLED
 float getCompassHeading()
 {
-    /* code */
+    if (calStart == 0)
+    {
+        calStart = millis();
+    }

    /* Get a new sensor event */
    sensors_event_t event2;
    mag.getEvent(&event2);
+    sensors_event_t event3;
+    mag.getEvent(&event3);

 #ifdef COMPASS_DEBUG
    /* Display the results (magnetic vector values are in micro-Tesla (uT)) */
@@ -2263,20 +2428,26 @@ float getCompassHeading()
    // Dynamicly Calibrated out

    // Read raw magnetometer data
-    float x = event2.magnetic.x;
-    float y = event2.magnetic.y;
-    float z = event2.magnetic.z;
+    float x = (event2.magnetic.x + event3.magnetic.x) / 2;
+    float y = (event2.magnetic.y + event3.magnetic.y) / 2;
+    float z = (event2.magnetic.z + event3.magnetic.z) / 2;

-    // Update min/max values dynamically
-    x_min = min(x_min, x);
-    x_max = max(x_max, x);
-    y_min = min(y_min, y);
-    y_max = max(y_max, y);
-    z_min = min(z_min, z);
-    z_max = max(z_max, z);
+    // Doing calibration first 1 minute
+    if (millis() - calStart < 60000)
+    {
+        // Update min/max values dynamically
+        x_min = min(x_min, x);
+        x_max = max(x_max, x);
+        y_min = min(y_min, y);
+        y_max = max(y_max, y);
+        z_min = min(z_min, z);
+        z_max = max(z_max, z);
+    }

+#ifdef COMPASS_DEBUG
    Serial.println("x_min:" + String(x_min) + " x_max: " + String(x_max) +
                   " y_min: " + String(y_min));
+#endif

    // Calculate offsets and scales in real-time
    float x_offset = (x_max + x_min) / 2;
@@ -2350,9 +2521,54 @@ void loop(void)
        doScan();
        reportScan();
    }
+
+#ifdef BT_MOBILE
+#ifdef BT_RSSI
+    while (true)
+    {
+        float startFreq = FREQ_BEGIN - 0.5; // Start 2 MHz left
+        float endFreq = FREQ_END + 0.5;     // End 2 MHz right
+        float step = 0.5;                   // Step size in MHz
+        float rssi = -122;
+        float rssiMax = -999;
+
+        for (float freq = startFreq; freq <= endFreq; freq += step)
+        {
+            setFrequency(freq);
+            // Serial.println("COMPASS FREQ SET: " + String(freq));
+
+            // heltec_delay(5);
+#ifdef USING_LR1121
+            radio.getRssiInst(&rssi);
+#else
+            rssi = getRssi(false);
+#endif
+            if (rssi > rssiMax)
+            {
+                rssiMax = rssi;
+            }
+            Serial.println("RSSI: " + String(freq) + ":" + String(rssiMax));
+            // String p = _scan_result_str(m.payload.dump, 10);
+
+            if (pServer && pServer->getConnectedCount())
+            {
+                String str = "RSSI:TEST";
+                if (pCharacteristic)
+                {
+                    pCharacteristic->setValue(
+                        str.c_str());          // Set BLE characteristic value
+                    pCharacteristic->notify(); // Notify connected client
+                }
+                delay(50);
+            }
+        }
+    }
+#endif
+#endif
+
 #ifdef COMPASS_ENABLED
 #if defined(COMPASS_FREQ)
-    delay(1000);
+    // delay(1000);
    display.clear();
 #endif // COMPAS_FREQ
    // Redraw Chart scale line
@@ -2383,22 +2599,22 @@ void loop(void)
            draw360Scale(0, 360, 128, 64);
        }

-        float rssi = -122;
-        float rssiMax = -999;
        if (headingDegrees >= 0 && headingDegrees <= 360)
        {
-            float startFreq = COMPASS_FREQ - 1.0; // Start 2 MHz left
-            float endFreq = COMPASS_FREQ + 1.0;   // End 2 MHz right
+            float startFreq = COMPASS_FREQ - 0.5; // Start 2 MHz left
+            float endFreq = COMPASS_FREQ + 0.5;   // End 2 MHz right
            float step = 0.5;                     // Step size in MHz
 #ifdef COMPASS_RSSI
+            draw360Scale(0, 360, 128, 64);
+            float rssi = -122;
+            float rssiMax = -999;
            for (int i = 0; i < SAMPLES_RSSI; i++)
            {
-
                for (float freq = startFreq; freq <= endFreq; freq += step)
                {
                    setFrequency(freq);
                    // Serial.println("COMPASS FREQ SET: " + String(freq));
-                    draw360Scale(0, 360, 128, 64);
+
                    // heltec_delay(5);
 #ifdef USING_LR1121
                    radio.getRssiInst(&rssi);
@@ -2830,6 +3046,7 @@ void doScan()
                    max_x_rssi[display_x] = max_rssi;
                }
            }
+
 #endif // SCAN_METHOD == METHOD_RSSI

            // if this code is not executed LORA radio doesn't work
@@ -3183,6 +3400,7 @@ void loraSendMessage(Message &msg)

 void reportScan()
 {
+
    if (!config.lora_enabled)
        return;

--- a/web_app/compass/index.html
+++ b/web_app/compass/index.html
@@ -171,11 +171,16 @@
        ctx.stroke();
      }

+      let lineCoef = 2.5;
      // Draw data points
      dataPoints.forEach(({ angle, rssi }) => {
        const rad = (angle * Math.PI) / 180;
-        const length = ((120 + rssi) / (radius / 2)) * radius; // Scale RSSI to fit within radar
-
+        //const length = (120 + rssi) / (radius / 2) * radius;
+        const length = ((120 - 90) + (rssi + 90)) * lineCoef; // Scale RSSI to fit within radar
+        if (length > radius) {
+          length = radius;
+        }
+        console.log("Length: " + length);
        const x = centerX + length * Math.cos(rad);
        const y = centerY + length * Math.sin(rad);

@@ -224,7 +229,7 @@

      // Draw current RSSI line in yellow
      const currentRad = (currentPoint.angle * Math.PI) / 180;
-      const currentLength = ((120 + currentPoint.rssi) / (radius / 2)) * radius;
+      const currentLength = ((120 + currentPoint.rssi) / (radius / 2)) * radius * 1.2;

      const currentX = centerX + currentLength * Math.cos(currentRad);
      const currentY = centerY + currentLength * Math.sin(currentRad);
@@ -420,7 +425,7 @@
        console.log(`Reconnecting to device: ${bluetoothDevice.name}`);
        const server = await bluetoothDevice.gatt.connect();
        console.log(`Reconnected to device: ${bluetoothDevice.name}`);
-        deviceNameElem.textContent = bluetoothDevice.name;
+        //deviceNameElem.textContent = bluetoothDevice.name;
        statusElem.textContent = "Reconnected";
      } catch (error) {
        console.error("Error reconnecting to Bluetooth device:", error);
--- a/web_app/spectr/index.html
+++ b/web_app/spectr/index.html
@@ -0,0 +1,303 @@
+<!DOCTYPE html>
+<html lang="en">
+
+<head>
+    <meta charset="UTF-8">
+    <meta name="viewport" content="width=device-width, initial-scale=1.0">
+    <title>Spectrum Analyzer</title>
+    <style>
+        body {
+            font-family: Arial, sans-serif;
+            text-align: center;
+            background-color: black;
+            color: white;
+        }
+
+        .container {
+            display: flex;
+            flex-direction: column;
+            align-items: center;
+            width: 80%;
+            margin: auto;
+        }
+
+        .chart-container {
+            width: 100%;
+            display: flex;
+            flex-direction: column;
+            position: relative;
+        }
+
+        .chart {
+            height: 40vh;
+            border: 1px solid white;
+            position: relative;
+        }
+
+        .waterfall {
+            height: 40vh;
+            border: 1px solid white;
+        }
+
+        .console {
+            height: 20vh;
+            width: 100%;
+            background-color: black;
+            color: white;
+            overflow-y: auto;
+            padding: 10px;
+            text-align: left;
+            border: 1px solid white;
+        }
+
+        .labels {
+            position: absolute;
+            left: -50px;
+            top: 0;
+            height: 100%;
+            display: flex;
+            flex-direction: column;
+            justify-content: space-between;
+        }
+
+        .freq-labels {
+            display: flex;
+            justify-content: space-between;
+            position: absolute;
+            bottom: -20px;
+            width: 100%;
+        }
+
+        canvas {
+            width: 100%;
+            height: 100%;
+            display: block;
+            image-rendering: crisp-edges;
+        }
+
+        button {
+            margin: 10px;
+            padding: 10px;
+            font-size: 16px;
+            background-color: white;
+            color: black;
+            border: none;
+            cursor: pointer;
+        }
+
+        button:hover {
+            background-color: gray;
+        }
+    </style>
+</head>
+
+<body>
+    <h1>Spectrum Analyzer</h1>
+    <button id="simulationButton" onclick="toggleSimulation()">Start Simulation</button>
+    <button onclick="connectBluetooth()">Connect Bluetooth</button>
+    <div class="container">
+        <div class="chart-container">
+            <div class="labels" id="dbLabels"></div>
+            <canvas id="chartCanvas" class="chart"></canvas>
+            <div class="freq-labels" id="freqLabels"></div>
+        </div>
+        <canvas id="waterfallCanvas" class="waterfall"></canvas>
+        <div class="console" id="consoleOutput"></div>
+    </div>
+    <script>
+        const chartCanvas = document.getElementById('chartCanvas');
+        const chartCtx = chartCanvas.getContext('2d');
+        const waterfallCanvas = document.getElementById('waterfallCanvas');
+        const waterfallCtx = waterfallCanvas.getContext('2d');
+        const dbLabels = document.getElementById('dbLabels');
+        const freqLabels = document.getElementById('freqLabels');
+        const simulationButton = document.getElementById('simulationButton');
+
+        const scaleFactor = 1; //window.devicePixelRatio || 2;
+        chartCanvas.width = chartCanvas.clientWidth * scaleFactor;
+        chartCanvas.height = chartCanvas.clientHeight * scaleFactor;
+        chartCtx.scale(scaleFactor, scaleFactor);
+
+        waterfallCanvas.width = waterfallCanvas.clientWidth * scaleFactor;
+        waterfallCanvas.height = waterfallCanvas.clientHeight * scaleFactor;
+        waterfallCtx.scale(scaleFactor, scaleFactor);
+
+        let RSSI = {};
+        let spectrumData = Array.from({ length: 80 }, () => new Array(900).fill(0));
+        let waterfallHistory = Array.from({ length: 80 }, () => new Array(900).fill("black"));
+        let simulationInterval;
+        let isSimulationRunning = false;
+        let bluetoothDevice;
+        let bluetoothCharacteristic;
+
+        async function connectBluetooth() {
+            try {
+                const bluetoothDevice = await navigator.bluetooth.requestDevice({
+                    acceptAllDevices: true,
+                    optionalServices: ['00001234-0000-1000-8000-00805f9b34fb']
+                });
+
+                const server = await bluetoothDevice.gatt.connect();
+                const service = await server.getPrimaryService('00001234-0000-1000-8000-00805f9b34fb');
+                const bluetoothCharacteristic = await service.getCharacteristic('00001234-0000-1000-8000-00805f9b34ac');
+
+                // Save the device's ID to localStorage
+                localStorage.setItem("bluetoothDeviceId", bluetoothDevice.id);
+
+                bluetoothCharacteristic.addEventListener('characteristicvaluechanged', handleBluetoothData)
+
+                await bluetoothCharacteristic.startNotifications();
+
+                //statusElem.textContent = "Connected";
+            } catch (error) {
+                console.error("Error scanning for devices:", error);
+                alert("Could not connect to device.");
+                // statusElem.textContent = "Connection Error";
+            }
+        }
+
+        function handleBluetoothData(event) {
+            const decoder = new TextDecoder("utf-8");
+            const value = decoder.decode(event.target.value);
+            console.log("Received data:", value); // For debugging
+            console.log(value);
+            parseBluetoothData(value);
+        }
+
+        function parseBluetoothData(data) {
+            let matches = data.match(/\((\d+),\s*(-?\d+)\)/g);
+            if (!matches) return;
+
+            RSSI = {};
+            matches.forEach(match => {
+                let [_, freq, rssi] = match.match(/\((\d+),\s*(-?\d+)\)/);
+                RSSI[freq] = { freq: parseInt(freq), rssi: parseInt(rssi) };
+            });
+
+            spectrumData.pop();
+            spectrumData.unshift(Object.values(RSSI).map(d => d.rssi));
+            updateConsole();
+            drawChart();
+            drawWaterfall();
+        }
+
+        function generateRSSI() {
+            for (let freq = 100; freq < 1000; freq++) {
+                RSSI[freq] = { freq, rssi: Math.random() * -120 - 30 };
+            }
+            spectrumData.pop();
+            spectrumData.unshift(Object.values(RSSI).map(d => d.rssi));
+            updateConsole();
+        }
+
+        function getColor(rssi) {
+            if (rssi > -40) return "red";
+            if (rssi > -50) return "magenta";
+            if (rssi > -60) return "yellow";
+            if (rssi > -70) return "green";
+            return "blue";
+        }
+
+        function drawChart() {
+            chartCtx.clearRect(0, 0, chartCanvas.width, chartCanvas.height);
+            let keys = Object.keys(RSSI).map(Number);
+            console.log("chartCanvas.height: " + chartCanvas.height);
+            keys.forEach((freq, index) => {
+                let rssi = RSSI[freq].rssi;
+                let color = getColor(rssi);
+                chartCtx.fillStyle = color;
+                const x = ((freq - 100) / 900) * chartCanvas.width;
+                let minRssi = -90;
+                let maxRssi = -30;
+                let rssiDiapason = Math.abs(minRssi) - Math.abs(maxRssi);
+                let yCoef = (chartCanvas.height / rssiDiapason);
+
+                let y = 0;
+                if (rssi < minRssi) {
+                    y = chartCanvas.height;
+                } else {
+                    y = (rssiDiapason - (rssiDiapason - Math.abs(Math.abs(rssi) - Math.abs(maxRssi)))) * yCoef;
+                    // y = chartCanvas.height * 0.1;
+                }
+                console.log(freq + ":" + rssi + " Y height: " + y);
+                console.log("Y height: " + chartCanvas.height + "Coef: " + yCoef);
+                /* x - The x-axis coordinate of the rectangle's starting point.
+                y - The y-axis coordinate of the rectangle's starting point.
+                width - The rectangle's width. Positive values are to the right, and negative to the left.
+                height - The rectangle's height. Positive values are down, and negative are up.
+                */
+
+                chartCtx.fillRect(x, chartCanvas.height, chartCanvas.width / keys.length, -chartCanvas.height + y);
+                waterfallHistory[0][index] = color;
+            });
+        }
+
+        function drawWaterfall() {
+            waterfallCtx.clearRect(0, 0, waterfallCanvas.width, waterfallCanvas.height);
+            waterfallHistory.pop();
+            waterfallHistory.unshift([...waterfallHistory[0]]);
+            for (let y = 0; y < waterfallHistory.length; y++) {
+                for (let x = 0; x < waterfallHistory[y].length; x++) {
+                    waterfallCtx.fillStyle = waterfallHistory[y][x];
+                    waterfallCtx.fillRect(
+                        (x / waterfallHistory[y].length) * waterfallCanvas.width,
+                        (y / waterfallHistory.length) * waterfallCanvas.height,
+                        waterfallCanvas.width / waterfallHistory[y].length,
+                        waterfallCanvas.height / waterfallHistory.length
+                    );
+                }
+            }
+        }
+
+        function updateScales() {
+            dbLabels.innerHTML = "";
+            for (let db = 30; db <= 80; db += 10) {
+                let div = document.createElement("div");
+                div.textContent = "-" + db + " dB";
+                div.style.flexGrow = "1";
+                dbLabels.appendChild(div);
+            }
+
+            freqLabels.innerHTML = "";
+            for (let freq = 100; freq <= 1000; freq += 100) {
+                let div = document.createElement("div");
+                div.textContent = freq + " MHz";
+                freqLabels.appendChild(div);
+            }
+        }
+
+        function updateConsole() {
+            const consoleOutput = document.getElementById('consoleOutput');
+            consoleOutput.innerHTML = Object.values(RSSI)
+                .map(entry => `Freq: ${entry.freq} MHz | RSSI: ${entry.rssi.toFixed(2)} dB`)
+                .join('<br>');
+        }
+
+        function startSimulation() {
+            updateScales();
+            simulationInterval = setInterval(() => {
+                generateRSSI();
+                drawChart();
+                drawWaterfall();
+            }, 100);
+        }
+
+        function stopSimulation() {
+            clearInterval(simulationInterval);
+            simulationInterval = null;
+        }
+
+        function toggleSimulation() {
+            if (isSimulationRunning) {
+                stopSimulation();
+                simulationButton.textContent = "Start Simulation";
+            } else {
+                startSimulation();
+                simulationButton.textContent = "Stop Simulation";
+            }
+            isSimulationRunning = !isSimulationRunning;
+        }
+    </script>
+</body>
+
+</html>