diff --git a/.vscode/settings.json b/.vscode/settings.json
index 1ca7fe0..34e25be 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -40,6 +40,8 @@
         "math.h": "c",
         "esp_check.h": "c",
         "radio.h": "c",
-        "sx1262.h": "c"
+        "sx1262.h": "c",
+        "gps.h": "c",
+        "servocontroller.h": "c"
     }
 }
diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt
index d8774d5..23f3ede 100644
--- a/main/CMakeLists.txt
+++ b/main/CMakeLists.txt
@@ -21,6 +21,8 @@ idf_component_register(SRCS
 "hw/sx1262.h"
 "components/sensors.c"
 "components/sensors.h"
+"components/servocontroller.c"
+"components/servocontroller.h"
 "components/radio.c"
 "components/radio.h"
     "main.c"
diff --git a/main/components/packets.h b/main/components/packets.h
index e69de29..7448f63 100644
--- a/main/components/packets.h
+++ b/main/components/packets.h
@@ -0,0 +1,88 @@
+#ifndef PACKETS_STRUCTS
+#define PACKETS_STRUCTS
+#include "stdint.h"
+
+typedef struct __attribute__((packed))
+{
+    uint32_t packetIndex;
+    uint8_t packetType;
+    uint32_t missionTimer;
+} DownBoundPacket;
+
+typedef struct __attribute__((packed))
+{
+    // MPU data
+    int16_t accelerationX;
+    int16_t accelerationY;
+    int16_t accelerationZ;
+    int16_t gyroX;
+    int16_t gyroY;
+    int16_t gyroZ;
+    int16_t magnetX;
+    int16_t magnetY;
+    int16_t magnetZ;
+    int16_t accelerometer_temperature;
+
+    // CCS data
+    uint16_t eCO2;
+    uint16_t tvoc;
+    uint8_t currentCCS;
+    uint16_t rawCCSData;
+
+    // INA data
+    uint16_t volts;
+    uint16_t current;
+    uint16_t power;
+
+    // BME DATA
+    uint32_t temperature;
+    uint16_t humidity;
+    uint32_t pressure;
+    uint16_t gas;
+    bool gas_valid;
+    bool heater_stable;
+    uint8_t gas_range;
+    uint8_t gas_index;
+
+    // GPS DATA
+    uint32_t time_seconds;           // Seconds since start of day
+    int32_t latitude_centi_degrees;  // Latitude * 10,000
+    int32_t longitude_centi_degrees; // Longitude * 10,000
+    int16_t altitude_centi_meters; // Altitude * 100
+    uint8_t fix_quality;
+    uint8_t num_satellites;
+    uint16_t date_yyddmm;          // YYDDMM (from GPRMC)
+    uint16_t speed_centi_knots;    // Speed * 100 (from GPRMC)
+
+    int32_t predicted_latitude_centi_degrees;  // Latitude * 10,000
+    int32_t predicted_longitude_centi_degrees; // Longitude * 10,000
+    int16_t predicted_altitude_centi_meters; // Altitude * 100
+
+    // ADC DATA
+    int32_t NH3;
+    int32_t CO;
+    int32_t NO2;
+    int32_t UVC;
+
+    int16_t currentServoA;
+    int16_t targetServoA;
+    int16_t currentServoB;
+    int16_t targetServoB;
+
+} TelemetryPacket;
+
+typedef struct __attribute__((packed))
+{
+    uint32_t packetIndex;
+    uint8_t packetType;
+} UplinkPacket;
+
+typedef struct __attribute__((packed))
+{
+    uint8_t powerMode;
+    uint8_t controlMode;
+    uint16_t servoA;
+    uint16_t servoB;
+} SystemControlPacket;
+
+#endif
\ No newline at end of file
diff --git a/main/components/radio.c b/main/components/radio.c
index 303907b..2d8b613 100644
--- a/main/components/radio.c
+++ b/main/components/radio.c
@@ -2,206 +2,107 @@
 #include "freertos/task.h"
 #include "../hw/sx1262.h"
 #include "radio.h"
+#include "esp_log.h"
+#include "string.h"
+#include "esp_timer.h"
 
 const char *msg = "Testing 123 test why is this not on air";
 
+#define TAG "LoRa"
+
+uint32_t packetIndex = 0;
+
+TelemetryPacket telemetryPacket;
+uint8_t packetReadiness = 0;
+
 void lora_comms_task(void *pvParameters)
 {
-    sx1262_init();
-    sx1262_resetStats();
-    ESP_LOGI(TAG_RADIO, "Setting standby RC");
-    sx1262_setStandby(SX1262_STANDBY_RC);
 
-    ESP_LOGI(TAG_RADIO, "Getting version");
-    unsigned char versDat[16];
-    sx1262_readRegister(0x0320, versDat, 16);
-    ESP_LOGI(TAG_RADIO, "Version: %s", versDat);
+    const int64_t interval_us = 100000; // 100 ms
+    int64_t start_time, end_time, elapsed;
+    LoRaInit();
+    int8_t txPowerInDbm = 20;
 
-    sx1262_get_status();
+    uint32_t frequencyInHz = 0;
+    frequencyInHz = 869525000;
+    ESP_LOGW(TAG, "Enable TCXO");
+    float tcxoVoltage = 2.2;     // use TCXO
+    bool useRegulatorLDO = true; // use DCDC + LDO
 
-    // Read status
-    sx1262_get_status();
+    // LoRaDebugPrint(true);
+    if (LoRaBegin(frequencyInHz, txPowerInDbm, tcxoVoltage, useRegulatorLDO) != 0)
+    {
+        ESP_LOGE(TAG, "Does not recognize the module");
+        while (1)
+        {
+            vTaskDelay(1);
+        }
+    }
 
-    ESP_LOGI(TAG_RADIO, "Setting TCXO");
-    sx1262_setDIO3AsTCXOCtrl(SX1262_TCXO_VOLTAGE22dV, 320);
+    uint8_t spreadingFactor = 7;
+    uint8_t bandwidth = SX126X_LORA_BW_250_0;
+    uint8_t codingRate = SX126X_LORA_CR_4_8;
+    uint16_t preambleLength = 8;
+    uint8_t payloadLen = 0;
+    bool crcOn = true;
+    bool invertIrq = false;
+    LoRaConfig(spreadingFactor, bandwidth, codingRate, preambleLength, payloadLen, crcOn, invertIrq);
+    uint8_t bufIn[256];  // Maximum Payload size of SX1261/62/68 is 255
+    uint8_t bufOut[256]; // Maximum Payload size of SX1261/62/68 is 255
 
-    sx1262_get_status();
+    DownBoundPacket downboundPacket;
+    UplinkPacket uplinkPacket;
+    SystemControlPacket systemControlPacket;
 
-    ESP_LOGI(TAG_RADIO, "Setting Buffer base address");
-    sx1262_setBufferBaseAddress(0, 0);
-    sx1262_get_status();
-
-    ESP_LOGI(TAG_RADIO, "Setting packet Type");
-    sx1262_setPacketType(SX1262_PACKET_TYPE_LORA);
-    sx1262_get_status();
-
-    ESP_LOGI(TAG_RADIO, "Setting RXTX fallback mode");
-    sx1262_setRxTXFallbackMode(SX1262_FALLBACK_RC);
-    sx1262_get_status();
-
-    ESP_LOGI(TAG_RADIO, "Setting CAD params");
-    sx1262_setCadParams(0x03, 0x16, 0x0A, 0, 0);
-    sx1262_get_status();
-
-    ESP_LOGI(TAG_RADIO, "Clearing IRQ status");
-    sx1262_clearIrqStatus(0x43FF);
-    sx1262_get_status();
-
-    ESP_LOGI(TAG_RADIO, "Setting Image rejection");
-    sx1262_calibrateImage(0xD7, 0xDB);
-
-    sx1262_get_status();
-
-    ESP_LOGI(TAG_RADIO, "Calibrating Image rejection");
-    sx1262_calibrate(SX1262_CALIBRATION_ALL);
-    sx1262_get_status();
-
-    ESP_LOGI(TAG_RADIO, "Setting regulator");
-    sx1262_setRegulatorMode(SX1262_REGULATOR_DC_DC_LDO);
-    sx1262_get_status();
-
-    uint8_t modType;
-    modType = sx1262_getPacketType();
-    sx1262_get_status();
-    ESP_LOGI(TAG_RADIO, "ModType %d", modType);
-
-    sx1262_LoRaModulationParams_t loraModParams;
-
-    loraModParams.bandwidth = SX1262_LORA_BW_125;
-    loraModParams.codingRate = SX1262_LORA_CR_4_5;
-    loraModParams.lowDataRateOpt = 1;
-    loraModParams.spreadingFactor = SX1262_LORA_SF7;
-
-    ESP_LOGI(TAG_RADIO, "Setting modulation params");
-    sx1262_setLoRaModulationParams(&loraModParams);
-
-    sx1262_get_status();
-
-    ESP_LOGI(TAG_RADIO, "Setting Sync word");
-    uint8_t syncWord[2] = {0x14, 0x24};
-
-    sx1262_writeRegister(SX1262_LORA_SYNC_WORD_MSB, syncWord, 2);
-    sx1262_get_status();
-
-    uint8_t ocpBuf[1] = {SX126X_OCP_LEVEL_SX1262};
-
-    ESP_LOGI(TAG_RADIO, "Setting overcurrent protection");
-    sx1262_writeRegister(SX126X_OCP_CONFIGURATION, ocpBuf, 1);
-
-    sx1262_get_status();
-
-    ESP_LOGI(TAG_RADIO, "Setting RF Switch out");
-    sx1262_setDIO2AsRfSwitchCtrl(1);
-
-    sx1262_get_status();
-
-    sx1262_LoRaPacketParams_t loraPacketParams;
-    loraPacketParams.preambleLength = 10;
-    loraPacketParams.crcType = SX1262_CRC_ON;
-    loraPacketParams.headerType = SX1262_HEADER_TYPE_VARIABLE;
-    loraPacketParams.invertIQ = SX1262_STANDARD_IQ;
-    loraPacketParams.payloadLength = sizeof(msg);
-
-    ESP_LOGI(TAG_RADIO, "Setting packet params");
-    sx1262_setLoRaPacketParams(&loraPacketParams);
-
-    sx1262_get_status();
-
-    ESP_LOGI(TAG_RADIO, "Setting frequency");
-    sx1262_setFrequency(869525000);
-
-    uint8_t clampConfig[1] = {0xDE};
-
-    ESP_LOGI(TAG_RADIO, "Setting TX clamp");
-    sx1262_writeRegister(SX126X_TX_CLAMP_CONFIG, clampConfig, 1);
-    sx1262_get_status();
-
-    ESP_LOGI(TAG_RADIO, "Setting TX power");
-    sx1262_configure_tx_power(0x04, 0x07, 0x01, LORA_TX_POWER, SX1262_Ramp_200U);
-
-    sx1262_get_status();
-
-    sx1262_get_status();
-
-    ESP_LOGI(TAG_RADIO, "Setting standby XOSC");
-    sx1262_setStandby(SX1262_STANDBY_XOSC);
-
-    sx1262_get_status();
-
-    sx1262_setDioIrqParams(SX1262_IRQ_ALL, SX1262_IRQ_TXDone, 0, 0);
-
-    sx1262_get_status();
-
-    ESP_LOGI(TAG_RADIO, "Setting buffer base address");
-    sx1262_setBufferBaseAddress(0, 0);
-
-    sx1262_get_status();
-
-    ESP_LOGI(TAG_RADIO, "Writing message");
-    sx1262_writeBuffer(0, (uint8_t *)msg, sizeof(msg));
-
-    ESP_LOGI(TAG_RADIO, "Clearing IRQs");
-    sx1262_clearIrqStatus(SX1262_IRQ_ALL);
-
-    // sx1262_setTxContinuousWave();
-    sx1262_get_status();
-
-    ESP_LOGI(TAG_RADIO, "Getting modulation type...");
-    modType = sx1262_getPacketType();
-    ESP_LOGI(TAG_RADIO, "ModType %d", modType);
-    sx1262_get_status();
-
-    // ESP_LOGI(TAG_RADIO, "Enabling CW...");
-    // sx1262_setTxContinuousWave();
-    // sx1262_get_status();
-
-    uint8_t txModulation[1] = {0x04};
-
-    ESP_LOGI(TAG_RADIO, "Setting modulation thing");
-    sx1262_writeRegister(SX126X_TX_MODULATION, txModulation, 1);
-    sx1262_get_status();
-
-    ESP_LOGI(TAG_RADIO, "Clearing IRQs");
-    sx1262_clearIrqStatus(SX1262_IRQ_ALL);
-    sx1262_get_status();
-
-    ESP_LOGI(TAG_RADIO, "Enabling TX...");
-    sx1262_setMode(SX1262_TRANSMIT_MODE, 0);
-    sx1262_get_status();
-
-    ESP_LOGI(TAG_RADIO, "Waiting for TX finish");
     while (1)
     {
-        if (gpio_get_level(LORA_DIO1))
+        start_time = esp_timer_get_time(); // µs since boot
+        if (packetReadiness == 1)
         {
-            ESP_LOGI(TAG_RADIO, "Got IRQin");
-            uint16_t irqS = sx1262_getIrqStatus();
-            if (irqS & SX1262_IRQ_TXDone)
+            uint8_t downPacketSize = 0;
+            memset(bufOut, 0, sizeof(bufOut));
+            downboundPacket.missionTimer = start_time;
+            downboundPacket.packetIndex = packetIndex++;
+            downboundPacket.packetType = 1;
+
+            memcpy(bufOut, &downboundPacket, sizeof(downboundPacket));
+            downPacketSize += sizeof(downboundPacket);
+            memcpy(((uint8_t *)bufOut) + downPacketSize, &telemetryPacket, sizeof(telemetryPacket));
+            downPacketSize += sizeof(telemetryPacket);
+
+            ESP_LOGI(pcTaskGetName(NULL), "%d byte packet sent...", downPacketSize);
+
+            // Wait for transmission to complete
+            if (LoRaSend(bufOut, downPacketSize, SX126x_TXMODE_SYNC) == false)
             {
-                break;
+                ESP_LOGE(pcTaskGetName(NULL), "LoRaSend fail");
+            } else {
+                packetReadiness = 0;
             }
         }
-        vTaskDelay(pdMS_TO_TICKS(10));
+
+        uint8_t rxLen = LoRaReceive(bufIn, sizeof(bufIn));
+        if (rxLen > 0)
+        {
+            ESP_LOGI(pcTaskGetName(NULL), "%d byte packet received:[%.*s]", rxLen, rxLen, bufIn);
+
+            int8_t rssi, snr;
+            GetPacketStatus(&rssi, &snr);
+            ESP_LOGI(pcTaskGetName(NULL), "rssi=%d[dBm] snr=%d[dB]", rssi, snr);
+        }
+
+        int lost = GetPacketLost();
+        if (lost != 0)
+        {
+            ESP_LOGW(pcTaskGetName(NULL), "%d packets lost", lost);
+        }
+
+        end_time = esp_timer_get_time();
+        elapsed = end_time - start_time;
+
+        if (elapsed < interval_us)
+        {
+            vTaskDelay(pdMS_TO_TICKS((interval_us - elapsed) / 1000));
+        }
     }
-
-    // vTaskDelay(pdMS_TO_TICKS(2000));
-    // ESP_LOGI(TAG_RADIO, "Returning back to sleep");
-    // sx1262_setStandby(SX1262_STANDBY_XOSC);
-
-    // sx1262_get_status();
-
-    ESP_LOGI(TAG_RADIO, "Clearing IRQs");
-    sx1262_clearIrqStatus(SX1262_IRQ_TXDone);
-
-    sx1262_get_status();
-    while (1)
-    {
-        vTaskDelay(pdMS_TO_TICKS(1000));
-    }
-
-    // Set frequency to 868 MHz
-
-    // Set TX power to 22 dBm
-
-    // sx1262_setTxContinuousWave();
 }
\ No newline at end of file
diff --git a/main/components/radio.h b/main/components/radio.h
index 19684b3..f7b9bcb 100644
--- a/main/components/radio.h
+++ b/main/components/radio.h
@@ -9,6 +9,11 @@
 
 #define TAG_RADIO "RADIO"
 
+#include "packets.h"
+
 void lora_comms_task(void *pvParameters);
 
+extern TelemetryPacket telemetryPacket;
+extern uint8_t packetReadiness;
+
 #endif
\ No newline at end of file
diff --git a/main/components/sensors.c b/main/components/sensors.c
index a83be22..a085d34 100644
--- a/main/components/sensors.c
+++ b/main/components/sensors.c
@@ -1,4 +1,8 @@
 #include "sensors.h"
+#include "esp_timer.h"
+#include "radio.h"
+#include "../hw/gps.h"
+#include "servocontroller.h"
 
 #define BLINK_GPIO 2
 
@@ -13,8 +17,9 @@ static void configure_led(void)
 void i2c_sensors_task(void *pvParameters)
 {
     // initialize the xLastWakeTime variable with the current time.
-    TickType_t last_wake_time = xTaskGetTickCount();
-    const TickType_t I2C0_TASK_SAMPLING_RATE = 5;
+    const int64_t interval_us = 100000; // 100 ms
+    int64_t start_time, end_time, elapsed;
+
     //
     // initialize i2c device configuration
 
@@ -26,8 +31,8 @@ void i2c_sensors_task(void *pvParameters)
     mcp3550_spi_init();
     configure_led();
 
-    int16_t accel[3], gyro[3], temp;
-    float accel_f[3], gyro_f[3], temp_f;
+    int16_t accel[3], gyro[3], temp, magnet[3];
+    float accel_f[3], gyro_f[3], temp_f, magnet_f[3];
 
     uint16_t eCO2;
     uint16_t tvoc;
@@ -38,30 +43,32 @@ void i2c_sensors_task(void *pvParameters)
     uint16_t current;
     uint16_t power;
 
+    bme680_data_t bmeData;
     // task loop entry point
     for (;;)
     {
+        packetReadiness = 2;
+        start_time = esp_timer_get_time(); // µs since boot
         //
         // handle sensor
 
         if (BME680_DEV_HANDLE)
         {
-            bme680_data_t data;
-            esp_err_t result = bme680_get_data(BME680_DEV_HANDLE, &data);
+            esp_err_t result = bme680_get_data(BME680_DEV_HANDLE, &bmeData);
             if (result != ESP_OK)
             {
                 ESP_LOGE(TAG_BME, "bme680 device read failed (%s)", esp_err_to_name(result));
             }
             else
             {
-                data.barometric_pressure = data.barometric_pressure / 100;
-                ESP_LOGI(TAG_BME, "dewpoint temperature:%.2f °C", data.dewpoint_temperature);
-                ESP_LOGI(TAG_BME, "air temperature:     %.2f °C", data.air_temperature);
-                ESP_LOGI(TAG_BME, "relative humidity:   %.2f %%", data.relative_humidity);
-                ESP_LOGI(TAG_BME, "barometric pressure: %.2f hPa", data.barometric_pressure);
-                ccs811_set_env_data(data.air_temperature, data.relative_humidity);
-                ESP_LOGI(TAG_BME, "gas resistance:      %.2f kOhms", data.gas_resistance / 1000);
-                ESP_LOGI(TAG_BME, "iaq score:           %u (%s)", data.iaq_score, bme680_air_quality_to_string(data.iaq_score));
+                bmeData.barometric_pressure = bmeData.barometric_pressure / 100;
+                ESP_LOGI(TAG_BME, "dewpoint temperature:%.2f °C", bmeData.dewpoint_temperature);
+                ESP_LOGI(TAG_BME, "air temperature:     %.2f °C", bmeData.air_temperature);
+                ESP_LOGI(TAG_BME, "relative humidity:   %.2f %%", bmeData.relative_humidity);
+                ESP_LOGI(TAG_BME, "barometric pressure: %.2f hPa", bmeData.barometric_pressure);
+                ccs811_set_env_data(bmeData.air_temperature, bmeData.relative_humidity);
+                ESP_LOGI(TAG_BME, "gas resistance:      %.2f kOhms", bmeData.gas_resistance / 1000);
+                ESP_LOGI(TAG_BME, "iaq score:           %u (%s)", bmeData.iaq_score, bme680_air_quality_to_string(bmeData.iaq_score));
             }
         }
         else
@@ -73,12 +80,13 @@ void i2c_sensors_task(void *pvParameters)
         ESP_LOGI(TAG_CCS, "eCO₂: %d ppm, TVOC: %d ppb", eCO2, tvoc);
         ESP_LOGI(TAG_CCS, "Current: %d μA, Raw voltage: %d V", currentCCS, rawData);
 
-        if (mpu9250_read_sensor_data(MPU9250_DEV_HANDLE, accel, gyro, &temp) == ESP_OK)
+        if (mpu9250_read_sensor_data(MPU9250_DEV_HANDLE, accel, gyro, &temp, magnet) == ESP_OK)
         {
-            mpu9250_convert_data(accel, gyro, temp, accel_f, gyro_f, &temp_f);
+            mpu9250_convert_data(accel, gyro, temp, magnet, accel_f, gyro_f, &temp_f, magnet_f);
 
             ESP_LOGI(TAG_MPU, "Accel: X=%.2f g, Y=%.2f g, Z=%.2f g", accel_f[0], accel_f[1], accel_f[2]);
             ESP_LOGI(TAG_MPU, "Gyro: X=%.2f°/s, Y=%.2f°/s, Z=%.2f°/s", gyro_f[0], gyro_f[1], gyro_f[2]);
+            ESP_LOGI(TAG_MPU, "Magnet: X=%.2fμT, Y=%.2fμT, Z=%.2fμT", magnet_f[0], magnet_f[1], magnet_f[2]);
             ESP_LOGI(TAG_MPU, "Temperature: %.2f °C", temp_f);
         }
         else
@@ -89,7 +97,7 @@ void i2c_sensors_task(void *pvParameters)
         ina260_readParams(&volts, &current, &power);
         ina260_printParams(volts, current, power);
 
-        float VREFVoltage = volts * 1.25 / 1000; //Mame vobec nieco na VREFE? Na scheme su len medzi sebou prepojene
+        float VREFVoltage = volts * 1.25 / 1000; // Mame vobec nieco na VREFE? Na scheme su len medzi sebou prepojene
 
         mics_adc_data_t ADCData = mcp3550_read_all(5.0); // vref = 5.0V
         log_mics_adc_values(&ADCData);
@@ -101,7 +109,74 @@ void i2c_sensors_task(void *pvParameters)
         gpio_set_level(BLINK_GPIO, s_led_state);
         /* Toggle the LED state */
         s_led_state = !s_led_state;
-        vTaskDelaySecUntil(&last_wake_time, I2C0_TASK_SAMPLING_RATE);
+        end_time = esp_timer_get_time();
+        elapsed = end_time - start_time;
+
+        if (elapsed < interval_us)
+        {
+            vTaskDelay(pdMS_TO_TICKS((interval_us - elapsed) / 1000));
+        }
+        if (packetReadiness == 0) {
+            memset(&telemetryPacket, 0, sizeof(telemetryPacket));
+            telemetryPacket.accelerationX = accel[0];
+            telemetryPacket.accelerationY = accel[1];
+            telemetryPacket.accelerationZ = accel[2];
+
+            telemetryPacket.gyroX = gyro[0];
+            telemetryPacket.gyroX = gyro[1];
+            telemetryPacket.gyroX = gyro[2];
+
+            telemetryPacket.magnetX = gyro[0];
+            telemetryPacket.magnetX = gyro[1];
+            telemetryPacket.magnetX = gyro[2];
+
+            telemetryPacket.accelerometer_temperature = temp;
+
+            telemetryPacket.eCO2 = eCO2;
+            telemetryPacket.tvoc = tvoc;
+            telemetryPacket.currentCCS = currentCCS;
+            telemetryPacket.rawCCSData = rawData;
+
+            telemetryPacket.volts = volts;
+            telemetryPacket.current = current;
+            telemetryPacket.power = power;
+
+
+            telemetryPacket.temperature = bmeData.raw_data.temperature;
+            telemetryPacket.humidity = bmeData.raw_data.humidity;
+            telemetryPacket.pressure = bmeData.raw_data.pressure;
+            telemetryPacket.gas = bmeData.raw_data.gas;
+            telemetryPacket.gas_range = bmeData.raw_data.gas_range;
+            telemetryPacket.heater_stable = bmeData.raw_data.heater_stable;
+            telemetryPacket.gas_valid = bmeData.raw_data.gas_valid;
+            
+            telemetryPacket.NH3 = ADCData.raw_nh3;
+            telemetryPacket.CO = ADCData.raw_co;
+            telemetryPacket.NO2 = ADCData.raw_no2;
+            telemetryPacket.UVC = ADCData.raw_uvc;
+
+            //TODO MOVE THIS TO A BETTER PLACE FOR SYNC
+            telemetryPacket.time_seconds = gpsDataOut.time_seconds;
+            telemetryPacket.latitude_centi_degrees = gpsDataOut.latitude_centi_degrees;
+            telemetryPacket.longitude_centi_degrees = gpsDataOut.longitude_centi_degrees;
+            telemetryPacket.fix_quality = gpsDataOut.fix_quality;
+            telemetryPacket.num_satellites = gpsDataOut.num_satellites;
+            telemetryPacket.altitude_centi_meters = gpsDataOut.altitude_centi_meters;
+            telemetryPacket.date_yyddmm = gpsDataOut.date_yyddmm;
+            telemetryPacket.speed_centi_knots = gpsDataOut.speed_centi_knots;
+
+            telemetryPacket.predicted_altitude_centi_meters = predictedPosition.altitude_centi_meters;
+            telemetryPacket.predicted_latitude_centi_degrees = predictedPosition.latitude_centi_degrees;
+            telemetryPacket.predicted_longitude_centi_degrees = predictedPosition.longitude_centi_degrees;
+
+            telemetryPacket.targetServoA = servoState.targetServoA;
+            telemetryPacket.targetServoB = servoState.targetServoB;
+            telemetryPacket.currentServoA = servoState.currentServoA;
+            telemetryPacket.currentServoB = servoState.currentServoB;
+
+
+        }
+        packetReadiness = 1;
     }
     //
     // free resources
diff --git a/main/components/servocontroller.c b/main/components/servocontroller.c
new file mode 100644
index 0000000..82a4f1a
--- /dev/null
+++ b/main/components/servocontroller.c
@@ -0,0 +1,10 @@
+#include "servocontroller.h"
+#include "string.h"
+
+ServoState servoState;
+
+//TODO add a task and implement
+
+void servoControllerInit() {
+    memset(&servoState, 0, sizeof(servoState));
+}
\ No newline at end of file
diff --git a/main/components/servocontroller.h b/main/components/servocontroller.h
new file mode 100644
index 0000000..1b64728
--- /dev/null
+++ b/main/components/servocontroller.h
@@ -0,0 +1,19 @@
+#ifndef SERVO_CONTROLLER_COMPONENT
+#define SERVO_CONTROLLER_COMPONENT
+
+#include "stdint.h"
+
+typedef struct
+{
+    int16_t currentServoA;
+    int16_t targetServoA;
+    int16_t currentServoB;
+    int16_t targetServoB;
+} ServoState;
+
+
+extern ServoState servoState;
+
+void servoControllerInit();
+
+#endif
diff --git a/main/hw/gps.c b/main/hw/gps.c
index 0b363fa..cc8dced 100644
--- a/main/hw/gps.c
+++ b/main/hw/gps.c
@@ -9,11 +9,15 @@
 #define BUF_SIZE 1024
 #define GPS_LINE_MAX_LEN 128
 
+gps_binary_struct_t gpsDataOut;
 static QueueHandle_t uart_queue;
 
+predicted_binary_position_struct_t predictedPosition;
+
 // Initializes UART for GPS
 void gps_init()
 {
+    memset(&gpsDataOut, 0, sizeof(gpsDataOut));
     uart_config_t uart_config = {
         .baud_rate = 9600,
         .data_bits = UART_DATA_8_BITS,
@@ -49,9 +53,12 @@ void gps_task(void *arg)
 
                     if (strstr(line, "$GPGGA") == line) {
                         parse_gpgga(line);
+                        parse_gpgga_to_struct(line, &gpsDataOut);
                     } else if (strstr(line, "$GPRMC") == line) {
                         parse_gprmc(line);
+                        parse_gprmc_to_struct(line, &gpsDataOut);
                     }
+
                 }
 
                 line_pos = 0;
@@ -134,3 +141,104 @@ void parse_gprmc(const char *nmea)
              date, utc_time, lat, lat_dir, lon, lon_dir, speed_knots, status);
 }
 
+
+// Function to convert time string (hhmmss.sss) to seconds since start of day
+static uint32_t time_to_seconds_struct(const char *time_str) {
+    if (time_str == NULL || strlen(time_str) < 6) return 0;
+    uint32_t hours = (time_str[0] - '0') * 10 + (time_str[1] - '0');
+    uint32_t minutes = (time_str[2] - '0') * 10 + (time_str[3] - '0');
+    uint32_t seconds = (time_str[4] - '0') * 10 + (time_str[5] - '0');
+    return hours * 3600 + minutes * 60 + seconds;
+}
+
+// Function to convert DMS (degrees minutes.decimalminutes) to centi-degrees
+static int32_t dms_to_centi_degrees_struct(const char *dms_str, const char *direction) {
+    if (dms_str == NULL || direction == NULL || strlen(dms_str) < 7) return 0;
+    char degrees_str[4] = {0};
+    char minutes_decimal_str[10] = {0};
+    strncpy(degrees_str, dms_str, 2);
+    strncpy(minutes_decimal_str, dms_str + 2, strlen(dms_str) - 2);
+    double degrees = atof(degrees_str);
+    double minutes = atof(minutes_decimal_str);
+    double decimal_degrees = degrees + (minutes / 60.0);
+    if (direction[0] == 'S' || direction[0] == 'W') {
+        decimal_degrees *= -1.0;
+    }
+    return (int32_t)(decimal_degrees * 10000);
+}
+
+// Function to convert altitude string to centi-meters
+static int16_t altitude_to_centi_meters_struct(const char *alt_str) {
+    if (alt_str == NULL) return 0;
+    return (int16_t)(atof(alt_str) * 100);
+}
+
+// Function to convert speed from knots to centi-knots
+static uint16_t speed_to_centi_knots_struct(const char *speed_str) {
+    if (speed_str == NULL) return 0;
+    return (uint16_t)(atof(speed_str) * 100);
+}
+
+// Function to convert date string (ddmmyy) to yymmdd integer
+static uint16_t date_to_yyddmm_struct(const char *date_str) {
+    if (date_str == NULL || strlen(date_str) != 6) return 0;
+    uint16_t day = (date_str[0] - '0') * 10 + (date_str[1] - '0');
+    uint16_t month = (date_str[2] - '0') * 10 + (date_str[3] - '0');
+    uint16_t year_short = (date_str[4] - '0') * 10 + (date_str[5] - '0');
+    // Assuming year is in the 21st century for simplicity
+    return (2000 + year_short) * 10000 + month * 100 + day;
+}
+
+// Function to parse GPGGA NMEA string and return the struct
+void parse_gpgga_to_struct(const char *nmea, gps_binary_struct_t *data)
+{
+    char *fields[15];
+    char temp[GPS_LINE_MAX_LEN];
+    strncpy(temp, nmea, GPS_LINE_MAX_LEN);
+    temp[GPS_LINE_MAX_LEN - 1] = '\0';
+
+    int i = 0;
+    char *token = strtok(temp, ",");
+    while (token != NULL && i < 15) {
+        fields[i++] = token;
+        token = strtok(NULL, ",");
+    }
+
+    if (i >= 10) {
+        data->time_seconds = time_to_seconds_struct(fields[1]);
+        data->latitude_centi_degrees = dms_to_centi_degrees_struct(fields[2], fields[3]);
+        data->longitude_centi_degrees = dms_to_centi_degrees_struct(fields[4], fields[5]);
+        data->fix_quality = atoi(fields[6]);
+        data->num_satellites = atoi(fields[7]);
+        data->altitude_centi_meters = altitude_to_centi_meters_struct(fields[9]);
+    } else {
+        ESP_LOGW(TAG, "GPGGA: Not enough fields to parse struct");
+    }
+}
+
+// Function to parse GPRMC NMEA string and return the struct
+void parse_gprmc_to_struct(const char *nmea, gps_binary_struct_t *data)
+{
+    char *fields[13];
+    char temp[GPS_LINE_MAX_LEN];
+    strncpy(temp, nmea, GPS_LINE_MAX_LEN);
+    temp[GPS_LINE_MAX_LEN - 1] = '\0';
+
+    int i = 0;
+    char *token = strtok(temp, ",");
+    while (token != NULL && i < 13) {
+        fields[i++] = token;
+        token = strtok(NULL, ",");
+    }
+
+    if (i >= 12) {
+        data->time_seconds = time_to_seconds_struct(fields[1]);
+        data->latitude_centi_degrees = dms_to_centi_degrees_struct(fields[3], fields[4]);
+        data->longitude_centi_degrees = dms_to_centi_degrees_struct(fields[5], fields[6]);
+        data->date_yyddmm = date_to_yyddmm_struct(fields[9]);
+        data->speed_centi_knots = speed_to_centi_knots_struct(fields[7]);
+        // Fix quality and num_satellites are typically in GPGGA, so they might be 0 here.
+    } else {
+        ESP_LOGW(TAG, "GPRMC: Not enough fields to parse struct");
+    }
+}
\ No newline at end of file
diff --git a/main/hw/gps.h b/main/hw/gps.h
index 7713b78..1f44966 100644
--- a/main/hw/gps.h
+++ b/main/hw/gps.h
@@ -5,10 +5,35 @@
 #include "soc/uart_struct.h"
 #include "buscfg.h"
 
+// Structure to hold parsed GPS data in a compact binary format
+typedef struct {
+    uint32_t time_seconds;      // Seconds since start of day
+    int32_t latitude_centi_degrees;   // Latitude * 10,000
+    int32_t longitude_centi_degrees;  // Longitude * 10,000
+    uint8_t fix_quality;
+    uint8_t num_satellites;
+    int16_t altitude_centi_meters;    // Altitude * 100
+    uint16_t date_yyddmm;       // YYDDMM (from GPRMC)
+    uint16_t speed_centi_knots;  // Speed * 100 (from GPRMC)
+} gps_binary_struct_t;
 
+typedef struct {
+    int32_t latitude_centi_degrees;  // Latitude * 10,000
+    int32_t longitude_centi_degrees; // Longitude * 10,000
+    int16_t altitude_centi_meters; // Altitude * 100
+} predicted_binary_position_struct_t;
+
+extern gps_binary_struct_t gpsDataOut;
+
+extern predicted_binary_position_struct_t predictedPosition;
 
 void gps_task(void *arg);
 void parse_gpgga(const char *nmea);
 void parse_gprmc(const char *nmea);
 
+void parse_gprmc_to_struct(const char *nmea, gps_binary_struct_t *data);
+void parse_gpgga_to_struct(const char *nmea, gps_binary_struct_t *data);
+
+
+
 #endif
\ No newline at end of file
diff --git a/main/hw/mpu9250.c b/main/hw/mpu9250.c
index 8fa616b..d3b2996 100644
--- a/main/hw/mpu9250.c
+++ b/main/hw/mpu9250.c
@@ -1,4 +1,5 @@
 #include "mpu9250.h"
+#include "esp_timer.h"
 
 i2c_device_config_t MPU9250_DEV_CFG = {
     .dev_addr_length = I2C_ADDR_BIT_LEN_7,
@@ -6,9 +7,11 @@ i2c_device_config_t MPU9250_DEV_CFG = {
     .scl_speed_hz = 100000,
 };
 
+#define MAG_SCALE (4912.0f / 32760.0f)
+
 i2c_master_dev_handle_t MPU9250_DEV_HANDLE;
 
-esp_err_t mpu9250_read_sensor_data(i2c_master_dev_handle_t dev_handle, int16_t *accel, int16_t *gyro, int16_t *temp)
+esp_err_t mpu9250_read_sensor_data(i2c_master_dev_handle_t dev_handle, int16_t *accel, int16_t *gyro, int16_t *temp, int16_t *magnet)
 {
     uint8_t buffer[14]; // 6 (Accel) + 2 (Temp) + 6 (Gyro)
     esp_err_t ret = i2c_read_register(dev_handle, 0x3B, buffer, 14);
@@ -16,18 +19,28 @@ esp_err_t mpu9250_read_sensor_data(i2c_master_dev_handle_t dev_handle, int16_t *
         return ret;
 
     // Convert raw data (Big-Endian)
-    accel[0] = (((uint16_t)(buffer[0] & 0xFF)) << 8) | (buffer[1] & 0xFF);  // Accel X
-    accel[1] = (((uint16_t)(buffer[2] & 0xFF)) << 8) | (buffer[3] & 0xFF);  // Accel Y
-    accel[2] = (((uint16_t)(buffer[4] & 0xFF)) << 8) | (buffer[5] & 0xFF);  // Accel Z
-    *temp = (((uint16_t)(buffer[6] & 0xFF)) << 8) | (buffer[7] & 0xFF);     // Temperature
-    gyro[0] = (((uint16_t)(buffer[8] & 0xFF)) << 8) | (buffer[9] & 0xFF);;   // Gyro X
-    gyro[1] = (((uint16_t)(buffer[10] & 0xFF)) << 8) | (buffer[11] & 0xFF);; // Gyro Y
-    gyro[2] = (((uint16_t)(buffer[12] & 0xFF)) << 8) | (buffer[13] & 0xFF);; // Gyro Z
+    accel[0] = (((uint16_t)(buffer[0] & 0xFF)) << 8) | (buffer[1] & 0xFF); // Accel X
+    accel[1] = (((uint16_t)(buffer[2] & 0xFF)) << 8) | (buffer[3] & 0xFF); // Accel Y
+    accel[2] = (((uint16_t)(buffer[4] & 0xFF)) << 8) | (buffer[5] & 0xFF); // Accel Z
+    *temp = (((uint16_t)(buffer[6] & 0xFF)) << 8) | (buffer[7] & 0xFF);    // Temperature
+    gyro[0] = (((uint16_t)(buffer[8] & 0xFF)) << 8) | (buffer[9] & 0xFF);
+    ; // Gyro X
+    gyro[1] = (((uint16_t)(buffer[10] & 0xFF)) << 8) | (buffer[11] & 0xFF);
+    ; // Gyro Y
+    gyro[2] = (((uint16_t)(buffer[12] & 0xFF)) << 8) | (buffer[13] & 0xFF);
+    ; // Gyro Z
+
+    ret = i2c_read_register(dev_handle, 0x03, buffer, 6);
+    if (ret != ESP_OK)
+        return ret;
+    magnet[0] = (((uint16_t)(buffer[0] & 0xFF)) << 8) | (buffer[1] & 0xFF); // Magnet X
+    magnet[1] = (((uint16_t)(buffer[2] & 0xFF)) << 8) | (buffer[3] & 0xFF); // Magnet Y
+    magnet[2] = (((uint16_t)(buffer[4] & 0xFF)) << 8) | (buffer[5] & 0xFF); // Magnet Z
 
     return ESP_OK;
 }
 
-void mpu9250_convert_data(int16_t *accel, int16_t *gyro, int16_t temp, float *accel_out, float *gyro_out, float *temp_out)
+void mpu9250_convert_data(int16_t *accel, int16_t *gyro, int16_t temp, int16_t *magnet, float *accel_out, float *gyro_out, float *temp_out, float *magnet_out)
 {
     accel_out[0] = accel[0] / 16384.0; // Accel X in g
     accel_out[1] = accel[1] / 16384.0; // Accel Y in g
@@ -37,10 +50,16 @@ void mpu9250_convert_data(int16_t *accel, int16_t *gyro, int16_t temp, float *ac
     gyro_out[1] = gyro[1] / 131.0; // Gyro Y in deg/s
     gyro_out[2] = gyro[2] / 131.0; // Gyro Z in deg/s
 
+    magnet_out[0] = magnet[0] * MAG_SCALE; // Gyro X in deg/s
+    magnet_out[1] = magnet[1] * MAG_SCALE; // Gyro Y in deg/s
+    magnet_out[2] = magnet[2] * MAG_SCALE; // Gyro Z in deg/s
+
     *temp_out = (temp / 333.87) + 21.0; // Temperature in °C
 }
 
-void mpu9250_init() {
+void mpu9250_init()
+{
     ESP_ERROR_CHECK(i2c_master_bus_add_device(i2c0_bus_hdl, &MPU9250_DEV_CFG, &MPU9250_DEV_HANDLE));
     i2c_write_register_16(MPU9250_DEV_HANDLE, 0x6B, 0x0001); // zapni uz tu hovadinu
+    i2c_write_register_16(MPU9250_DEV_HANDLE, 0x0A, 0x0012); // zapni uz tu hovadinu
 }
\ No newline at end of file
diff --git a/main/hw/mpu9250.h b/main/hw/mpu9250.h
index a9eef8b..0ca03c2 100644
--- a/main/hw/mpu9250.h
+++ b/main/hw/mpu9250.h
@@ -8,8 +8,8 @@ extern i2c_device_config_t MPU9250_DEV_CFG;
 extern i2c_master_dev_handle_t MPU9250_DEV_HANDLE;
 
 
-esp_err_t mpu9250_read_sensor_data(i2c_master_dev_handle_t dev_handle, int16_t *accel, int16_t *gyro, int16_t *temp);
-void mpu9250_convert_data(int16_t *accel, int16_t *gyro, int16_t temp, float *accel_out, float *gyro_out, float *temp_out);
+esp_err_t mpu9250_read_sensor_data(i2c_master_dev_handle_t dev_handle, int16_t *accel, int16_t *gyro, int16_t *temp, int16_t *magnet);
+void mpu9250_convert_data(int16_t *accel, int16_t *gyro, int16_t temp, int16_t *magnet, float *accel_out, float *gyro_out, float *temp_out, float *magnet_out);
 void mpu9250_init();
 
 #endif
\ No newline at end of file
diff --git a/main/hw/sx1262.c b/main/hw/sx1262.c
index 22cce0a..7854c2c 100644
--- a/main/hw/sx1262.c
+++ b/main/hw/sx1262.c
@@ -1,578 +1,976 @@
+#include <stdio.h>
+#include <inttypes.h>
+#include <string.h>
+#include <math.h>
+#include <stdlib.h>
+
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+
+#include <driver/spi_master.h>
+#include <driver/gpio.h>
+#include "esp_log.h"
+
 #include "sx1262.h"
 
-spi_device_handle_t spiSXko;
+#define TAG "SX1262"
 
-#define TAG_SPIDUMP "[SPI]"
+#define HOST_ID SPI3_HOST
 
-void sx1262_init()
+#define CONFIG_MISO_GPIO GPIO_NUM_11
+#define CONFIG_MOSI_GPIO GPIO_NUM_10
+#define CONFIG_SCLK_GPIO GPIO_NUM_9
+#define CONFIG_NSS_GPIO GPIO_NUM_8
+#define CONFIG_RST_GPIO GPIO_NUM_12
+#define CONFIG_BUSY_GPIO GPIO_NUM_13
+
+static spi_device_handle_t SpiHandle;
+
+// Global Stuff
+static uint8_t PacketParams[6];
+static bool txActive;
+static int txLost = 0;
+static bool debugPrint;
+static int SX126x_SPI_SELECT;
+static int SX126x_RESET;
+static int SX126x_BUSY;
+
+// Arduino compatible macros
+#define delayMicroseconds(us) esp_rom_delay_us(us)
+#define delay(ms) esp_rom_delay_us(ms*1000)
+
+
+void LoRaErrorDefault(int error)
 {
-  ESP_LOGI(TAG, "Initializing SX1262...");
-
-  gpio_set_direction(HSPI_LORA_CS, GPIO_MODE_OUTPUT);
-  gpio_set_direction(LORA_BUSY, GPIO_MODE_INPUT);
-
-  gpio_set_level(HSPI_LORA_CS, 1);
-  mcp23018_set_pin(MCP23018_DEV_HANDLE, MCP_LORA_RST, 1);
-
-  spi_bus_config_t buscfg;
-  memset(&buscfg, 0, sizeof(spi_bus_config_t));
-  buscfg.miso_io_num = HSPI_MISO_GPIO;
-  buscfg.mosi_io_num = HSPI_MOSI_GPIO;
-  buscfg.sclk_io_num = HSPI_SCK_GPIO;
-  buscfg.max_transfer_sz = 32;
-  buscfg.quadwp_io_num = -1;
-  buscfg.quadhd_io_num = -1;
-
-  spi_device_interface_config_t devcfg = {
-      .command_bits = 0,
-      .address_bits = 0,
-      .mode = 0,
-      .cs_ena_pretrans = 10,
-      .clock_speed_hz = 8 * 1000 * 1000,
-      .spics_io_num = HSPI_LORA_CS,
-      .flags = 0,
-      .queue_size = 1};
-
-  ESP_LOGI(TAG, "Adding SPI device...");
-  esp_err_t ret = spi_bus_add_device(LORA_SPI_HOST, &devcfg, &spiSXko);
-  if (ret != ESP_OK)
-  {
-    ESP_LOGE(TAG, "SPI device init failed!");
-    return;
-  }
-
-  ESP_LOGI(TAG, "SX1262 Initialized");
-  sx1262_reset();
+	if (debugPrint) {
+		ESP_LOGE(TAG, "LoRaErrorDefault=%d", error);
+	}
+	while (true) {
+		vTaskDelay(1);
+	}
 }
 
-void sx1262_reset()
+__attribute__ ((weak, alias ("LoRaErrorDefault"))) void LoRaError(int error);
+
+
+void LoRaInit(void)
 {
-  ESP_LOGI(TAG, "Resetting SX1262...");
-  vTaskDelay(pdMS_TO_TICKS(10));
-  mcp23018_set_pin(MCP23018_DEV_HANDLE, MCP_LORA_RST, 0);
-  vTaskDelay(pdMS_TO_TICKS(20));
-  mcp23018_set_pin(MCP23018_DEV_HANDLE, MCP_LORA_RST, 1);
-  vTaskDelay(pdMS_TO_TICKS(10));
-  sx1262_wait_for_busy();
-  ESP_LOGI(TAG, "SX1262 Reset complete.");
+	ESP_LOGI(TAG, "CONFIG_MISO_GPIO=%d", CONFIG_MISO_GPIO);
+	ESP_LOGI(TAG, "CONFIG_MOSI_GPIO=%d", CONFIG_MOSI_GPIO);
+	ESP_LOGI(TAG, "CONFIG_SCLK_GPIO=%d", CONFIG_SCLK_GPIO);
+	ESP_LOGI(TAG, "CONFIG_NSS_GPIO=%d", CONFIG_NSS_GPIO);
+	ESP_LOGI(TAG, "CONFIG_RST_GPIO=%d", CONFIG_RST_GPIO);
+	ESP_LOGI(TAG, "CONFIG_BUSY_GPIO=%d", CONFIG_BUSY_GPIO);
+
+	SX126x_SPI_SELECT = CONFIG_NSS_GPIO;
+	SX126x_RESET = CONFIG_RST_GPIO;
+	SX126x_BUSY	= CONFIG_BUSY_GPIO;
+	
+	txActive = false;
+	debugPrint = false;
+
+	gpio_reset_pin(SX126x_SPI_SELECT);
+	gpio_set_direction(SX126x_SPI_SELECT, GPIO_MODE_OUTPUT);
+	gpio_set_level(SX126x_SPI_SELECT, 1);
+
+	gpio_reset_pin(SX126x_RESET);
+	gpio_set_direction(SX126x_RESET, GPIO_MODE_OUTPUT);
+	
+	gpio_reset_pin(SX126x_BUSY);
+	gpio_set_direction(SX126x_BUSY, GPIO_MODE_INPUT);
+
+	spi_bus_config_t spi_bus_config = {
+		.sclk_io_num = CONFIG_SCLK_GPIO,
+		.mosi_io_num = CONFIG_MOSI_GPIO,
+		.miso_io_num = CONFIG_MISO_GPIO,
+		.quadwp_io_num = -1,
+		.quadhd_io_num = -1
+	};
+
+	esp_err_t ret;
+	spi_device_interface_config_t devcfg = {
+		.clock_speed_hz = 9000000,
+		.mode = 0,
+		.spics_io_num = CONFIG_NSS_GPIO,
+		.queue_size = 7,
+		.flags = 0,
+		.pre_cb = NULL
+	};
+	//spi_device_handle_t handle;
+	ret = spi_bus_add_device( HOST_ID, &devcfg, &SpiHandle);
+	ESP_LOGI(TAG, "spi_bus_add_device=%d",ret);
+	assert(ret==ESP_OK);
 }
 
-void sx1262_wait_for_busy()
+void spi_write_byte(uint8_t* Dataout, size_t DataLength )
 {
-  ESP_LOGV("WAIT", "Waiting for SX1262 to be ready...");
-  while (gpio_get_level(LORA_BUSY) == 1)
-  {
-    vTaskDelay(pdMS_TO_TICKS(10));
-  }
-  ESP_LOGV("WAIT", "SX1262 is ready.");
+	spi_transaction_t SPITransaction;
+
+	if ( DataLength > 0 ) {
+		memset( &SPITransaction, 0, sizeof( spi_transaction_t ) );
+		SPITransaction.length = DataLength * 8;
+		SPITransaction.tx_buffer = Dataout;
+		SPITransaction.rx_buffer = NULL;
+		spi_device_transmit( SpiHandle, &SPITransaction );
+	}
+
+	return;
 }
 
-void sx1262_write_command(uint8_t cmd, uint8_t *data, uint8_t len)
+void spi_read_byte(uint8_t* Datain, uint8_t* Dataout, size_t DataLength )
 {
-  ESP_LOGV(TAG, "Writing command 0x%02X with length %d", cmd, len);
-  spi_transaction_t t;
-  memset(&t, 0, sizeof(t));
-  t.length = (len + 1) * 8;
-  uint8_t *tx_data = malloc(len + 1);
-  memset(tx_data, 0, len + 1);
-  tx_data[0] = cmd;
-  if (len)
-  {
-    memcpy(tx_data + 1, data, len);
-  }
-  t.tx_buffer = tx_data;
+	spi_transaction_t SPITransaction;
 
-  sx1262_wait_for_busy();
+	if ( DataLength > 0 ) {
+		memset( &SPITransaction, 0, sizeof( spi_transaction_t ) );
+		SPITransaction.length = DataLength * 8;
+		SPITransaction.tx_buffer = Dataout;
+		SPITransaction.rx_buffer = Datain;
+		spi_device_transmit( SpiHandle, &SPITransaction );
+	}
 
-  ESP_LOGI(TAG_SPIDUMP, "SPI TX (cmd 0x%02X):", cmd);
-  ESP_LOG_BUFFER_HEXDUMP(TAG_SPIDUMP, tx_data, len + 1, ESP_LOG_INFO);
-
-  esp_err_t ret = spi_device_transmit(spiSXko, &t);
-
-  if (ret != ESP_OK)
-  {
-    ESP_LOGE(TAG, "SPI write failed!");
-  }
-  else
-  {
-    ESP_LOGV(TAG, "SPI write successful");
-  }
-  free(tx_data);
+	return;
 }
 
-uint8_t SX1262_STATUS = 0;
-
-void sx1262_read_command(uint8_t cmd, uint8_t *tx_payload_buffer, uint8_t tx_payload_len, uint8_t *rx_buffer, uint8_t len)
+uint8_t spi_transfer(uint8_t address)
 {
-  ESP_LOGV(TAG, "Reading command 0x%02X with expected length %d", cmd, len);
-  spi_transaction_t t;
-  memset(&t, 0, sizeof(t));
-  t.length = (len + 2) * 8;
-  uint8_t *tx_data = malloc(len + 2);
-  uint8_t *rx_data = malloc(len + 2);
-  memset(tx_data, 0, len + 2);
-  memset(rx_data, 0, len + 2);
-
-  memset(tx_data, 0, sizeof(tx_data));
-  memset(rx_data, 0, sizeof(rx_data));
-  if (tx_payload_buffer != NULL && tx_payload_len > 0)
-  {
-    if (tx_payload_len > len + 1)
-    {
-      tx_payload_len = len + 1;
-    }
-    memcpy(tx_data + 1, tx_payload_buffer, tx_payload_len);
-  }
-  tx_data[0] = cmd;
-
-  t.tx_buffer = tx_data;
-  t.rx_buffer = rx_data;
-
-  sx1262_wait_for_busy();
-
-  ESP_LOGI(TAG_SPIDUMP, "SPI TX (cmd 0x%02X):", cmd);
-  ESP_LOG_BUFFER_HEXDUMP(TAG_SPIDUMP, tx_data, len + 1, ESP_LOG_INFO);
-
-  esp_err_t ret = spi_device_transmit(spiSXko, &t);
-
-  if (ret == ESP_OK)
-  {
-    SX1262_STATUS = rx_data[1];
-    memcpy(rx_buffer, &rx_data[2], len);
-    ESP_LOGI(TAG_SPIDUMP, "SPI RX (cmd 0x%02X):", cmd);
-    ESP_LOG_BUFFER_HEXDUMP(TAG_SPIDUMP, rx_data, len + 2, ESP_LOG_INFO);
-    ESP_LOGV(TAG_SPIDUMP, "SPI read successful, status: 0x%02X", SX1262_STATUS);
-  }
-  else
-  {
-    ESP_LOGE(TAG, "SPI read failed!");
-  }
-  free(rx_data);
-  free(tx_data);
+	uint8_t datain[1];
+	uint8_t dataout[1];
+	dataout[0] = address;
+	//spi_write_byte(dataout, 1 );
+	spi_read_byte(datain, dataout, 1 );
+	return datain[0];
 }
 
-void resolve_status_byte(uint8_t status, uint16_t op_error)
+
+int16_t LoRaBegin(uint32_t frequencyInHz, int8_t txPowerInDbm, float tcxoVoltage, bool useRegulatorLDO) 
 {
-  const char *chip_modes[] = {
-      "Unused", "RFU", "STBY_RC", "STBY_XOSC",
-      "FS", "RX", "TX", "RFU"};
+	if ( txPowerInDbm > 22 )
+		txPowerInDbm = 22;
+	if ( txPowerInDbm < -3 )
+		txPowerInDbm = -3;
+	
+	Reset();
+	
+	uint8_t wk[2];
+	ReadRegister(SX126X_REG_LORA_SYNC_WORD_MSB, wk, 2); // 0x0740
+	uint16_t syncWord = (wk[0] << 8) + wk[1];
+	ESP_LOGI(TAG, "syncWord=0x%x", syncWord);
+	if (syncWord != SX126X_SYNC_WORD_PUBLIC && syncWord != SX126X_SYNC_WORD_PRIVATE) {
+		ESP_LOGE(TAG, "SX126x error, maybe no SPI connection");
+		return ERR_INVALID_MODE;
+	}
 
-  const char *command_statuses[] = {
-      "Reserved", "RFU", "Data is available to host", "Command timeout",
-      "Command processing error", "Failure to execute command", "Command TX done", "RFU"};
+	ESP_LOGI(TAG, "SX126x installed");
+	SetStandby(SX126X_STANDBY_RC);
 
-  uint8_t chip_mode = (status >> 4) & 0x07;      // Bits 6:4
-  uint8_t command_status = (status >> 1) & 0x07; // Bits 3:1
+	SetDio2AsRfSwitchCtrl(true);
+	ESP_LOGI(TAG, "tcxoVoltage=%f", tcxoVoltage);
+	// set TCXO control, if requested
+	if(tcxoVoltage > 0.0) {
+		SetDio3AsTcxoCtrl(tcxoVoltage, RADIO_TCXO_SETUP_TIME); // Configure the radio to use a TCXO controlled by DIO3
+	}
 
-  const char *chip_mode_str = chip_modes[chip_mode];
-  const char *command_status_str = command_statuses[command_status];
+	Calibrate(	SX126X_CALIBRATE_IMAGE_ON
+		| SX126X_CALIBRATE_ADC_BULK_P_ON
+		| SX126X_CALIBRATE_ADC_BULK_N_ON
+		| SX126X_CALIBRATE_ADC_PULSE_ON
+		| SX126X_CALIBRATE_PLL_ON
+		| SX126X_CALIBRATE_RC13M_ON
+		| SX126X_CALIBRATE_RC64K_ON
+	);
 
-  const char *op_errors[] = {
-      "RC64k calibration failed",
-      "RC13M calibration failed",
-      "PLL calibration failed",
-      "ADC calibration failed",
-      "IMG calibration failed",
-      "XOSC failed to start",
-      "PLL failed to lock",
-      "RFU",
-      "PA ramping failed"};
+	ESP_LOGI(TAG, "useRegulatorLDO=%d", useRegulatorLDO);
+	if (useRegulatorLDO) {
+		SetRegulatorMode(SX126X_REGULATOR_LDO); // set regulator mode: LDO
+	} else {
+		SetRegulatorMode(SX126X_REGULATOR_DC_DC); // set regulator mode: DC-DC
+	}
 
-  char errorStr[1024];
-  memset(errorStr, 0, sizeof(errorStr));
-
-  if (command_status == 0x05)
-  {
-    strcat(errorStr, "Command error: ");
-  }
-
-  for (int i = 0; i < 9; i++)
-  {
-    if (op_error & (1 << i))
-    {
-      strcat(errorStr, op_errors[i]);
-      strcat(errorStr, ", ");
-      ESP_LOGV(TAG, "OpError: %s", op_errors[i]);
-    }
-  }
-
-  ESP_LOGI(TAG, "Chip Mode: %s(0x%02X), Command Status: %s(0x%02X) %s", chip_mode_str, chip_mode, command_status_str, command_status, errorStr);
+	SetBufferBaseAddress(0, 0);
+#if 0
+	// SX1261_TRANCEIVER
+	SetPaConfig(0x06, 0x00, 0x01, 0x01); // PA Optimal Settings +15 dBm
+	// SX1262_TRANCEIVER
+	SetPaConfig(0x04, 0x07, 0x00, 0x01); // PA Optimal Settings +22 dBm
+	// SX1268_TRANCEIVER
+	SetPaConfig(0x04, 0x07, 0x00, 0x01); // PA Optimal Settings +22 dBm
+#endif
+	SetPaConfig(0x04, 0x07, 0x00, 0x01); // PA Optimal Settings +22 dBm
+	SetOvercurrentProtection(60.0);  // current max 60mA for the whole device
+	SetPowerConfig(txPowerInDbm, SX126X_PA_RAMP_200U); //0 fuer Empfaenger
+	SetRfFrequency(frequencyInHz);
+	return ERR_NONE;
 }
 
-sx1262_status_t sx1262_get_status()
+void FixInvertedIQ(uint8_t iqConfig)
 {
-  ESP_LOGV(TAG, "Getting SX1262 status...");
-  sx1262_status_t status;
-  uint8_t buffer[1] = {0};
-  sx1262_read_command(0xC0, NULL, 0, buffer, 1);
-  status.status = buffer[0];
-  status.error = sx1262_getDeviceErrors();
-  sx1262_clearDeviceErrors();
-  resolve_status_byte(status.status, status.error);
-  return status;
+	// fixes IQ configuration for inverted IQ
+	// see SX1262/SX1268 datasheet, chapter 15 Known Limitations, section 15.4 for details
+	// When exchanging LoRa packets with inverted IQ polarity, some packet losses may be observed for longer packets.
+	// Workaround: Bit 2 at address 0x0736 must be set to:
+	// “0” when using inverted IQ polarity (see the SetPacketParam(...) command)
+	// “1” when using standard IQ polarity
+
+	// read current IQ configuration
+	uint8_t iqConfigCurrent = 0;
+	ReadRegister(SX126X_REG_IQ_POLARITY_SETUP, &iqConfigCurrent, 1); // 0x0736
+
+	// set correct IQ configuration
+	//if(iqConfig == SX126X_LORA_IQ_STANDARD) {
+	if(iqConfig == SX126X_LORA_IQ_INVERTED) {
+		iqConfigCurrent &= 0xFB; // using inverted IQ polarity
+	} else {
+		iqConfigCurrent |= 0x04; // using standard IQ polarity
+	}
+
+	// update with the new value
+	WriteRegister(SX126X_REG_IQ_POLARITY_SETUP, &iqConfigCurrent, 1); // 0x0736
 }
 
-void sx1262_setSleep(uint8_t sleepCFG)
+
+void LoRaConfig(uint8_t spreadingFactor, uint8_t bandwidth, uint8_t codingRate, uint16_t preambleLength, uint8_t payloadLen, bool crcOn, bool invertIrq) 
 {
-  sx1262_write_command(0x84, &sleepCFG, 1);
+	SetStopRxTimerOnPreambleDetect(false);
+	SetLoRaSymbNumTimeout(0); 
+	SetPacketType(SX126X_PACKET_TYPE_LORA); // SX126x.ModulationParams.PacketType : MODEM_LORA
+	uint8_t ldro = 0; // LowDataRateOptimize OFF
+	SetModulationParams(spreadingFactor, bandwidth, codingRate, ldro);
+	
+	PacketParams[0] = (preambleLength >> 8) & 0xFF;
+	PacketParams[1] = preambleLength;
+	if ( payloadLen )
+	{
+		PacketParams[2] = 0x01; // Fixed length packet (implicit header)
+		PacketParams[3] = payloadLen;
+	}
+	else
+	{
+		PacketParams[2] = 0x00; // Variable length packet (explicit header)
+		PacketParams[3] = 0xFF;
+	}
+
+	if ( crcOn )
+		PacketParams[4] = SX126X_LORA_CRC_ON;
+	else
+		PacketParams[4] = SX126X_LORA_CRC_OFF;
+
+	if ( invertIrq )
+		PacketParams[5] = 0x01; // Inverted LoRa I and Q signals setup
+	else
+		PacketParams[5] = 0x00; // Standard LoRa I and Q signals setup
+
+	// fixes IQ configuration for inverted IQ
+	FixInvertedIQ(PacketParams[5]);
+
+	WriteCommand(SX126X_CMD_SET_PACKET_PARAMS, PacketParams, 6); // 0x8C
+
+	// Do not use DIO interruptst
+	SetDioIrqParams(SX126X_IRQ_ALL, //all interrupts enabled
+		SX126X_IRQ_NONE, //interrupts on DIO1
+		SX126X_IRQ_NONE, //interrupts on DIO2
+		SX126X_IRQ_NONE //interrupts on DIO3
+	);
+
+	// Receive state no receive timeoout
+	SetRx(0xFFFFFF);
 }
 
-void sx1262_setStandby(uint8_t standbyConf)
+
+void LoRaDebugPrint(bool enable) 
 {
-  sx1262_write_command(0x80, &standbyConf, 1);
+	debugPrint = enable;
 }
 
-void sx1262_setFrequencySynthesis()
+
+uint8_t LoRaReceive(uint8_t *pData, int16_t len) 
 {
-  sx1262_write_command(0xC1, NULL, 0);
+	uint8_t rxLen = 0;
+	uint16_t irqRegs = GetIrqStatus();
+	//uint8_t status = GetStatus();
+	
+	if( irqRegs & SX126X_IRQ_RX_DONE )
+	{
+		//ClearIrqStatus(SX126X_IRQ_RX_DONE);
+		ClearIrqStatus(SX126X_IRQ_ALL);
+		rxLen = ReadBuffer(pData, len);
+	}
+	
+	return rxLen;
 }
 
-void sx1262_setMode(uint8_t mode, uint32_t timeout)
+
+bool LoRaSend(uint8_t *pData, int16_t len, uint8_t mode)
 {
+	uint16_t irqStatus;
+	bool rv = false;
+	
+	if ( txActive == false )
+	{
+		txActive = true;
+		if (PacketParams[2] == 0x00) { // Variable length packet (explicit header)
+			PacketParams[3] = len;
+		}
+		WriteCommand(SX126X_CMD_SET_PACKET_PARAMS, PacketParams, 6); // 0x8C
+		
+		//ClearIrqStatus(SX126X_IRQ_TX_DONE | SX126X_IRQ_TIMEOUT);
+		ClearIrqStatus(SX126X_IRQ_ALL);
+		
+		WriteBuffer(pData, len);
+		SetTx(500);
 
-  uint32_t timeoutUnits = 0;
-
-  if (timeout != 0)
-  {
-    uint64_t timeoutInUs = (uint64_t)timeout * 1000;      // Convert ms → us
-    timeoutUnits = (uint32_t)((timeoutInUs * 64) / 1000); // Convert us → SX1262 units
-  }
-
-  uint8_t buffer[4] = {
-      (uint8_t)((timeoutUnits >> 24) & 0xFF),
-      (uint8_t)((timeoutUnits >> 16) & 0xFF),
-      (uint8_t)((timeoutUnits >> 8) & 0xFF),
-      (uint8_t)((timeoutUnits) & 0xFF)};
-  if (mode)
-  {
-    sx1262_write_command(0x83, buffer, 3);
-  }
-  else
-  {
-    sx1262_write_command(0x82, buffer, 3);
-  }
+		if ( mode & SX126x_TXMODE_SYNC )
+		{
+			irqStatus = GetIrqStatus();
+			while ( (!(irqStatus & SX126X_IRQ_TX_DONE)) && (!(irqStatus & SX126X_IRQ_TIMEOUT)) )
+			{
+				delay(1);
+				irqStatus = GetIrqStatus();
+			}
+			if (debugPrint) {
+				ESP_LOGI(TAG, "irqStatus=0x%x", irqStatus);
+				if (irqStatus & SX126X_IRQ_TX_DONE) {
+					ESP_LOGI(TAG, "SX126X_IRQ_TX_DONE");
+				}
+				if (irqStatus & SX126X_IRQ_TIMEOUT) {
+					ESP_LOGI(TAG, "SX126X_IRQ_TIMEOUT");
+				}
+			}
+			txActive = false;
+	
+			SetRx(0xFFFFFF);
+	
+			if ( irqStatus & SX126X_IRQ_TX_DONE) {
+				rv = true;
+			}
+		}
+		else
+		{
+			rv = true;
+		}
+	}
+	if (debugPrint) {
+		ESP_LOGI(TAG, "Send rv=0x%x", rv);
+	}
+	if (rv == false) txLost++;
+	return rv;
 }
 
-void sx1262_stopTimerOnPreamble(uint8_t enable)
+
+bool ReceiveMode(void)
 {
-  sx1262_write_command(0x9F, &enable, 1);
+	uint16_t irq;
+	bool rv = false;
+
+	if ( txActive == false )
+	{
+		rv = true;
+	}
+	else
+	{
+		irq = GetIrqStatus();
+		if ( irq & (SX126X_IRQ_TX_DONE | SX126X_IRQ_TIMEOUT) )
+		{ 
+			SetRx(0xFFFFFF);
+			txActive = false;
+			rv = true;
+		}
+	}
+
+	return rv;
 }
 
-void sx1262_setRxDutyCycle(uint32_t rxPeriod, uint32_t sleepPeriod)
+
+void GetPacketStatus(int8_t *rssiPacket, int8_t *snrPacket)
 {
-  uint8_t payload[6];
-  payload[0] = (rxPeriod >> 16) & 0xFF;
-  payload[1] = (rxPeriod >> 8) & 0xFF;
-  payload[2] = rxPeriod & 0xFF;
-  payload[3] = (sleepPeriod >> 16) & 0xFF;
-  payload[4] = (sleepPeriod >> 8) & 0xFF;
-  payload[5] = sleepPeriod & 0xFF;
-  sx1262_write_command(0x94, payload, 6);
+	uint8_t buf[4];
+	ReadCommand( SX126X_CMD_GET_PACKET_STATUS, buf, 4 ); // 0x14
+	*rssiPacket = (buf[3] >> 1) * -1;
+	( buf[2] < 128 ) ? ( *snrPacket = buf[2] >> 2 ) : ( *snrPacket = ( ( buf[2] - 256 ) >> 2 ) );
 }
 
-void sx1262_setChannelActivityDetection(void)
+
+void SetTxPower(int8_t txPowerInDbm)
 {
-  sx1262_write_command(0xC5, NULL, 0);
+	SetPowerConfig(txPowerInDbm, SX126X_PA_RAMP_200U);
 }
 
-void sx1262_setTxContinuousWave(void)
+
+void Reset(void)
 {
-  sx1262_write_command(0xD1, NULL, 0);
+	delay(10);
+	gpio_set_level(SX126x_RESET,0);
+	delay(20);
+	gpio_set_level(SX126x_RESET,1);
+	delay(10);
+	// ensure BUSY is low (state meachine ready)
+	WaitForIdle(BUSY_WAIT, "Reset", true);
 }
 
-void sx1262_setTxInfinitePreamble(void)
+
+void Wakeup(void)
 {
-  sx1262_write_command(0xD2, NULL, 0);
+	GetStatus();
 }
 
-void sx1262_setRegulatorMode(uint8_t mode)
+
+void SetStandby(uint8_t mode)
 {
-  sx1262_write_command(0x96, &mode, 1);
+	uint8_t data = mode;
+	WriteCommand(SX126X_CMD_SET_STANDBY, &data, 1); // 0x80
 }
 
-void sx1262_calibrate(uint8_t calibParam)
+
+uint8_t GetStatus(void)
 {
-  sx1262_write_command(0x89, &calibParam, 1);
+	uint8_t rv;
+	ReadCommand(SX126X_CMD_GET_STATUS, &rv, 1); // 0xC0
+	return rv;
 }
 
-void sx1262_calibrateImage(uint8_t freq1, uint8_t freq2)
+
+void SetDio3AsTcxoCtrl(float voltage, uint32_t delay)
 {
-  uint8_t payload[2] = {freq1, freq2};
-  sx1262_write_command(0x98, payload, 2);
+	uint8_t buf[4];
+
+	//buf[0] = tcxoVoltage & 0x07;
+	if(fabs(voltage - 1.6) <= 0.001) {
+		buf[0] = SX126X_DIO3_OUTPUT_1_6;
+	} else if(fabs(voltage - 1.7) <= 0.001) {
+		buf[0] = SX126X_DIO3_OUTPUT_1_7;
+	} else if(fabs(voltage - 1.8) <= 0.001) {
+		buf[0] = SX126X_DIO3_OUTPUT_1_8;
+	} else if(fabs(voltage - 2.2) <= 0.001) {
+		buf[0] = SX126X_DIO3_OUTPUT_2_2;
+	} else if(fabs(voltage - 2.4) <= 0.001) {
+		buf[0] = SX126X_DIO3_OUTPUT_2_4;
+	} else if(fabs(voltage - 2.7) <= 0.001) {
+		buf[0] = SX126X_DIO3_OUTPUT_2_7;
+	} else if(fabs(voltage - 3.0) <= 0.001) {
+		buf[0] = SX126X_DIO3_OUTPUT_3_0;
+	} else {
+		buf[0] = SX126X_DIO3_OUTPUT_3_3;
+	}
+
+	uint32_t delayValue = (float)delay / 15.625;
+	buf[1] = ( uint8_t )( ( delayValue >> 16 ) & 0xFF );
+	buf[2] = ( uint8_t )( ( delayValue >> 8 ) & 0xFF );
+	buf[3] = ( uint8_t )( delayValue & 0xFF );
+
+	WriteCommand(SX126X_CMD_SET_DIO3_AS_TCXO_CTRL, buf, 4); // 0x97
 }
 
-void sx1262_setRxTXFallbackMode(uint8_t fallbackMode)
+
+void Calibrate(uint8_t calibParam)
 {
-  sx1262_write_command(0x93, &fallbackMode, 1);
+	uint8_t data = calibParam;
+	WriteCommand(SX126X_CMD_CALIBRATE, &data, 1); // 0x89
 }
 
-// Write to register function
-void sx1262_writeRegister(uint16_t address, const uint8_t *data, size_t length)
+
+void SetDio2AsRfSwitchCtrl(uint8_t enable)
 {
-  uint8_t payload[length + 2];
-  payload[0] = (address >> 8) & 0xFF;
-  payload[1] = address & 0xFF;
-  for (size_t i = 0; i < length; i++)
-  {
-    payload[i + 2] = data[i];
-  }
-  sx1262_write_command(0x0D, payload, length + 2);
+	uint8_t data = enable;
+	WriteCommand(SX126X_CMD_SET_DIO2_AS_RF_SWITCH_CTRL, &data, 1); // 0x9D
 }
 
-// Read from register function
-void sx1262_readRegister(uint16_t address, uint8_t *data, size_t length)
+
+void SetRfFrequency(uint32_t frequency)
 {
-  uint8_t payload[2];
-  payload[0] = (address >> 8) & 0xFF;
-  payload[1] = address & 0xFF;
-  uint8_t rx_payload[2 + length];
-  sx1262_read_command(0x1D, payload, 2, rx_payload, 2 + length);
-  for (size_t i = 0; i < length; i++)
-  {
-    data[i] = rx_payload[i + 2];
-  }
+	uint8_t buf[4];
+	uint32_t freq = 0;
+
+	CalibrateImage(frequency);
+
+	freq = (uint32_t)((double)frequency / (double)FREQ_STEP);
+	buf[0] = (uint8_t)((freq >> 24) & 0xFF);
+	buf[1] = (uint8_t)((freq >> 16) & 0xFF);
+	buf[2] = (uint8_t)((freq >> 8) & 0xFF);
+	buf[3] = (uint8_t)(freq & 0xFF);
+	WriteCommand(SX126X_CMD_SET_RF_FREQUENCY, buf, 4); // 0x86
 }
 
-// Write to buffer function
-void sx1262_writeBuffer(uint8_t offset, const uint8_t *data, size_t length)
+
+void CalibrateImage(uint32_t frequency)
 {
-  uint8_t payload[length + 1];
-  payload[0] = offset;
-  for (size_t i = 0; i < length; i++)
-  {
-    payload[i + 1] = data[i];
-  }
-  sx1262_write_command(0x0E, payload, length + 1);
+	uint8_t calFreq[2];
+
+	if( frequency> 900000000 )
+	{
+		calFreq[0] = 0xE1;
+		calFreq[1] = 0xE9;
+	}
+	else if( frequency > 850000000 )
+	{
+		calFreq[0] = 0xD7;
+		calFreq[1] = 0xDB;
+	}
+	else if( frequency > 770000000 )
+	{
+		calFreq[0] = 0xC1;
+		calFreq[1] = 0xC5;
+	}
+	else if( frequency > 460000000 )
+	{
+		calFreq[0] = 0x75;
+		calFreq[1] = 0x81;
+	}
+	else if( frequency > 425000000 )
+	{
+		calFreq[0] = 0x6B;
+		calFreq[1] = 0x6F;
+	}
+	WriteCommand(SX126X_CMD_CALIBRATE_IMAGE, calFreq, 2); // 0x98
 }
 
-// Read from buffer function
-void sx1262_readBuffer(uint8_t offset, uint8_t *data, size_t length)
+
+void SetRegulatorMode(uint8_t mode)
 {
-  uint8_t tx_payload[1];
-  uint8_t payload[1 + length];
-  tx_payload[0] = offset;
-  sx1262_read_command(0x1E, tx_payload, 1, payload, 1 + length);
-  for (size_t i = 0; i < length; i++)
-  {
-    data[i] = payload[i + 1];
-  }
+	uint8_t data = mode;
+	WriteCommand(SX126X_CMD_SET_REGULATOR_MODE, &data, 1); // 0x96
 }
 
-void sx1262_setDioIrqParams(uint16_t irqMask, uint16_t dio1Mask, uint16_t dio2Mask, uint16_t dio3Mask)
+
+void SetBufferBaseAddress(uint8_t txBaseAddress, uint8_t rxBaseAddress)
 {
-  uint8_t payload[8] = {
-      (uint8_t)((irqMask >> 8) & 0xFF), (uint8_t)((irqMask) & 0xFF),
-      (uint8_t)((dio1Mask >> 8) & 0xFF), (uint8_t)(dio1Mask & 0xFF),
-      (uint8_t)((dio2Mask >> 8) & 0xFF), (uint8_t)(dio2Mask & 0xFF),
-      (uint8_t)((dio3Mask >> 8) & 0xFF), (uint8_t)(dio3Mask & 0xFF)};
-  sx1262_write_command(0x08, payload, 8);
+	uint8_t buf[2];
+
+	buf[0] = txBaseAddress;
+	buf[1] = rxBaseAddress;
+	WriteCommand(SX126X_CMD_SET_BUFFER_BASE_ADDRESS, buf, 2); // 0x8F
 }
 
-uint16_t sx1262_getIrqStatus(void)
+
+void SetPowerConfig(int8_t power, uint8_t rampTime)
 {
-  uint8_t response[2] = {0};
-  sx1262_read_command(0x12, NULL, 0, response, 2);
-  return (response[0] << 8) | response[1];
+	uint8_t buf[2];
+
+	if( power > 22 )
+	{
+		power = 22;
+	}
+	else if( power < -3 )
+	{
+		power = -3;
+	}
+		
+	buf[0] = power;
+	buf[1] = ( uint8_t )rampTime;
+	WriteCommand(SX126X_CMD_SET_TX_PARAMS, buf, 2); // 0x8E
 }
 
-void sx1262_clearIrqStatus(uint16_t clearIrqParam)
+
+void SetPaConfig(uint8_t paDutyCycle, uint8_t hpMax, uint8_t deviceSel, uint8_t paLut)
 {
-  uint8_t payload[2] = {
-      (uint8_t)((clearIrqParam >> 8) & 0xFF),
-      (uint8_t)(clearIrqParam & 0xFF)};
-  sx1262_write_command(0x02, payload, 2);
+	uint8_t buf[4];
+
+	buf[0] = paDutyCycle;
+	buf[1] = hpMax;
+	buf[2] = deviceSel;
+	buf[3] = paLut;
+	WriteCommand(SX126X_CMD_SET_PA_CONFIG, buf, 4); // 0x95
 }
 
-void sx1262_setDIO2AsRfSwitchCtrl(uint8_t enable)
+
+void SetOvercurrentProtection(float currentLimit)
 {
-  uint8_t buf[1];
-  buf[0] = enable;
-  sx1262_write_command(0x9D, buf, 1);
+	if((currentLimit >= 0.0) && (currentLimit <= 140.0)) {
+		uint8_t buf[1];
+		buf[0] = (uint8_t)(currentLimit / 2.5);
+		WriteRegister(SX126X_REG_OCP_CONFIGURATION, buf, 1); // 0x08E7
+	}
 }
 
-void sx1262_setDIO3AsTCXOCtrl(uint8_t tcxoVoltage, uint32_t delay)
-{
-  uint8_t payload[4] = {
-      tcxoVoltage,
-      (uint8_t)((delay >> 16) & 0xFF),
-      (uint8_t)((delay >> 8) & 0xFF),
-      (uint8_t)(delay & 0xFF)};
-  sx1262_write_command(0x97, payload, 4);
+void SetSyncWord(int16_t sync) {
+	uint8_t buf[2];
+
+	buf[0] = (uint8_t)((sync >> 8) & 0x00FF);
+	buf[1] = (uint8_t)(sync & 0x00FF);
+	WriteRegister(SX126X_REG_LORA_SYNC_WORD_MSB, buf, 2); // 0x0740
 }
 
-void sx1262_setFrequency(uint32_t frequency)
+void SetDioIrqParams
+( uint16_t irqMask, uint16_t dio1Mask, uint16_t dio2Mask, uint16_t dio3Mask )
 {
-  uint32_t freq_reg = frequency * FREQ_STEP;
-  uint8_t buffer[4] = {
-      (uint8_t)((freq_reg >> 24) & 0xFF),
-      (uint8_t)((freq_reg >> 16) & 0xFF),
-      (uint8_t)((freq_reg >> 8) & 0xFF),
-      (uint8_t)((freq_reg) & 0xFF)};
-  sx1262_write_command(0x86, buffer, 4);
+	uint8_t buf[8];
+
+	buf[0] = (uint8_t)((irqMask >> 8) & 0x00FF);
+	buf[1] = (uint8_t)(irqMask & 0x00FF);
+	buf[2] = (uint8_t)((dio1Mask >> 8) & 0x00FF);
+	buf[3] = (uint8_t)(dio1Mask & 0x00FF);
+	buf[4] = (uint8_t)((dio2Mask >> 8) & 0x00FF);
+	buf[5] = (uint8_t)(dio2Mask & 0x00FF);
+	buf[6] = (uint8_t)((dio3Mask >> 8) & 0x00FF);
+	buf[7] = (uint8_t)(dio3Mask & 0x00FF);
+	WriteCommand(SX126X_CMD_SET_DIO_IRQ_PARAMS, buf, 8); // 0x08
 }
 
-void sx1262_setPacketType(uint8_t packetType)
+
+void SetStopRxTimerOnPreambleDetect(bool enable)
 {
-  uint8_t data[1];
-  data[0] = packetType;
-  sx1262_write_command(0x8A, data, 1);
+	ESP_LOGI(TAG, "SetStopRxTimerOnPreambleDetect enable=%d", enable);
+	//uint8_t data = (uint8_t)enable;
+	uint8_t data = 0;
+	if (enable) data = 1;
+	WriteCommand(SX126X_CMD_STOP_TIMER_ON_PREAMBLE, &data, 1); // 0x9F
 }
 
-void sx1262_setLoRaModulationParams(const sx1262_LoRaModulationParams_t *params)
+
+void SetLoRaSymbNumTimeout(uint8_t SymbNum)
 {
-  uint8_t payload[4] = {
-      params->spreadingFactor,
-      params->bandwidth,
-      params->codingRate,
-      params->lowDataRateOpt};
-  sx1262_write_command(0x8B, payload, sizeof(payload));
+	uint8_t data = SymbNum;
+	WriteCommand(SX126X_CMD_SET_LORA_SYMB_NUM_TIMEOUT, &data, 1); // 0xA0
 }
 
-void sx1262_setGFSKModulationParams(const sx1262_GFSKModulationParams_t *params)
+
+void SetPacketType(uint8_t packetType)
 {
-  uint8_t payload[8] = {
-      (uint8_t)((params->bitRate >> 16) & 0xFF),
-      (uint8_t)((params->bitRate >> 8) & 0xFF),
-      (uint8_t)(params->bitRate & 0xFF),
-      params->pulseShape,
-      params->bandwidth,
-      (uint8_t)((params->frequencyDev >> 16) & 0xFF),
-      (uint8_t)((params->frequencyDev >> 8) & 0xFF),
-      (uint8_t)(params->frequencyDev & 0xFF)};
-  sx1262_write_command(0x8B, payload, sizeof(payload));
+	uint8_t data = packetType;
+	WriteCommand(SX126X_CMD_SET_PACKET_TYPE, &data, 1); // 0x01
 }
 
-uint8_t sx1262_getPacketType()
+
+void SetModulationParams(uint8_t spreadingFactor, uint8_t bandwidth, uint8_t codingRate, uint8_t lowDataRateOptimize)
 {
-  uint8_t response;
-  sx1262_read_command(0x11, NULL, 0, &response, 1);
-  return response;
+	uint8_t data[4];
+	//currently only LoRa supported
+	data[0] = spreadingFactor;
+	data[1] = bandwidth;
+	data[2] = codingRate;
+	data[3] = lowDataRateOptimize;
+	WriteCommand(SX126X_CMD_SET_MODULATION_PARAMS, data, 4); // 0x8B
 }
 
-void sx1262_configure_tx_power(uint8_t paDutyCycle, uint8_t hpMax, uint8_t paLut, int8_t power, uint8_t rampTime)
+
+void SetCadParams(uint8_t cadSymbolNum, uint8_t cadDetPeak, uint8_t cadDetMin, uint8_t cadExitMode, uint32_t cadTimeout)
 {
-  if (paDutyCycle > 0x04)
-    paDutyCycle = 0x04;
-  if (rampTime > 0x07)
-    rampTime = 0x07;
-  if (hpMax > 0x07)
-    hpMax = 0x07;
-
-  uint8_t deviceSel;
-  if (power >= -9)
-  {
-    deviceSel = 0x00; // High Power PA
-    if (power > 22)
-      power = 22;
-    if (power < -9)
-      power = -9;
-  }
-  else
-  {
-    deviceSel = 0x01; // Low Power PA
-    if (power > 14)
-      power = 14;
-    if (power < -17)
-      power = -17;
-  }
-
-  uint8_t paConfig[4] = {paDutyCycle, hpMax, deviceSel, paLut};
-  sx1262_write_command(0x95, paConfig, 4);
-
-  // Direct cast keeps correct 2's complement representation
-  uint8_t txPower[2] = {(uint8_t)power, rampTime};
-  sx1262_write_command(0x8E, txPower, 2);
+	uint8_t data[7];
+	data[0] = cadSymbolNum;
+	data[1] = cadDetPeak;
+	data[2] = cadDetMin;
+	data[3] = cadExitMode;
+	data[4] = (uint8_t)((cadTimeout >> 16) & 0xFF);
+	data[5] = (uint8_t)((cadTimeout >> 8) & 0xFF);
+	data[6] = (uint8_t)(cadTimeout & 0xFF);
+	WriteCommand(SX126X_CMD_SET_CAD_PARAMS, data, 7); // 0x88
 }
 
-void sx1262_setLoRaPacketParams(sx1262_LoRaPacketParams_t *params)
+
+void SetCad()
 {
-  uint8_t payload[6] = {
-      (uint8_t)((params->preambleLength >> 8) & 0xFF),
-      (uint8_t)(params->preambleLength & 0xFF),
-      params->headerType,
-      params->payloadLength,
-      params->crcType,
-      params->invertIQ};
-  sx1262_write_command(0x8C, payload, sizeof(payload));
+	uint8_t data = 0;
+	WriteCommand(SX126X_CMD_SET_CAD, &data, 0); // 0xC5
 }
 
-void sx1262_setCadParams(uint8_t cadSymbolNum, uint8_t cadDetPeak, uint8_t cadDetMin, uint8_t cadExitMode, uint32_t cadTimeout)
+
+uint16_t GetIrqStatus( void )
 {
-  uint8_t payload[7] = {
-      cadSymbolNum,
-      cadDetPeak,
-      cadDetMin,
-      cadExitMode,
-      (uint8_t)((cadTimeout >> 16) & 0xFF),
-      (uint8_t)((cadTimeout >> 8) & 0xFF),
-      (uint8_t)(cadTimeout & 0xFF)};
-  sx1262_write_command(0x88, payload, sizeof(payload));
+	uint8_t data[3];
+	ReadCommand(SX126X_CMD_GET_IRQ_STATUS, data, 3); // 0x12
+	return (data[1] << 8) | data[2];
 }
 
-void sx1262_setBufferBaseAddress(uint8_t txBaseAddr, uint8_t rxBaseAddr)
+
+void ClearIrqStatus(uint16_t irq)
 {
-  uint8_t payload[2] = {txBaseAddr, rxBaseAddr};
-  sx1262_write_command(0x8F, payload, sizeof(payload));
+	uint8_t buf[2];
+
+	buf[0] = (uint8_t)(((uint16_t)irq >> 8) & 0x00FF);
+	buf[1] = (uint8_t)((uint16_t)irq & 0x00FF);
+	WriteCommand(SX126X_CMD_CLEAR_IRQ_STATUS, buf, 2); // 0x02
 }
 
-void sx1262_setLoRaSymbNumTimeout(uint8_t symbNum)
+
+void SetRx(uint32_t timeout)
 {
-  sx1262_write_command(0xA0, &symbNum, 1);
+	if (debugPrint) {
+		ESP_LOGI(TAG, "----- SetRx timeout=%"PRIu32, timeout);
+	}
+	SetStandby(SX126X_STANDBY_RC);
+	uint8_t buf[3];
+	buf[0] = (uint8_t)((timeout >> 16) & 0xFF);
+	buf[1] = (uint8_t)((timeout >> 8) & 0xFF);
+	buf[2] = (uint8_t)(timeout & 0xFF);
+	WriteCommand(SX126X_CMD_SET_RX, buf, 3); // 0x82
+
+	for(int retry=0;retry<10;retry++) {
+		if ((GetStatus() & 0x70) == 0x50) break;
+		delay(1);
+	}
+	if ((GetStatus() & 0x70) != 0x50) {
+		ESP_LOGE(TAG, "SetRx Illegal Status");
+		LoRaError(ERR_INVALID_SETRX_STATE);
+	}
 }
 
-void sx1262_getStatus(uint8_t *status)
+
+void SetTx(uint32_t timeoutInMs)
 {
-  sx1262_read_command(0xC0, NULL, 0, status, 0);
+	if (debugPrint) {
+		ESP_LOGI(TAG, "----- SetTx timeoutInMs=%"PRIu32, timeoutInMs);
+	}
+	SetStandby(SX126X_STANDBY_RC);
+	uint8_t buf[3];
+	uint32_t tout = timeoutInMs;
+	if (timeoutInMs != 0) {
+		uint32_t timeoutInUs = timeoutInMs * 1000;
+		tout = (uint32_t)(timeoutInUs / 0.015625);
+	}
+	if (debugPrint) {
+		ESP_LOGI(TAG, "SetTx timeoutInMs=%"PRIu32" tout=%"PRIu32, timeoutInMs, tout);
+	}
+	buf[0] = (uint8_t)((tout >> 16) & 0xFF);
+	buf[1] = (uint8_t)((tout >> 8) & 0xFF);
+	buf[2] = (uint8_t )(tout & 0xFF);
+	WriteCommand(SX126X_CMD_SET_TX, buf, 3); // 0x83
+	
+	for(int retry=0;retry<10;retry++) {
+		if ((GetStatus() & 0x70) == 0x60) break;
+		vTaskDelay(1);
+	}
+	if ((GetStatus() & 0x70) != 0x60) {
+		ESP_LOGE(TAG, "SetTx Illegal Status");
+		LoRaError(ERR_INVALID_SETTX_STATE);
+	}
 }
 
-void sx1262_getRxBufferStatus(uint8_t *payloadLengthRx, uint8_t *rxStartBufferPointer)
+int GetPacketLost()
 {
-  uint8_t response[2];
-  sx1262_read_command(0x13, NULL, 0, response, sizeof(response));
-  *payloadLengthRx = response[0];
-  *rxStartBufferPointer = response[1];
+	return txLost;
 }
 
-void sx1262_getPacketStatus(uint8_t *rssi, uint8_t *snr, uint8_t *signalRssi)
+
+uint8_t GetRssiInst()
 {
-  uint8_t response[3];
-  sx1262_read_command(0x14, NULL, 0, response, sizeof(response));
-  *rssi = response[0];
-  *snr = response[1];
-  *signalRssi = response[2];
+	uint8_t buf[2];
+	ReadCommand( SX126X_CMD_GET_RSSI_INST, buf, 2 ); // 0x15
+	return buf[1];
 }
 
-uint8_t sx1262_getRssiInst(uint8_t *rssiInst)
+
+void GetRxBufferStatus(uint8_t *payloadLength, uint8_t *rxStartBufferPointer)
 {
-  uint8_t response;
-  sx1262_read_command(0x15, NULL, 0, &response, 1);
-  return response;
+	uint8_t buf[3];
+	ReadCommand( SX126X_CMD_GET_RX_BUFFER_STATUS, buf, 3 ); // 0x13
+	*payloadLength = buf[1];
+	*rxStartBufferPointer = buf[2];
 }
 
-void sx1262_getStats(uint16_t *pktReceived, uint16_t *pktCrcError, uint16_t *pktHeaderErr)
-{
-  uint8_t response[6];
-  sx1262_read_command(0x10, NULL, 0, response, sizeof(response));
-  *pktReceived = (response[0] << 8) | response[1];
-  *pktCrcError = (response[2] << 8) | response[3];
-  *pktHeaderErr = (response[4] << 8) | response[5];
+
+void WaitForIdleBegin(unsigned long timeout, char *text) {
+	// ensure BUSY is low (state meachine ready)
+	bool stop = false;
+	for (int retry=0;retry<10;retry++) {
+		if (retry == 9) stop = true;
+		bool ret = WaitForIdle(BUSY_WAIT, text, stop);
+		if (ret == true) break;
+		ESP_LOGW(TAG, "WaitForIdle fail retry=%d", retry);
+		vTaskDelay(1);
+	}
 }
 
-void sx1262_resetStats(void)
+
+bool WaitForIdle(unsigned long timeout, char *text, bool stop)
 {
-  uint8_t payload[6] = {0};
-  sx1262_write_command(0x00, payload, sizeof(payload));
+	bool ret = true;
+	TickType_t start = xTaskGetTickCount();
+	delayMicroseconds(1);
+	while(xTaskGetTickCount() - start < (timeout/portTICK_PERIOD_MS)) {
+		if (gpio_get_level(SX126x_BUSY) == 0) break;
+		delayMicroseconds(1);
+	}
+	if (gpio_get_level(SX126x_BUSY)) {
+		if (stop) {
+			ESP_LOGE(TAG, "WaitForIdle Timeout text=%s timeout=%lu start=%"PRIu32, text, timeout, start);
+			LoRaError(ERR_IDLE_TIMEOUT);
+		} else {
+			ESP_LOGW(TAG, "WaitForIdle Timeout text=%s timeout=%lu start=%"PRIu32, text, timeout, start);
+			ret = false;
+		}
+	}
+	return ret;
 }
 
-uint16_t sx1262_getDeviceErrors()
+
+uint8_t ReadBuffer(uint8_t *rxData, int16_t rxDataLen)
 {
-  uint8_t response[2];
-  sx1262_read_command(0x17, NULL, 0, response, sizeof(response));
-  return (((uint16_t)response[0]) << 8) | (uint16_t)response[1];
+	uint8_t offset = 0;
+	uint8_t payloadLength = 0;
+	GetRxBufferStatus(&payloadLength, &offset);
+	if( payloadLength > rxDataLen )
+	{
+		ESP_LOGW(TAG, "ReadBuffer rxDataLen too small. payloadLength=%d rxDataLen=%d", payloadLength, rxDataLen);
+		return 0;
+	}
+
+	// ensure BUSY is low (state meachine ready)
+	WaitForIdle(BUSY_WAIT, "start ReadBuffer", true);
+
+	// start transfer
+	uint8_t *buf;
+	buf = malloc(payloadLength+3);
+	if (buf != NULL) {
+		buf[0] = SX126X_CMD_READ_BUFFER; // 0x1E
+		buf[1] = offset; // offset in rx fifo
+		buf[2] = SX126X_CMD_NOP;
+		memset(&buf[3], SX126X_CMD_NOP, payloadLength);
+		spi_read_byte(buf, buf, payloadLength+3);
+		memcpy(rxData, &buf[3], payloadLength);
+		free(buf);
+	} else {
+		ESP_LOGE(TAG, "ReadBuffer malloc fail");
+		payloadLength = 0;
+	}
+
+	// wait for BUSY to go low
+	WaitForIdle(BUSY_WAIT, "end ReadBuffer", false);
+
+	return payloadLength;
 }
 
-void sx1262_clearDeviceErrors(void)
+
+void WriteBuffer(uint8_t *txData, int16_t txDataLen)
 {
-  uint8_t payload[2] = {0x00, 0x00};
-  sx1262_write_command(0x07, payload, sizeof(payload));
+	// ensure BUSY is low (state meachine ready)
+	WaitForIdle(BUSY_WAIT, "start WriteBuffer", true);
+
+	// start transfer
+	uint8_t *buf;
+	buf = malloc(txDataLen+2);
+	if (buf != NULL) {
+		buf[0] = SX126X_CMD_WRITE_BUFFER; // 0x0E
+		buf[1] = 0; // offset in tx fifo
+		memcpy(&buf[2], txData, txDataLen);
+		spi_write_byte(buf, txDataLen+2);
+		free(buf);
+	} else {
+		ESP_LOGE(TAG, "WriteBuffer malloc fail");
+	}
+
+	// wait for BUSY to go low
+	WaitForIdle(BUSY_WAIT, "end WriteBuffer", false);
 }
+
+
+void WriteRegister(uint16_t reg, uint8_t* data, uint8_t numBytes) {
+	// ensure BUSY is low (state meachine ready)
+	WaitForIdle(BUSY_WAIT, "start WriteRegister", true);
+
+	if(debugPrint) {
+		ESP_LOGI(TAG, "WriteRegister: REG=0x%02x", reg);
+		for(uint8_t n = 0; n < numBytes; n++) {
+			ESP_LOGI(TAG, "DataOut:%02x ", data[n]);
+		}
+	}
+
+	// start transfer
+	uint8_t buf[16];
+	buf[0] = SX126X_CMD_WRITE_REGISTER;
+	buf[1] = (reg & 0xFF00) >> 8;
+	buf[2] = reg & 0xff;
+	memcpy(&buf[3], data, numBytes);
+	spi_write_byte(buf, 3 + numBytes);
+
+	// wait for BUSY to go low
+	WaitForIdle(BUSY_WAIT, "end WriteRegister", false);
+}
+
+
+void ReadRegister(uint16_t reg, uint8_t* data, uint8_t numBytes) {
+	// ensure BUSY is low (state meachine ready)
+	WaitForIdle(BUSY_WAIT, "start ReadRegister", true);
+
+	if(debugPrint) {
+		ESP_LOGI(TAG, "ReadRegister: REG=0x%02x", reg);
+	}
+
+	// start transfer
+	uint8_t buf[16];
+	memset(buf, SX126X_CMD_NOP, sizeof(buf));
+	buf[0] = SX126X_CMD_READ_REGISTER;
+	buf[1] = (reg & 0xFF00) >> 8;
+	buf[2] = reg & 0xff;
+	spi_read_byte(buf, buf, 4 + numBytes);
+	memcpy(data, &buf[4], numBytes);
+	if(debugPrint) {
+		for(uint8_t n = 0; n < numBytes; n++) {
+			ESP_LOGI(TAG, "DataIn:%02x ", data[n]);
+		}
+	}
+
+	// wait for BUSY to go low
+	WaitForIdle(BUSY_WAIT, "end ReadRegister", false);
+}
+
+// WriteCommand with retry
+void WriteCommand(uint8_t cmd, uint8_t* data, uint8_t numBytes) {
+	uint8_t status;
+	for (int retry=1; retry<10; retry++) {
+		status = WriteCommand2(cmd, data, numBytes);
+		ESP_LOGD(TAG, "status=%02x", status);
+		if (status == 0) break;
+		ESP_LOGW(TAG, "WriteCommand2 status=%02x retry=%d", status, retry);
+	}
+	if (status != 0) {
+		ESP_LOGE(TAG, "SPI Transaction error:0x%02x", status);
+		LoRaError(ERR_SPI_TRANSACTION);
+	}
+}
+
+uint8_t WriteCommand2(uint8_t cmd, uint8_t* data, uint8_t numBytes) {
+	// ensure BUSY is low (state meachine ready)
+	WaitForIdle(BUSY_WAIT, "start WriteCommand2", true);
+
+	if(debugPrint) {
+		ESP_LOGI(TAG, "WriteCommand: CMD=0x%02x", cmd);
+	}
+
+	// start transfer
+	uint8_t buf[16];
+	buf[0] = cmd;
+	memcpy(&buf[1], data, numBytes);
+	spi_read_byte(buf, buf, numBytes + 1);
+
+	uint8_t status = 0;
+	uint8_t cmd_status = buf[1] & 0xe;
+
+	switch(cmd_status){
+		case SX126X_STATUS_CMD_TIMEOUT:
+		case SX126X_STATUS_CMD_INVALID:
+		case SX126X_STATUS_CMD_FAILED:
+			status = cmd_status;
+			break;
+
+		case 0:
+		case 7:
+			status = SX126X_STATUS_SPI_FAILED;
+			break;
+			// default: break; // success
+	}
+
+	// wait for BUSY to go low
+	WaitForIdle(BUSY_WAIT, "end WriteCommand2", false);
+	return status;
+}
+
+
+void ReadCommand(uint8_t cmd, uint8_t* data, uint8_t numBytes) {
+	// ensure BUSY is low (state meachine ready)
+	WaitForIdleBegin(BUSY_WAIT, "start ReadCommand");
+
+	if(debugPrint) {
+		ESP_LOGI(TAG, "ReadCommand: CMD=0x%02x", cmd);
+	}
+
+	// start transfer
+	uint8_t buf[16];
+	memset(buf, SX126X_CMD_NOP, sizeof(buf));
+	buf[0] = cmd;
+	spi_read_byte(buf, buf, 1 + numBytes);
+	if (data != NULL && numBytes)
+		memcpy(data, &buf[1], numBytes);
+
+	// wait for BUSY to go low
+	vTaskDelay(1);
+	WaitForIdle(BUSY_WAIT, "end ReadCommand", false);
+}
\ No newline at end of file
diff --git a/main/hw/sx1262.h b/main/hw/sx1262.h
index f43ee5e..08ad225 100644
--- a/main/hw/sx1262.h
+++ b/main/hw/sx1262.h
@@ -1,330 +1,434 @@
-#ifndef SX1262_H
-#define SX1262_H
+#ifndef _RA01S_H
+#define _RA01S_H
 
 #include "driver/spi_master.h"
-#include "driver/gpio.h"
-#include <stdint.h>
-#include "buscfg.h"
-#include "mcp23018.h"
 
-#include <string.h>
+//return values
+#define ERR_NONE                        0
+#define ERR_PACKET_TOO_LONG             1
+#define ERR_UNKNOWN                     2
+#define ERR_TX_TIMEOUT                  3
+#define ERR_RX_TIMEOUT                  4
+#define ERR_CRC_MISMATCH                5
+#define ERR_WRONG_MODEM                 6
+#define ERR_INVALID_BANDWIDTH           7
+#define ERR_INVALID_SPREADING_FACTOR    8
+#define ERR_INVALID_CODING_RATE         9
+#define ERR_INVALID_FREQUENCY_DEVIATION 10
+#define ERR_INVALID_BIT_RATE            11
+#define ERR_INVALID_RX_BANDWIDTH        12
+#define ERR_INVALID_DATA_SHAPING        13
+#define ERR_INVALID_SYNC_WORD           14
+#define ERR_INVALID_OUTPUT_POWER        15
+#define ERR_INVALID_MODE                16
+#define ERR_INVALID_TRANCEIVER          17
+#define ERR_INVALID_SETRX_STATE         18
+#define ERR_INVALID_SETTX_STATE         19
+#define ERR_IDLE_TIMEOUT                20
+#define ERR_SPI_TRANSACTION             21
 
-#include "esp_task.h"
+// SX126X physical layer properties
+#define XTAL_FREQ                       ( double )32000000
+#define FREQ_DIV                        ( double )pow( 2.0, 25.0 )
+#define FREQ_STEP                       ( double )( XTAL_FREQ / FREQ_DIV )
 
-#define TAG "SX1262"
+#define LOW                             0
+#define HIGH                            1
+#define BUSY_WAIT                       5000
 
-#define LOG_LOCAL_LEVEL ESP_LOG_DEBUG
-#include "esp_log.h"
+// SX126X Model
+#define SX1261_TRANCEIVER                             0x01
+#define SX1262_TRANCEIVER                             0x02
+#define SX1268_TRANCEIVER                             0x08
 
-#define LOG_LOCAL_LEVEL ESP_LOG_DEBUG
+// SX126X SPI commands
+// operational modes commands
+#define SX126X_CMD_NOP                                0x00
+#define SX126X_CMD_SET_SLEEP                          0x84
+#define SX126X_CMD_SET_STANDBY                        0x80
+#define SX126X_CMD_SET_FS                             0xC1
+#define SX126X_CMD_SET_TX                             0x83
+#define SX126X_CMD_SET_RX                             0x82
+#define SX126X_CMD_STOP_TIMER_ON_PREAMBLE             0x9F
+#define SX126X_CMD_SET_RX_DUTY_CYCLE                  0x94
+#define SX126X_CMD_SET_CAD                            0xC5
+#define SX126X_CMD_SET_TX_CONTINUOUS_WAVE             0xD1
+#define SX126X_CMD_SET_TX_INFINITE_PREAMBLE           0xD2
+#define SX126X_CMD_SET_REGULATOR_MODE                 0x96
+#define SX126X_CMD_CALIBRATE                          0x89
+#define SX126X_CMD_CALIBRATE_IMAGE                    0x98
+#define SX126X_CMD_SET_PA_CONFIG                      0x95
+#define SX126X_CMD_SET_RX_TX_FALLBACK_MODE            0x93
 
-#define LORA_SPI_HOST SPI3_HOST
+// register and buffer access commands
+#define SX126X_CMD_WRITE_REGISTER                     0x0D
+#define SX126X_CMD_READ_REGISTER                      0x1D
+#define SX126X_CMD_WRITE_BUFFER                       0x0E
+#define SX126X_CMD_READ_BUFFER                        0x1E
 
-extern spi_device_handle_t spi;
+// DIO and IRQ control
+#define SX126X_CMD_SET_DIO_IRQ_PARAMS                 0x08
+#define SX126X_CMD_GET_IRQ_STATUS                     0x12
+#define SX126X_CMD_CLEAR_IRQ_STATUS                   0x02
+#define SX126X_CMD_SET_DIO2_AS_RF_SWITCH_CTRL         0x9D
+#define SX126X_CMD_SET_DIO3_AS_TCXO_CTRL              0x97
 
-typedef struct
-{
-    uint8_t status;
-    uint16_t error;
-} sx1262_status_t;
+// RF, modulation and packet commands
+#define SX126X_CMD_SET_RF_FREQUENCY                   0x86
+#define SX126X_CMD_SET_PACKET_TYPE                    0x8A
+#define SX126X_CMD_GET_PACKET_TYPE                    0x11
+#define SX126X_CMD_SET_TX_PARAMS                      0x8E
+#define SX126X_CMD_SET_MODULATION_PARAMS              0x8B
+#define SX126X_CMD_SET_PACKET_PARAMS                  0x8C
+#define SX126X_CMD_SET_CAD_PARAMS                     0x88
+#define SX126X_CMD_SET_BUFFER_BASE_ADDRESS            0x8F
+#define SX126X_CMD_SET_LORA_SYMB_NUM_TIMEOUT          0xA0
 
-typedef struct
-{
-    uint16_t preambleLength;
-    uint8_t headerType;
-    uint8_t payloadLength;
-    uint8_t crcType;
-    uint8_t invertIQ;
-} sx1262_LoRaPacketParams_t;
+#define SX126X_PA_CONFIG_SX1261                       0x01
+#define SX126X_PA_CONFIG_SX1262                       0x00
 
-#define SX126X_DIOX_OUTPUT_ENABLE 0x0580
-#define SX126X_DIOX_INPUT_ENABLE 0x0583
-#define SX126X_DIOX_PULL_UP_CONTROL 0x0584
-#define SX126X_DIOX_PULL_DOWN_CONTROL 0x0585
+// status commands
+#define SX126X_CMD_GET_STATUS                         0xC0
+#define SX126X_CMD_GET_RSSI_INST                      0x15
+#define SX126X_CMD_GET_RX_BUFFER_STATUS               0x13
+#define SX126X_CMD_GET_PACKET_STATUS                  0x14
+#define SX126X_CMD_GET_DEVICE_ERRORS                  0x17
+#define SX126X_CMD_CLEAR_DEVICE_ERRORS                0x07
+#define SX126X_CMD_GET_STATS                          0x10
+#define SX126X_CMD_RESET_STATS                        0x00
 
-#define SX126X_WHITENING_INIT_MSB 0x06B8
-#define SX126X_WHITENING_INIT_LSB 0x06B9
 
-#define SX126X_CRC_INIT_MSB 0x06BC
-#define SX126X_CRC_INIT_LSB 0x06BD
+// SX126X register map
+#define SX126X_REG_HOPPING_ENABLE                     0x0385
+#define SX126X_REG_PACKECT_LENGTH                     0x0386
+#define SX126X_REG_NB_HOPPING_BLOCKS                  0x0387
+#define SX126X_REG_NB_SYMBOLS0                        0x0388
+#define SX126X_REG_FREQ0                              0x038A
+#define SX126X_REG_NB_SYMBOLS15                       0x03E2
+#define SX126X_REG_FREQ15                             0x03E4
+#define SX126X_REG_DIOX_OUTPUT_ENABLE                 0x0580
+#define SX126X_REG_DIOX_INPUT_ENABLE                  0x0583
+#define SX126X_REG_DIOX_PILL_UP_CONTROL               0x0584
+#define SX126X_REG_DIOX_PULL_DOWN_CONTROL             0x0585
+#define SX126X_REG_WHITENING_INITIAL_MSB              0x06B8
+#define SX126X_REG_WHITENING_INITIAL_LSB              0x06B9
+#define SX126X_REG_CRC_INITIAL_MSB                    0x06BC
+#define SX126X_REG_CRC_INITIAL_LSB                    0x06BD
+#define SX126X_REG_CRC_POLYNOMIAL_MSB                 0x06BE
+#define SX126X_REG_CRC_POLYNOMIAL_LSB                 0x06BF
+#define SX126X_REG_SYNC_WORD_0                        0x06C0
+#define SX126X_REG_SYNC_WORD_1                        0x06C1
+#define SX126X_REG_SYNC_WORD_2                        0x06C2
+#define SX126X_REG_SYNC_WORD_3                        0x06C3
+#define SX126X_REG_SYNC_WORD_4                        0x06C4
+#define SX126X_REG_SYNC_WORD_5                        0x06C5
+#define SX126X_REG_SYNC_WORD_6                        0x06C6
+#define SX126X_REG_SYNC_WORD_7                        0x06C7
+#define SX126X_REG_NODE_ADDRESS                       0x06CD
+#define SX126X_REG_BROADCAST_ADDRESS                  0x06CE
+#define SX126X_REG_IQ_POLARITY_SETUP                  0x0736
+#define SX126X_REG_LORA_SYNC_WORD_MSB                 0x0740
+#define SX126X_REG_LORA_SYNC_WORD_LSB                 0x0741
+#define SX126X_REG_RANDOM_NUMBER_0                    0x0819
+#define SX126X_REG_RANDOM_NUMBER_1                    0x081A
+#define SX126X_REG_RANDOM_NUMBER_2                    0x081B
+#define SX126X_REG_RANDOM_NUMBER_3                    0x081C
+#define SX126X_REG_TX_MODULETION                      0x0889
+#define SX126X_REG_RX_GAIN                            0x08AC
+#define SX126X_REG_TX_CLAMP_CONFIG                    0x08D8
+#define SX126X_REG_OCP_CONFIGURATION                  0x08E7
+#define SX126X_REG_RTC_CONTROL                        0x0902
+#define SX126X_REG_XTA_TRIM                           0x0911
+#define SX126X_REG_XTB_TRIM                           0x0912
+#define SX126X_REG_DIO3_OUTPUT_VOLTAGE_CONTROL        0x0920
+#define SX126X_REG_EVENT_MASK                         0x0944
 
-#define SX126X_CRC_POLY_MSB 0x06BE
-#define SX126X_CRC_POLY_LSB 0x06BF
 
-#define SX126X_SYNCWORD_0 0x06C0
-#define SX126X_SYNCWORD_1 0x06C1
-#define SX126X_SYNCWORD_2 0x06C2
-#define SX126X_SYNCWORD_3 0x06C3
-#define SX126X_SYNCWORD_4 0x06C4
-#define SX126X_SYNCWORD_5 0x06C5
-#define SX126X_SYNCWORD_6 0x06C6
-#define SX126X_SYNCWORD_7 0x06C7
+// SX126X SPI command variables
+//SX126X_CMD_SET_SLEEP
+#define SX126X_SLEEP_START_COLD                       0b00000000  //  2     2     sleep mode: cold start, configuration is lost (default)
+#define SX126X_SLEEP_START_WARM                       0b00000100  //  2     2                 warm start, configuration is retained
+#define SX126X_SLEEP_RTC_OFF                          0b00000000  //  0     0     wake on RTC timeout: disabled
+#define SX126X_SLEEP_RTC_ON                           0b00000001  //  0     0                          enabled
 
-#define SX126X_NODE_ADDRESS 0x06CD
-#define SX126X_BROADCAST_ADDRESS 0x06CE
+//SX126X_CMD_SET_STANDBY
+#define SX126X_STANDBY_RC                             0x00        //  7     0     standby mode: 13 MHz RC oscillator
+#define SX126X_STANDBY_XOSC                           0x01        //  7     0                   32 MHz crystal oscillator
 
-#define SX126X_IQ_POLARITY_SETUP 0x0736
+//SX126X_CMD_SET_RX
+#define SX126X_RX_TIMEOUT_NONE                        0x000000    //  23    0     Rx timeout duration: no timeout (Rx single mode)
+#define SX126X_RX_TIMEOUT_INF                         0xFFFFFF    //  23    0                          infinite (Rx continuous mode)
 
-#define SX126X_LORA_SYNCWORD_MSB 0x0740
-#define SX126X_LORA_SYNCWORD_LSB 0x0741
+//SX126X_CMD_STOP_TIMER_ON_PREAMBLE
+#define SX126X_STOP_ON_PREAMBLE_OFF                   0x00        //  7     0     stop timer on: sync word or header (default)
+#define SX126X_STOP_ON_PREAMBLE_ON                    0x01        //  7     0                    preamble detection
 
-#define SX126X_RANDOM_NUMBER_0 0x0819
-#define SX126X_RANDOM_NUMBER_1 0x081A
-#define SX126X_RANDOM_NUMBER_2 0x081B
-#define SX126X_RANDOM_NUMBER_3 0x081C
+//SX126X_CMD_SET_REGULATOR_MODE
+#define SX126X_REGULATOR_LDO                          0x00        //  7     0     set regulator mode: LDO (default)
+#define SX126X_REGULATOR_DC_DC                        0x01        //  7     0                         DC-DC
 
-#define SX126X_TX_MODULATION 0x0889
-#define SX126X_RX_GAIN 0x08AC
+//SX126X_CMD_CALIBRATE
+#define SX126X_CALIBRATE_IMAGE_OFF                    0b00000000  //  6     6     image calibration: disabled
+#define SX126X_CALIBRATE_IMAGE_ON                     0b01000000  //  6     6                        enabled
+#define SX126X_CALIBRATE_ADC_BULK_P_OFF               0b00000000  //  5     5     ADC bulk P calibration: disabled
+#define SX126X_CALIBRATE_ADC_BULK_P_ON                0b00100000  //  5     5                             enabled
+#define SX126X_CALIBRATE_ADC_BULK_N_OFF               0b00000000  //  4     4     ADC bulk N calibration: disabled
+#define SX126X_CALIBRATE_ADC_BULK_N_ON                0b00010000  //  4     4                             enabled
+#define SX126X_CALIBRATE_ADC_PULSE_OFF                0b00000000  //  3     3     ADC pulse calibration: disabled
+#define SX126X_CALIBRATE_ADC_PULSE_ON                 0b00001000  //  3     3                            enabled
+#define SX126X_CALIBRATE_PLL_OFF                      0b00000000  //  2     2     PLL calibration: disabled
+#define SX126X_CALIBRATE_PLL_ON                       0b00000100  //  2     2                      enabled
+#define SX126X_CALIBRATE_RC13M_OFF                    0b00000000  //  1     1     13 MHz RC osc. calibration: disabled
+#define SX126X_CALIBRATE_RC13M_ON                     0b00000010  //  1     1                                 enabled
+#define SX126X_CALIBRATE_RC64K_OFF                    0b00000000  //  0     0     64 kHz RC osc. calibration: disabled
+#define SX126X_CALIBRATE_RC64K_ON                     0b00000001  //  0     0                                 enabled
 
-#define SX126X_TX_CLAMP_CONFIG 0x08D8
-#define SX126X_OCP_CONFIGURATION 0x08E7
+//SX126X_CMD_CALIBRATE_IMAGE
+#define SX126X_CAL_IMG_430_MHZ_1                      0x6B
+#define SX126X_CAL_IMG_430_MHZ_2                      0x6F
+#define SX126X_CAL_IMG_470_MHZ_1                      0x75
+#define SX126X_CAL_IMG_470_MHZ_2                      0x81
+#define SX126X_CAL_IMG_779_MHZ_1                      0xC1
+#define SX126X_CAL_IMG_779_MHZ_2                      0xC5
+#define SX126X_CAL_IMG_863_MHZ_1                      0xD7
+#define SX126X_CAL_IMG_863_MHZ_2                      0xDB
+#define SX126X_CAL_IMG_902_MHZ_1                      0xE1
+#define SX126X_CAL_IMG_902_MHZ_2                      0xE9
 
-#define SX126X_RTC_CONTROL 0x0902
+//SX126X_CMD_SET_PA_CONFIG
+#define SX126X_PA_CONFIG_HP_MAX                       0x07
+#define SX126X_PA_CONFIG_SX1268                       0x01
+#define SX126X_PA_CONFIG_PA_LUT                       0x01
 
-#define SX126X_XTA_TRIM 0x0911
-#define SX126X_XTB_TRIM 0x0912
+//SX126X_CMD_SET_RX_TX_FALLBACK_MODE
+#define SX126X_RX_TX_FALLBACK_MODE_FS                 0x40        //  7     0     after Rx/Tx go to: FS mode
+#define SX126X_RX_TX_FALLBACK_MODE_STDBY_XOSC         0x30        //  7     0                        standby with crystal oscillator
+#define SX126X_RX_TX_FALLBACK_MODE_STDBY_RC           0x20        //  7     0                        standby with RC oscillator (default)
 
-#define SX126X_DIO3_OUTPUT_VOLTAGE 0x0920
-#define SX126X_EVENT_MASK 0x0944
+//SX126X_CMD_SET_DIO_IRQ_PARAMS
+#define SX126X_IRQ_TIMEOUT                            0b1000000000  //  9     9     Rx or Tx timeout
+#define SX126X_IRQ_CAD_DETECTED                       0b0100000000  //  8     8     channel activity detected
+#define SX126X_IRQ_CAD_DONE                           0b0010000000  //  7     7     channel activity detection finished
+#define SX126X_IRQ_CRC_ERR                            0b0001000000  //  6     6     wrong CRC received
+#define SX126X_IRQ_HEADER_ERR                         0b0000100000  //  5     5     LoRa header CRC error
+#define SX126X_IRQ_HEADER_VALID                       0b0000010000  //  4     4     valid LoRa header received
+#define SX126X_IRQ_SYNC_WORD_VALID                    0b0000001000  //  3     3     valid sync word detected
+#define SX126X_IRQ_PREAMBLE_DETECTED                  0b0000000100  //  2     2     preamble detected
+#define SX126X_IRQ_RX_DONE                            0b0000000010  //  1     1     packet received
+#define SX126X_IRQ_TX_DONE                            0b0000000001  //  0     0     packet transmission completed
+#define SX126X_IRQ_ALL                                0b1111111111  //  9     0     all interrupts
+#define SX126X_IRQ_NONE                               0b0000000000  //  9     0     no interrupts
 
-// Default values where applicable
-#define SX126X_RX_GAIN_POWER_SAVING 0x94
-#define SX126X_RX_GAIN_BOOSTED 0x96
+//SX126X_CMD_SET_DIO2_AS_RF_SWITCH_CTRL
+#define SX126X_DIO2_AS_IRQ                            0x00        //  7     0     DIO2 configuration: IRQ
+#define SX126X_DIO2_AS_RF_SWITCH                      0x01        //  7     0                         RF switch control
 
-#define SX126X_LORA_SYNCWORD_PUBLIC 0x3444
-#define SX126X_LORA_SYNCWORD_PRIVATE 0x1424
+//SX126X_CMD_SET_DIO3_AS_TCXO_CTRL
+#define SX126X_DIO3_OUTPUT_1_6                        0x00        //  7     0     DIO3 voltage output for TCXO: 1.6 V
+#define SX126X_DIO3_OUTPUT_1_7                        0x01        //  7     0                                   1.7 V
+#define SX126X_DIO3_OUTPUT_1_8                        0x02        //  7     0                                   1.8 V
+#define SX126X_DIO3_OUTPUT_2_2                        0x03        //  7     0                                   2.2 V
+#define SX126X_DIO3_OUTPUT_2_4                        0x04        //  7     0                                   2.4 V
+#define SX126X_DIO3_OUTPUT_2_7                        0x05        //  7     0                                   2.7 V
+#define SX126X_DIO3_OUTPUT_3_0                        0x06        //  7     0                                   3.0 V
+#define SX126X_DIO3_OUTPUT_3_3                        0x07        //  7     0                                   3.3 V
 
-#define SX126X_OCP_LEVEL_SX1262 0x38 // 140mA
-#define SX126X_OCP_LEVEL_SX1261 0x18 // 60mA
+//Radio complete Wake-up Time with TCXO stabilisation time
+#define RADIO_TCXO_SETUP_TIME                         5000        // [us]
 
-typedef struct
-{
-    uint8_t spreadingFactor; // LoRa SF: 0x05 (SF5) to 0x0C (SF12)
-    uint8_t bandwidth;       // LoRa BW: 0x00 (7.81 kHz) to 0x06 (500 kHz)
-    uint8_t codingRate;      // LoRa CR: 0x01 (4/5) to 0x04 (4/8)
-    uint8_t lowDataRateOpt;  // LDRO: 0x00 (disabled) or 0x01 (enabled)
-} sx1262_LoRaModulationParams_t;
+//SX126X_CMD_SET_PACKET_TYPE
+#define SX126X_PACKET_TYPE_GFSK                       0x00        //  7     0     packet type: GFSK
+#define SX126X_PACKET_TYPE_LORA                       0x01        //  7     0                  LoRa
 
-typedef struct
-{
-    uint32_t bitRate;      // GFSK bitrate (BR), calculated as 32 * Fxtal / BR
-    uint8_t pulseShape;    // GFSK filter: 0x00 (none) to 0x0B (Gaussian BT 1)
-    uint8_t bandwidth;     // GFSK RX bandwidth: 0x1F (4.8 kHz) to 0x09 (467 kHz)
-    uint32_t frequencyDev; // GFSK frequency deviation (Fdev)
-} sx1262_GFSKModulationParams_t;
+//SX126X_CMD_SET_TX_PARAMS
+#define SX126X_PA_RAMP_10U                            0x00        //  7     0     ramp time: 10 us
+#define SX126X_PA_RAMP_20U                            0x01        //  7     0                20 us
+#define SX126X_PA_RAMP_40U                            0x02        //  7     0                40 us
+#define SX126X_PA_RAMP_80U                            0x03        //  7     0                80 us
+#define SX126X_PA_RAMP_200U                           0x04        //  7     0                200 us
+#define SX126X_PA_RAMP_800U                           0x05        //  7     0                800 us
+#define SX126X_PA_RAMP_1700U                          0x06        //  7     0                1700 us
+#define SX126X_PA_RAMP_3400U                          0x07        //  7     0                3400 us
 
-#include <math.h>
+//SX126X_CMD_SET_MODULATION_PARAMS
+#define SX126X_GFSK_FILTER_NONE                       0x00        //  7     0     GFSK filter: none
+#define SX126X_GFSK_FILTER_GAUSS_0_3                  0x08        //  7     0                  Gaussian, BT = 0.3
+#define SX126X_GFSK_FILTER_GAUSS_0_5                  0x09        //  7     0                  Gaussian, BT = 0.5
+#define SX126X_GFSK_FILTER_GAUSS_0_7                  0x0A        //  7     0                  Gaussian, BT = 0.7
+#define SX126X_GFSK_FILTER_GAUSS_1                    0x0B        //  7     0                  Gaussian, BT = 1
+#define SX126X_GFSK_RX_BW_4_8                         0x1F        //  7     0     GFSK Rx bandwidth: 4.8 kHz
+#define SX126X_GFSK_RX_BW_5_8                         0x17        //  7     0                        5.8 kHz
+#define SX126X_GFSK_RX_BW_7_3                         0x0F        //  7     0                        7.3 kHz
+#define SX126X_GFSK_RX_BW_9_7                         0x1E        //  7     0                        9.7 kHz
+#define SX126X_GFSK_RX_BW_11_7                        0x16        //  7     0                        11.7 kHz
+#define SX126X_GFSK_RX_BW_14_6                        0x0E        //  7     0                        14.6 kHz
+#define SX126X_GFSK_RX_BW_19_5                        0x1D        //  7     0                        19.5 kHz
+#define SX126X_GFSK_RX_BW_23_4                        0x15        //  7     0                        23.4 kHz
+#define SX126X_GFSK_RX_BW_29_3                        0x0D        //  7     0                        29.3 kHz
+#define SX126X_GFSK_RX_BW_39_0                        0x1C        //  7     0                        39.0 kHz
+#define SX126X_GFSK_RX_BW_46_9                        0x14        //  7     0                        46.9 kHz
+#define SX126X_GFSK_RX_BW_58_6                        0x0C        //  7     0                        58.6 kHz
+#define SX126X_GFSK_RX_BW_78_2                        0x1B        //  7     0                        78.2 kHz
+#define SX126X_GFSK_RX_BW_93_8                        0x13        //  7     0                        93.8 kHz
+#define SX126X_GFSK_RX_BW_117_3                       0x0B        //  7     0                        117.3 kHz
+#define SX126X_GFSK_RX_BW_156_2                       0x1A        //  7     0                        156.2 kHz
+#define SX126X_GFSK_RX_BW_187_2                       0x12        //  7     0                        187.2 kHz
+#define SX126X_GFSK_RX_BW_234_3                       0x0A        //  7     0                        234.3 kHz
+#define SX126X_GFSK_RX_BW_312_0                       0x19        //  7     0                        312.0 kHz
+#define SX126X_GFSK_RX_BW_373_6                       0x11        //  7     0                        373.6 kHz
+#define SX126X_GFSK_RX_BW_467_0                       0x09        //  7     0                        467.0 kHz
+#define SX126X_LORA_BW_7_8                            0x00        //  7     0     LoRa bandwidth: 7.8 kHz
+#define SX126X_LORA_BW_10_4                           0x08        //  7     0                     10.4 kHz
+#define SX126X_LORA_BW_15_6                           0x01        //  7     0                     15.6 kHz
+#define SX126X_LORA_BW_20_8                           0x09        //  7     0                     20.8 kHz
+#define SX126X_LORA_BW_31_25                          0x02        //  7     0                     31.25 kHz
+#define SX126X_LORA_BW_41_7                           0x0A        //  7     0                     41.7 kHz
+#define SX126X_LORA_BW_62_5                           0x03        //  7     0                     62.5 kHz
+#define SX126X_LORA_BW_125_0                          0x04        //  7     0                     125.0 kHz
+#define SX126X_LORA_BW_250_0                          0x05        //  7     0                     250.0 kHz
+#define SX126X_LORA_BW_500_0                          0x06        //  7     0                     500.0 kHz
+#define SX126X_LORA_CR_4_5                            0x01        //  7     0     LoRa coding rate: 4/5
+#define SX126X_LORA_CR_4_6                            0x02        //  7     0                       4/6
+#define SX126X_LORA_CR_4_7                            0x03        //  7     0                       4/7
+#define SX126X_LORA_CR_4_8                            0x04        //  7     0                       4/8
+#define SX126X_LORA_LOW_DATA_RATE_OPTIMIZE_OFF        0x00        //  7     0     LoRa low data rate optimization: disabled
+#define SX126X_LORA_LOW_DATA_RATE_OPTIMIZE_ON         0x01        //  7     0                                      enabled
 
-#define XTAL_FREQ (double)32000000
-#define FREQ_DIV (double)pow(2.0, 25.0)
-#define FREQ_STEP (double)(XTAL_FREQ / FREQ_DIV)
+//SX126X_CMD_SET_PACKET_PARAMS
+#define SX126X_GFSK_PREAMBLE_DETECT_OFF               0x00        //  7     0     GFSK minimum preamble length before reception starts: detector disabled
+#define SX126X_GFSK_PREAMBLE_DETECT_8                 0x04        //  7     0                                                           8 bits
+#define SX126X_GFSK_PREAMBLE_DETECT_16                0x05        //  7     0                                                           16 bits
+#define SX126X_GFSK_PREAMBLE_DETECT_24                0x06        //  7     0                                                           24 bits
+#define SX126X_GFSK_PREAMBLE_DETECT_32                0x07        //  7     0                                                           32 bits
+#define SX126X_GFSK_ADDRESS_FILT_OFF                  0x00        //  7     0     GFSK address filtering: disabled
+#define SX126X_GFSK_ADDRESS_FILT_NODE                 0x01        //  7     0                             node only
+#define SX126X_GFSK_ADDRESS_FILT_NODE_BROADCAST       0x02        //  7     0                             node and broadcast
+#define SX126X_GFSK_PACKET_FIXED                      0x00        //  7     0     GFSK packet type: fixed (payload length known in advance to both sides)
+#define SX126X_GFSK_PACKET_VARIABLE                   0x01        //  7     0                       variable (payload length added to packet)
+#define SX126X_GFSK_CRC_OFF                           0x01        //  7     0     GFSK packet CRC: disabled
+#define SX126X_GFSK_CRC_1_BYTE                        0x00        //  7     0                      1 byte
+#define SX126X_GFSK_CRC_2_BYTE                        0x02        //  7     0                      2 byte
+#define SX126X_GFSK_CRC_1_BYTE_INV                    0x04        //  7     0                      1 byte, inverted
+#define SX126X_GFSK_CRC_2_BYTE_INV                    0x06        //  7     0                      2 byte, inverted
+#define SX126X_GFSK_WHITENING_OFF                     0x00        //  7     0     GFSK data whitening: disabled
+#define SX126X_GFSK_WHITENING_ON                      0x01        //  7     0                          enabled
+#define SX126X_LORA_HEADER_EXPLICIT                   0x00        //  7     0     LoRa header mode: explicit
+#define SX126X_LORA_HEADER_IMPLICIT                   0x01        //  7     0                       implicit
+#define SX126X_LORA_CRC_OFF                           0x00        //  7     0     LoRa CRC mode: disabled
+#define SX126X_LORA_CRC_ON                            0x01        //  7     0                    enabled
+#define SX126X_LORA_IQ_STANDARD                       0x00        //  7     0     LoRa IQ setup: standard
+#define SX126X_LORA_IQ_INVERTED                       0x01        //  7     0                    inverted
 
-#define SX1262_SLEEPCFG_ColdStart_RTCDisable 0
-#define SX1262_SLEEPCFG_ColdStart_RTCEnable 1
-#define SX1262_SLEEPCFG_WarmStart_RTCDisable 4
-#define SX1262_SLEEPCFG_WarmStart_RTCEnable 5
+//SX126X_CMD_SET_CAD_PARAMS
+#define SX126X_CAD_ON_1_SYMB                          0x00        //  7     0     number of symbols used for CAD: 1
+#define SX126X_CAD_ON_2_SYMB                          0x01        //  7     0                                     2
+#define SX126X_CAD_ON_4_SYMB                          0x02        //  7     0                                     4
+#define SX126X_CAD_ON_8_SYMB                          0x03        //  7     0                                     8
+#define SX126X_CAD_ON_16_SYMB                         0x04        //  7     0                                     16
+#define SX126X_CAD_GOTO_STDBY                         0x00        //  7     0     after CAD is done, always go to STDBY_RC mode
+#define SX126X_CAD_GOTO_RX                            0x01        //  7     0     after CAD is done, go to Rx mode if activity is detected
 
-#define SX1262_STANDBY_RC 0
-#define SX1262_STANDBY_XOSC 1
+//SX126X_CMD_GET_STATUS
+#define SX126X_STATUS_MODE_STDBY_RC                   0b00100000  //  6     4     current chip mode: STDBY_RC
+#define SX126X_STATUS_MODE_STDBY_XOSC                 0b00110000  //  6     4                        STDBY_XOSC
+#define SX126X_STATUS_MODE_FS                         0b01000000  //  6     4                        FS
+#define SX126X_STATUS_MODE_RX                         0b01010000  //  6     4                        RX
+#define SX126X_STATUS_MODE_TX                         0b01100000  //  6     4                        TX
+#define SX126X_STATUS_DATA_AVAILABLE                  0b00000100  //  3     1     command status: packet received and data can be retrieved
+#define SX126X_STATUS_CMD_TIMEOUT                     0b00000110  //  3     1                     SPI command timed out
+#define SX126X_STATUS_CMD_INVALID                     0b00001000  //  3     1                     invalid SPI command
+#define SX126X_STATUS_CMD_FAILED                      0b00001010  //  3     1                     SPI command failed to execute
+#define SX126X_STATUS_TX_DONE                         0b00001100  //  3     1                     packet transmission done
+#define SX126X_STATUS_SPI_FAILED                      0b11111111  //  7     0     SPI transaction failed
 
-#define SX1262_TIMEOUT_ONCE 0
-#define SX1262_TIMEOUT_RX_CONTINOUS 0xFFFFFF
+//SX126X_CMD_GET_PACKET_STATUS
+#define SX126X_GFSK_RX_STATUS_PREAMBLE_ERR            0b10000000  //  7     7     GFSK Rx status: preamble error
+#define SX126X_GFSK_RX_STATUS_SYNC_ERR                0b01000000  //  6     6                     sync word error
+#define SX126X_GFSK_RX_STATUS_ADRS_ERR                0b00100000  //  5     5                     address error
+#define SX126X_GFSK_RX_STATUS_CRC_ERR                 0b00010000  //  4     4                     CRC error
+#define SX126X_GFSK_RX_STATUS_LENGTH_ERR              0b00001000  //  3     3                     length error
+#define SX126X_GFSK_RX_STATUS_ABORT_ERR               0b00000100  //  2     2                     abort error
+#define SX126X_GFSK_RX_STATUS_PACKET_RECEIVED         0b00000010  //  2     2                     packet received
+#define SX126X_GFSK_RX_STATUS_PACKET_SENT             0b00000001  //  2     2                     packet sent
 
-#define SX1262_RECEIVE_MODE 0
-#define SX1262_TRANSMIT_MODE 1
+//SX126X_CMD_GET_DEVICE_ERRORS
+#define SX126X_PA_RAMP_ERR                            0b100000000  //  8     8     device errors: PA ramping failed
+#define SX126X_PLL_LOCK_ERR                           0b001000000  //  6     6                    PLL failed to lock
+#define SX126X_XOSC_START_ERR                         0b000100000  //  5     5                    crystal oscillator failed to start
+#define SX126X_IMG_CALIB_ERR                          0b000010000  //  4     4                    image calibration failed
+#define SX126X_ADC_CALIB_ERR                          0b000001000  //  3     3                    ADC calibration failed
+#define SX126X_PLL_CALIB_ERR                          0b000000100  //  2     2                    PLL calibration failed
+#define SX126X_RC13M_CALIB_ERR                        0b000000010  //  1     1                    RC13M calibration failed
+#define SX126X_RC64K_CALIB_ERR                        0b000000001  //  0     0                    RC64K calibration failed
 
-#define SX1262_STOP_TIMER_ON_PREAMBLE_DISABLE 0x00
-#define SX1262_STOP_TIMER_ON_PREAMBLE_ENABLE 0x01
 
-#define SX1262_REGULATOR_LDO_ONLY 0x00
-#define SX1262_REGULATOR_DC_DC_LDO 0x01
+// SX126X SPI register variables
+//SX126X_REG_LORA_SYNC_WORD_MSB + LSB
+#define SX126X_SYNC_WORD_PUBLIC                       0x3444
+#define SX126X_SYNC_WORD_PRIVATE                      0x1424
 
-#define SX1262_FALLBACK_FS 0x40
-#define SX1262_FALLBACK_STANDBY_XOSC 0x30
-#define SX1262_FALLBACK_RC 0x20
+#define SX126x_TXMODE_ASYNC                           0x01
+#define SX126x_TXMODE_SYNC                            0x02
+#define SX126x_TXMODE_BACK2RX                         0x04
 
-#define SX1262_IRQ_TXDone (1 << 0)
-#define SX1262_IRQ_RXDone (1 << 1)
-#define SX1262_IRQ_PreambleDetected (1 << 2)
-#define SX1262_IRQ_SyncWordValid (1 << 3)
-#define SX1262_IRQ_HeaderValid (1 << 4)
-#define SX1262_IRQ_HeaderError (1 << 5)
-#define SX1262_IRQ_CRCError (1 << 6)
-#define SX1262_IRQ_ChannelActivityDetectionDone (1 << 7)
-#define SX1262_IRQ_ChannelActivityDetected (1 << 8)
-#define SX1262_IRQ_Timeout (1 << 9)
-#define SX1262_IRQ_ALL (SX1262_IRQ_TXDone | SX1262_IRQ_RXDone | SX1262_IRQ_PreambleDetected | SX1262_IRQ_SyncWordValid | SX1262_IRQ_HeaderValid | SX1262_IRQ_HeaderError | SX1262_IRQ_CRCError | SX1262_IRQ_ChannelActivityDetectionDone | SX1262_IRQ_ChannelActivityDetected | SX1262_IRQ_Timeout)
+// Public function
+void     LoRaInit(void);
+int16_t  LoRaBegin(uint32_t frequencyInHz, int8_t txPowerInDbm, float tcxoVoltage, bool useRegulatorLDO);
+void     LoRaConfig(uint8_t spreadingFactor, uint8_t bandwidth, uint8_t codingRate, uint16_t preambleLength, uint8_t payloadLen, bool crcOn, bool invertIrq);
+uint8_t  LoRaReceive(uint8_t *pData, int16_t len);
+bool     LoRaSend(uint8_t *pData, int16_t len, uint8_t mode);
+void     LoRaDebugPrint(bool enable);
 
-#define SX1262_TCXO_VOLTAGE16dV 0x00
-#define SX1262_TCXO_VOLTAGE17dV 0x01
-#define SX1262_TCXO_VOLTAGE18dV 0x02
-#define SX1262_TCXO_VOLTAGE22dV 0x03
-#define SX1262_TCXO_VOLTAGE24dV 0x04
-#define SX1262_TCXO_VOLTAGE27dV 0x05
-#define SX1262_TCXO_VOLTAGE30dV 0x06
-#define SX1262_TCXO_VOLTAGE33dV 0x07
+// Private function
+void     spi_write_byte(uint8_t* Dataout, size_t DataLength );
+void     spi_read_byte(uint8_t* Datain, uint8_t* Dataout, size_t DataLength );
+uint8_t  spi_transfer(uint8_t address);
 
-#define SX1262_PACKET_TYPE_GFSK 0x00
-#define SX1262_PACKET_TYPE_LORA 0x01
+bool     ReceiveMode(void);
+void     GetPacketStatus(int8_t *rssiPacket, int8_t *snrPacket);
+void     SetTxPower(int8_t txPowerInDbm);
 
-#define SX1262_Ramp_10U (0x00)
-#define SX1262_Ramp_20U (0x01)
-#define SX1262_Ramp_40U (0x02)
-#define SX1262_Ramp_80U (0x03)
-#define SX1262_Ramp_200U (0x04)
-#define SX1262_Ramp_800U (0x05)
-#define SX1262_Ramp_1700U (0x06)
-#define SX1262_Ramp_3400U (0x07)
+void     FixInvertedIQ(uint8_t iqConfig);
+void     SetDio3AsTcxoCtrl(float voltage, uint32_t delay);
+void     SetDio2AsRfSwitchCtrl(uint8_t enable);
+void     Reset(void);
+void     SetStandby(uint8_t mode);
+void     SetRfFrequency(uint32_t frequency);
+void     Calibrate(uint8_t calibParam);
+void     CalibrateImage(uint32_t frequency);
+void     SetRegulatorMode(uint8_t mode);
+void     SetBufferBaseAddress(uint8_t txBaseAddress, uint8_t rxBaseAddress);
+void     SetPowerConfig(int8_t power, uint8_t rampTime);
+void     SetOvercurrentProtection(float currentLimit);
+void     SetSyncWord(int16_t sync);
+void     SetPaConfig(uint8_t paDutyCycle, uint8_t hpMax, uint8_t deviceSel, uint8_t paLut);
+void     SetDioIrqParams(uint16_t irqMask, uint16_t dio1Mask, uint16_t dio2Mask, uint16_t dio3Mask);
+void     SetStopRxTimerOnPreambleDetect(bool enable);
+void     SetLoRaSymbNumTimeout(uint8_t SymbNum);
+void     SetPacketType(uint8_t packetType);
+void     SetModulationParams(uint8_t spreadingFactor, uint8_t bandwidth, uint8_t codingRate, uint8_t lowDataRateOptimize);
+void     SetCadParams(uint8_t cadSymbolNum, uint8_t cadDetPeak, uint8_t cadDetMin, uint8_t cadExitMode, uint32_t cadTimeout);
+void     SetCad();
+uint8_t  GetStatus(void);
+uint16_t GetIrqStatus(void);
+void     ClearIrqStatus(uint16_t irq);
+void     SetRx(uint32_t timeout);
+void     SetTx(uint32_t timeoutInMs);
+int      GetPacketLost();
+uint8_t  GetRssiInst();
+void     GetRxBufferStatus(uint8_t *payloadLength, uint8_t *rxStartBufferPointer);
+void     Wakeup(void);
+void     WaitForIdleBegin(unsigned long timeout, char *text);
+bool     WaitForIdle(unsigned long timeout, char *text, bool stop);
+uint8_t  ReadBuffer(uint8_t *rxData, int16_t rxDataLen);
+void     WriteBuffer(uint8_t *txData, int16_t txDataLen);
+void     WriteRegister(uint16_t reg, uint8_t* data, uint8_t numBytes);
+void     ReadRegister(uint16_t reg, uint8_t* data, uint8_t numBytes);
+void     WriteCommand(uint8_t cmd, uint8_t* data, uint8_t numBytes);
+uint8_t  WriteCommand2(uint8_t cmd, uint8_t* data, uint8_t numBytes);
+void     ReadCommand(uint8_t cmd, uint8_t* data, uint8_t numBytes);
+void     SPItransfer(uint8_t cmd, bool write, uint8_t* dataOut, uint8_t* dataIn, uint8_t numBytes, bool waitForBusy);
+void     LoRaError(int error);
 
-#define SX1262_HEADER_TYPE_VARIABLE 0x00
-#define SX1262_HEADER_TYPE_FIXED 0x01
-
-#define SX1262_CRC_OFF 0x00
-#define SX1262_CRC_ON 0x01
-
-#define SX1262_STANDARD_IQ 0x00
-#define SX1262_INVERTED_IQ 0x01
-
-#define SX1262_CAD_ON_1_SYMB 0x00
-#define SX1262_CAD_ON_2_SYMB 0x01
-#define SX1262_CAD_ON_4_SYMB 0x02
-#define SX1262_CAD_ON_8_SYMB 0x03
-#define SX1262_CAD_ON_16_SYMB 0x04
-#define SX1262_CAD_ONLY 0x00
-#define SX1262_CAD_RX 0x01
-
-#define SX1262_ERROR_CALIBRATION_RC64K (1 << 0)
-#define SX1262_ERROR_CALIBRATION_RC13M (1 << 1)
-#define SX1262_ERROR_CALIBRATION_PLL (1 << 2)
-#define SX1262_ERROR_CALIBRATION_ADC (1 << 3)
-#define SX1262_ERROR_CALIBRATION_IMG (1 << 4)
-#define SX1262_ERROR_CALIBRATION_XOSC (1 << 5)
-#define SX1262_ERROR_PLL_LOCK (1 << 6)
-#define SX1262_ERROR_PA_RAMP (1 << 8)
-
-#define SX1262_CALIB_RC64K (1 << 0)      // RC64k calibration enabled
-#define SX1262_CALIB_RC13M (1 << 1)      // RC13M calibration enabled
-#define SX1262_CALIB_PLL (1 << 2)        // PLL calibration enabled
-#define SX1262_CALIB_ADC_PULSE (1 << 3)  // ADC pulse calibration enabled
-#define SX1262_CALIB_ADC_BULK_N (1 << 4) // ADC bulk N calibration enabled
-#define SX1262_CALIB_ADC_BULK_P (1 << 5) // ADC bulk P calibration enabled
-#define SX1262_CALIB_IMAGE (1 << 6)      // Image calibration enabled
-#define SX1262_CALIB_RESERVED (1 << 7)   // Reserved bit (RFU)
-
-// Combined mask for all calibration settings
-#define SX1262_CALIBRATION_ALL (SX1262_CALIB_RC64K | SX1262_CALIB_RC13M |           \
-                                SX1262_CALIB_PLL | SX1262_CALIB_ADC_PULSE |         \
-                                SX1262_CALIB_ADC_BULK_N | SX1262_CALIB_ADC_BULK_P | \
-                                SX1262_CALIB_IMAGE)
-
-// GFSK Pulse Shape
-#define SX1262_GFSK_NO_FILTER (0x00)
-#define SX1262_GFSK_BT_0_3 (0x08)
-#define SX1262_GFSK_BT_0_5 (0x09)
-#define SX1262_GFSK_BT_0_7 (0x0A)
-#define SX1262_GFSK_BT_1_0 (0x0B)
-
-// GFSK Bandwidth
-#define SX1262_GFSK_RX_BW_4800 (0x1F)
-#define SX1262_GFSK_RX_BW_5800 (0x17)
-#define SX1262_GFSK_RX_BW_7300 (0x0F)
-#define SX1262_GFSK_RX_BW_9700 (0x1E)
-#define SX1262_GFSK_RX_BW_11700 (0x16)
-#define SX1262_GFSK_RX_BW_14600 (0x0E)
-#define SX1262_GFSK_RX_BW_19500 (0x1D)
-#define SX1262_GFSK_RX_BW_23400 (0x15)
-#define SX1262_GFSK_RX_BW_29300 (0x0D)
-#define SX1262_GFSK_RX_BW_39000 (0x1C)
-#define SX1262_GFSK_RX_BW_46900 (0x14)
-#define SX1262_GFSK_RX_BW_58600 (0x0C)
-#define SX1262_GFSK_RX_BW_78200 (0x1B)
-#define SX1262_GFSK_RX_BW_93800 (0x13)
-#define SX1262_GFSK_RX_BW_117300 (0x0B)
-#define SX1262_GFSK_RX_BW_156200 (0x1A)
-#define SX1262_GFSK_RX_BW_187200 (0x12)
-#define SX1262_GFSK_RX_BW_234300 (0x0A)
-#define SX1262_GFSK_RX_BW_312000 (0x19)
-#define SX1262_GFSK_RX_BW_373600 (0x11)
-#define SX1262_GFSK_RX_BW_467000 (0x09)
-
-// LoRa Spreading Factor (SF)
-#define SX1262_LORA_SF5 (0x05)
-#define SX1262_LORA_SF6 (0x06)
-#define SX1262_LORA_SF7 (0x07)
-#define SX1262_LORA_SF8 (0x08)
-#define SX1262_LORA_SF9 (0x09)
-#define SX1262_LORA_SF10 (0x0A)
-#define SX1262_LORA_SF11 (0x0B)
-#define SX1262_LORA_SF12 (0x0C)
-
-// LoRa Bandwidth (BW)
-#define SX1262_LORA_BW_7 (0x00)   // 7.81 kHz
-#define SX1262_LORA_BW_10 (0x08)  // 10.42 kHz
-#define SX1262_LORA_BW_15 (0x01)  // 15.63 kHz
-#define SX1262_LORA_BW_20 (0x09)  // 20.83 kHz
-#define SX1262_LORA_BW_31 (0x02)  // 31.25 kHz
-#define SX1262_LORA_BW_41 (0x0A)  // 41.67 kHz
-#define SX1262_LORA_BW_62 (0x03)  // 62.50 kHz
-#define SX1262_LORA_BW_125 (0x04) // 125 kHz
-#define SX1262_LORA_BW_250 (0x05) // 250 kHz
-#define SX1262_LORA_BW_500 (0x06) // 500 kHz
-
-// LoRa Coding Rate (CR)
-#define SX1262_LORA_CR_4_5 (0x01)
-#define SX1262_LORA_CR_4_6 (0x02)
-#define SX1262_LORA_CR_4_7 (0x03)
-#define SX1262_LORA_CR_4_8 (0x04)
-
-#define SX1262_LORA_SYNC_WORD_MSB (0x0740)
-#define SX1262_LORA_SYNC_WORD_LSB (0x0741)
-
-void sx1262_setPacketType(uint8_t packetType);
-void sx1262_init();
-void sx1262_reset();
-void sx1262_wait_for_busy();
-void sx1262_write_command(uint8_t cmd, uint8_t *data, uint8_t len);
-void sx1262_read_command(uint8_t cmd, uint8_t *tx_payload_buffer, uint8_t tx_payload_len, uint8_t *rx_buffer, uint8_t len);
-sx1262_status_t sx1262_get_status();
-void sx1262_setSleep(uint8_t sleepCFG);
-void sx1262_setStandby(uint8_t standbyConf);
-void sx1262_setFrequencySynthesis();
-void sx1262_setMode(uint8_t mode, uint32_t timeout);
-void sx1262_stopTimerOnPreamble(uint8_t enable);
-void sx1262_setRxDutyCycle(uint32_t rxPeriod, uint32_t sleepPeriod);
-void sx1262_setChannelActivityDetection(void);
-void sx1262_setTxContinuousWave(void);
-void sx1262_setTxInfinitePreamble(void);
-void sx1262_setRegulatorMode(uint8_t mode);
-void sx1262_calibrate(uint8_t calibParam);
-void sx1262_calibrateImage(uint8_t freq1, uint8_t freq2);
-void sx1262_setRxTXFallbackMode(uint8_t fallbackMode);
-void sx1262_writeRegister(uint16_t address, const uint8_t *data, size_t length);
-void sx1262_readRegister(uint16_t address, uint8_t *data, size_t length);
-void sx1262_writeBuffer(uint8_t offset, const uint8_t *data, size_t length);
-void sx1262_readBuffer(uint8_t offset, uint8_t *data, size_t length);
-void sx1262_setDioIrqParams(uint16_t irqMask, uint16_t dio1Mask, uint16_t dio2Mask, uint16_t dio3Mask);
-uint16_t sx1262_getIrqStatus(void);
-void sx1262_clearIrqStatus(uint16_t clearIrqParam);
-void sx1262_setDIO2AsRfSwitchCtrl(uint8_t enable);
-void sx1262_setDIO3AsTCXOCtrl(uint8_t tcxoVoltage, uint32_t delay);
-void sx1262_setFrequency(uint32_t frequency);
-uint8_t sx1262_getPacketType();
-void sx1262_configure_tx_power(uint8_t paDutyCycle, uint8_t hpMax, uint8_t paLut, int8_t power, uint8_t rampTime);
-void sx1262_setLoRaPacketParams(sx1262_LoRaPacketParams_t *params);
-void sx1262_setCadParams(uint8_t cadSymbolNum, uint8_t cadDetPeak, uint8_t cadDetMin, uint8_t cadExitMode, uint32_t cadTimeout);
-void sx1262_setBufferBaseAddress(uint8_t txBaseAddr, uint8_t rxBaseAddr);
-void sx1262_setLoRaSymbNumTimeout(uint8_t symbNum);
-void sx1262_getStatus(uint8_t *status);
-void sx1262_getRxBufferStatus(uint8_t *payloadLengthRx, uint8_t *rxStartBufferPointer);
-void sx1262_getPacketStatus(uint8_t *rssi, uint8_t *snr, uint8_t *signalRssi);
-uint8_t sx1262_getRssiInst(uint8_t *rssiInst);
-void sx1262_getStats(uint16_t *pktReceived, uint16_t *pktCrcError, uint16_t *pktHeaderErr);
-void sx1262_resetStats(void);
-uint16_t sx1262_getDeviceErrors();
-void sx1262_clearDeviceErrors(void);
-
-void sx1262_setLoRaModulationParams(const sx1262_LoRaModulationParams_t *params);
-
-void sx1262_setGFSKModulationParams(const sx1262_GFSKModulationParams_t *params);
 
 #endif
diff --git a/main/main.c b/main/main.c
index f348047..4ec7b71 100644
--- a/main/main.c
+++ b/main/main.c
@@ -64,6 +64,8 @@ void app_main(void)
 
     mcp23018_init();
 
+    void servoControllerInit();
+
     /* create task pinned to the app core */
     xTaskCreate(
         i2c_sensors_task,
diff --git a/managed_components/k0i05__esp_bme680/bme680.c b/managed_components/k0i05__esp_bme680/bme680.c
index 0a8a7fd..28b73e7 100644
--- a/managed_components/k0i05__esp_bme680/bme680.c
+++ b/managed_components/k0i05__esp_bme680/bme680.c
@@ -1091,6 +1091,8 @@ esp_err_t bme680_get_data(bme680_handle_t handle, bme680_data_t *const data) {
     data->heater_stable         = adc_data.heater_stable;
     data->gas_valid             = adc_data.gas_valid;
 
+    data->raw_data = adc_data;
+
     /* compute scores */
     bme680_compute_iaq(data);
 
diff --git a/managed_components/k0i05__esp_bme680/include/bme680.h b/managed_components/k0i05__esp_bme680/include/bme680.h
index 1e81ecd..4deb256 100644
--- a/managed_components/k0i05__esp_bme680/include/bme680.h
+++ b/managed_components/k0i05__esp_bme680/include/bme680.h
@@ -308,6 +308,22 @@ typedef struct bme680_cal_factors_s {
     int8_t                  range_switching_error;
 } bme680_cal_factors_t;
 
+
+/**
+ * @brief BME680 ADC data structure definition.
+ */
+typedef struct bme680_adc_data_s {
+    uint32_t temperature;
+    uint16_t humidity;
+    uint32_t pressure;
+    uint16_t gas;
+    bool     gas_valid;
+    uint8_t  gas_range;
+    uint8_t  gas_index;
+    bool     heater_stable;
+} bme680_adc_data_t;
+
+
 /**
  * @brief BME680 data structure definition.
  */
@@ -325,22 +341,9 @@ typedef struct bme680_data_s {
     float    temperature_score;
     float    humidity_score;
     float    gas_score;
+    bme680_adc_data_t raw_data;
 } bme680_data_t;
 
-/**
- * @brief BME680 ADC data structure definition.
- */
-typedef struct bme680_adc_data_s {
-    uint32_t temperature;
-    uint16_t humidity;
-    uint32_t pressure;
-    uint16_t gas;
-    bool     gas_valid;
-    uint8_t  gas_range;
-    uint8_t  gas_index;
-    bool     heater_stable;
-} bme680_adc_data_t;
-
 /**
  * @brief BME680 configuration structure definition.
  */