RX buffer reader was simplified

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

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);
         }
     }
 };

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});
     }
 }
 

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;
             }
 

src/RF/BluetoothDispatcher.cpp

@@ -15,7 +15,7 @@ BluetoothDispatcher::BluetoothDispatcher(BluetoothSerial *controller, const char
     _controller = controller;
 }
 
-bool BluetoothDispatcher::initialize(volatile bool loop_on_fail, int readTimeoutMS) {
+bool BluetoothDispatcher::initialize(int readTimeoutMS) {
     _controller->enableSSP();
     _controller->onConfirmRequest([this](uint16_t pin) {
         _onConfirmRequest(pin);
@@ -31,8 +31,7 @@ bool BluetoothDispatcher::initialize(volatile bool loop_on_fail, int readTimeout
     bool success = _controller->begin(_device_name, false);
     if (!success) {
         ESP_LOGI(_tag, "Failed to initialize Bluetooth controller!");
-        while (loop_on_fail);
-        return false;
+        assert(false);
     } else {
         ESP_LOGI(_tag, "Bluetooth host initialized");
         return true;
@@ -45,23 +44,19 @@ void BluetoothDispatcher::onNewPackageReceived(NewPackageCallback newPackageRece
 
 void BluetoothDispatcher::tick(const char message_delimeter) {
     /* Call the callback, if new package received */
-    while (_controller->available()) { _buffer += (char) _controller->read(); }
-    if (_buffer.lastIndexOf(message_delimeter) != -1) {
-        char buffer[_buffer_size];
-        auto messageEnd = _buffer.indexOf(message_delimeter) - 1;
-        if (messageEnd == -1) {
+    /* Warning! Logging below can cause errors and freezes */
+    while (_controller->available()) {
+        int c = _controller->read();
+        if (c == -1) {
+            _buffer.clear();
+        } else if (c == message_delimeter) {
+            if (_newPackageReceivedCb) {
+                _newPackageReceivedCb(_buffer.buf);
+            }
             _buffer.clear();
         } else {
-            _buffer.substring(0, messageEnd, buffer);
+            _buffer += (char) c;
         }
-        ESP_LOGD(_tag, "Received new buffer %s", buffer);
-        if (_newPackageReceivedCb) {
-            _newPackageReceivedCb(buffer);
-        }
-        _buffer.clear();
-    }
-    if (_buffer.length() > _real_buffer_size) {
-        _buffer.clear();
     }
 }
 

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;
 };

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]
 };

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);
 }

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();
 

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));
 }

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;

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),