diff --git a/platformio.ini b/platformio.ini
index 57db5ba..cbf8e4e 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -41,7 +41,7 @@ build_src_flags =
     -Wno-error=pedantic
     -Wpedantic
     -O2
-build_flags = -DCORE_DEBUG_LEVEL=4
+build_flags = -DCORE_DEBUG_LEVEL=3
 check_tool = pvs-studio
 check_flags = pvs-studio: --analysis-mode=4 --exclude-path=.pio/libdeps --lic-file=~/.config/PVS-Studio/PVS-Studio.lic
 extra_scripts = post:convert_sysincludes.py
\ No newline at end of file
diff --git a/src/BoardI2C.hpp b/src/BoardI2C.hpp
index f8442e4..2c0a428 100644
--- a/src/BoardI2C.hpp
+++ b/src/BoardI2C.hpp
@@ -9,13 +9,13 @@
 
 class BoardI2C : public TwoWire {
 public:
-    BoardI2C(volatile bool loop_on_fail = true) : TwoWire(0) {
+    BoardI2C() : TwoWire(0) {
         if (begin(I2C_SDA_PIN, I2C_SCL_PIN, 100000)) {
             setTimeOut(10);
             ESP_LOGI("I2CBus", "Bus initialized");
         } else {
             ESP_LOGE("I2CBus", "Failed to initialize the i2c software bus!");
-            while (loop_on_fail);
+            assert(false);
         }
     }
 };
\ No newline at end of file
diff --git a/src/Logic/FlightController.cpp b/src/Logic/FlightController.cpp
index 6da654d..8d5b108 100644
--- a/src/Logic/FlightController.cpp
+++ b/src/Logic/FlightController.cpp
@@ -181,13 +181,13 @@ void FlightController::setHeightControllerParams(float p, float i, float d) {
 
 void FlightController::setYawControllerParams(float p, float i, float d) {
     if (_status == DeviceStatus::Idle) {
-        _heightController->setParams({.p = p, .i = i, .d = d});
+        _yawController->setParams({.p = p, .i = i, .d = d});
     }
 }
 
 void FlightController::setPitchControllerParams(float p, float i, float d) {
     if (_status == DeviceStatus::Idle) {
-        _heightController->setParams({.p = p, .i = i, .d = d});
+        _tailRotorController->setParams({.p = p, .i = i, .d = d});
     }
 }
 
diff --git a/src/Logic/FlightDispatcher.cpp b/src/Logic/FlightDispatcher.cpp
index e38c468..e43b2d9 100644
--- a/src/Logic/FlightDispatcher.cpp
+++ b/src/Logic/FlightDispatcher.cpp
@@ -47,7 +47,7 @@ void FlightDispatcher::_onNewDeviceConnected(BTAddress device) {
 
 void FlightDispatcher::_onNewMessageReceived(char *package) {
     using sutil::SH;
-    ESP_LOGI(_tag, "Received new package: %s", package);
+    ESP_LOGD(_tag, "Received new package: %s", package);
     gson::Doc pkg;
     if (!pkg.parse(package, _jsonMaxDepth)) {
         ESP_LOGE(_tag, "Parcing error occured with new package (error %s, place %d): %s",
@@ -57,10 +57,8 @@ void FlightDispatcher::_onNewMessageReceived(char *package) {
         for (int keyIndex = 0; keyIndex < pkg.length(); ++keyIndex) {
             auto pair = pkg[pkg.keyHash(keyIndex)];
             auto value = pair.value();
-            ESP_LOGD(_tag, "Key: %zu", pkg.keyHash(keyIndex));
 
             if (!pair.valid()) {
-                ESP_LOGW(_tag, "Pair isn't valid and was skipped");
                 continue;
             }
 
@@ -188,4 +186,4 @@ void FlightDispatcher::_sendTelemetry() {
     package.end();
     package.s = String(MessageType::UpdatePackage) + ";" + package.s;
     _bluetoothDispatcher->sendPackage(package.s.c_str());
-}
+}
\ No newline at end of file
diff --git a/src/RF/BluetoothDispatcher.hpp b/src/RF/BluetoothDispatcher.hpp
index b1463ba..bac8274 100644
--- a/src/RF/BluetoothDispatcher.hpp
+++ b/src/RF/BluetoothDispatcher.hpp
@@ -29,7 +29,7 @@ class BluetoothDispatcher {
 public:
     BluetoothDispatcher(BluetoothSerial *controller, const char *device_name = "Helicopter");
 
-    bool initialize(volatile bool loop_on_fail = true, int readTimeoutMS = 10);
+    bool initialize(int readTimeoutMS = 10);
 
     void tick(const char message_delimeter = '\n');
 
@@ -54,7 +54,6 @@ private:
     NewPackageCallback _newPackageReceivedCb = nullptr;
     constexpr static const char *_tag = "BluetoothDispatcher";
 
-    static constexpr uint16_t _buffer_size = 522;
-    static constexpr uint16_t _real_buffer_size = 512;
+    static constexpr uint16_t _buffer_size = 512;
     mString<_buffer_size> _buffer;
 };
\ No newline at end of file
diff --git a/src/Sensors/Barometer.hpp b/src/Sensors/Barometer.hpp
index 2daf62b..88c7fbb 100644
--- a/src/Sensors/Barometer.hpp
+++ b/src/Sensors/Barometer.hpp
@@ -75,6 +75,6 @@ private:
     uint32_t _calibrationTimer = 0;
     int _calibrationIterationsCounter = 0;
     OnMeasuringFinishedCb _measuaringHeightFinishedCb = nullptr;
-    static constexpr int _calibrationIterationsCount = 1000;
+    static constexpr int _calibrationIterationsCount = 800;
     static constexpr int _calibrationIterationDelay = 1;  // [ms]
 };
\ No newline at end of file
diff --git a/src/Sensors/RangingSensor.cpp b/src/Sensors/RangingSensor.cpp
index 38dd87a..241b6b5 100644
--- a/src/Sensors/RangingSensor.cpp
+++ b/src/Sensors/RangingSensor.cpp
@@ -5,7 +5,7 @@
 #include "RangingSensor.hpp"
 #include "esp_log.h"
 
-RangingSensor::RangingSensor(TwoWire &i2c, volatile bool loop_on_fail) {
+RangingSensor::RangingSensor(TwoWire &i2c) {
     _sensor = VL53L0X();
     _sensor.setBus(&i2c);
     if (_sensor.init()) {
@@ -14,7 +14,7 @@ RangingSensor::RangingSensor(TwoWire &i2c, volatile bool loop_on_fail) {
         ESP_LOGI(_tag, "Sensor initialized");
     } else {
         ESP_LOGE(_tag, "Failed to initialize RangingSensor!");
-        while (loop_on_fail) {}
+        assert(false);
     }
     _sensor.startContinuous(10);
 }
diff --git a/src/Sensors/RangingSensor.hpp b/src/Sensors/RangingSensor.hpp
index 2bb5874..1b5c54f 100644
--- a/src/Sensors/RangingSensor.hpp
+++ b/src/Sensors/RangingSensor.hpp
@@ -10,7 +10,7 @@
 
 class RangingSensor {
 public:
-    RangingSensor(TwoWire &i2c, volatile bool loop_on_fail = true);
+    RangingSensor(TwoWire &i2c);
 
     void tick();
 
diff --git a/src/Sensors/Sensors.cpp b/src/Sensors/Sensors.cpp
index 34cc7de..ad4d571 100644
--- a/src/Sensors/Sensors.cpp
+++ b/src/Sensors/Sensors.cpp
@@ -4,7 +4,7 @@
 #include "Sensors.hpp"
 
 Sensors::Sensors(Barometer *barometer, RangingSensor *ranging_sensor, MPU *mpu, Kalman2DFilter *filter,
-                 BatteryController *battery, HeightSensor heightSensor, volatile bool loop_on_fail) {
+                 BatteryController *battery, HeightSensor heightSensor) {
     assert(barometer != nullptr);
     assert(mpu != nullptr);
     assert(filter != nullptr);
@@ -23,7 +23,7 @@ Sensors::Sensors(Barometer *barometer, RangingSensor *ranging_sensor, MPU *mpu,
         ESP_LOGI(_tag, "BMP280 initialized");
     } else {
         ESP_LOGI(_tag, "BMP280 initialization failed!");
-        while (loop_on_fail);
+        assert(false);
     }
 
     ESP_LOGI(_tag, "Initialize MPU6050...");
@@ -31,7 +31,7 @@ Sensors::Sensors(Barometer *barometer, RangingSensor *ranging_sensor, MPU *mpu,
         ESP_LOGI(_tag, "MPU6050 initialized");
     } else {
         ESP_LOGI(_tag, "MPU6050 initialization failed!");
-        while (loop_on_fail);
+        assert(false);
     }
 
     _battery->initialize();
@@ -53,6 +53,12 @@ void Sensors::measureBaseAltitudeSync(void) {
 }
 
 void Sensors::measureBaseAltitudeAsync(void) {
+    if (_heightSensor == HeightSensor::RangeMeterOnly) {
+        if (_onMeasuaringAltitudeFinished) {
+            _onMeasuaringAltitudeFinished();
+        }
+        return;
+    }
     _barometer->measureBaseAltitudeAsync();
 }
 
@@ -129,5 +135,6 @@ void Sensors::startMpuCalibration() {
 }
 
 void Sensors::onMeasuaringAltitudeFinished(OnMeasuringFinishedCb callback) {
-    _barometer->onMeasuaringHeightFinished(callback);
+    _onMeasuaringAltitudeFinished = callback;
+    _barometer->onMeasuaringHeightFinished(std::move(callback));
 }
\ No newline at end of file
diff --git a/src/Sensors/Sensors.hpp b/src/Sensors/Sensors.hpp
index 874961e..5de3852 100644
--- a/src/Sensors/Sensors.hpp
+++ b/src/Sensors/Sensors.hpp
@@ -27,7 +27,7 @@ enum HeightSensor {
 class Sensors {
 public:
     Sensors(Barometer *barometer, RangingSensor *rangingSensor, MPU *mpu, Kalman2DFilter *filter,
-            BatteryController *battery, HeightSensor heightSensor = Hybrid, bool loop_on_fail = true);
+            BatteryController *battery, HeightSensor heightSensor = Hybrid);
 
     ~Sensors();
 
@@ -58,6 +58,7 @@ private:
     Kalman2DFilter *_2d_filter = nullptr;
     RangingSensor *_ranging_sensor = nullptr;
     HeightSensor _heightSensor;
+    OnMeasuringFinishedCb _onMeasuaringAltitudeFinished = nullptr;
 
     /* cached filtered values */
     float _flightHeightFromBarometer = 0.f;
diff --git a/src/main.cpp b/src/main.cpp
index 4754469..4823c9d 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -18,7 +18,7 @@ void setup() {
                                 new MPU(new MPU6050(MPU6050_DEFAULT_ADDRESS, &i2c)),
                                 new Kalman2DFilter(10.f, 1.f, 1.8f),
                                 new BatteryController(BATTERY_DATA_PIN, BATTERY_DATA_SWITCH_PIN),
-                                HeightSensor::Hybrid),
+                                HeightSensor::RangeMeterOnly),
                     new SavedPidRegulator(3.5f, 0.0015f, 0.01f, "HeightControl"),  // height
                     new SavedPidRegulator(2.f, 12.f, 0.f, "YawControl"),  // yaw
                     new BrushedMotor(1, 2, 3, 4, 1),