All modules and core of the app was created

.clang-format

@@ -1,7 +1,6 @@
 # Source: https://github.com/arduino/tooling-project-assets/tree/main/other/clang-format-configuration
 ---
 TabWidth: 2
-UseTab: Never
 AccessModifierOffset: -2
 AlignAfterOpenBracket: Align
 AlignConsecutiveAssignments: None
@@ -58,7 +57,7 @@ BreakConstructorInitializers: BeforeColon
 BreakConstructorInitializersBeforeComma: false
 BreakInheritanceList: BeforeColon
 BreakStringLiterals: false
-ColumnLimit: 120
+ColumnLimit: 90
 CommentPragmas: ''
 CompactNamespaces: false
 ConstructorInitializerAllOnOneLineOrOnePerLine: false

App.hpp

@@ -0,0 +1,23 @@
+#include "Logic/FlightDispatcher.hpp"
+#include "esp_log.h"
+
+class Application {
+  public:
+    Application(FlightDispatcher *dispatcher) {
+        assert(dispatcher != nullptr);
+        _dispatcher = dispatcher;
+        ESP_LOGI(_tag, "Application startup complete");
+    }
+
+    void tick() {
+        _dispatcher->tick();
+    }
+
+    ~Application() {
+        ESP_LOGI(_tag, "Application destroyed");
+        delete _dispatcher;
+    }
+  private:
+    FlightDispatcher *_dispatcher = nullptr;
+    static constexpr const char *_tag = "Application";
+};

BoardI2C.hpp

@@ -3,8 +3,9 @@
 #include "esp_log.h"
 
 class BoardI2C : public TwoWire {
+  public:
     BoardI2C(bool loop_on_fail = true) : TwoWire(0) {
-        if (begin(I2C_SDA_PIN, I2C_SCL_PIN, 400000)) {
+        if (begin(I2C_SDA_PIN, I2C_SCL_PIN, 100000)) {
             ESP_LOGI("I2CBus", "Bus initialized");
         } else {
             ESP_LOGE("I2CBus", "Failed to initialize the i2c software bus!");

Filters/Kalman2DFilter.hpp

@@ -4,25 +4,17 @@ class Kalman2DFilter {
   public:
     Kalman2DFilter(float dt = 4.f, float accelUncertainty = 10.f, float barometerUncertainty = 100.f) {
         dt /= 1000.f;
-        F = { 1, dt,
+        F = { 1, dt, 0, 1 };
-              0, 1 };
+        G = { 0.5f * dt * dt, dt };
-        G = { 0.5f * dt * dt,
-              dt };
         H = { 1, 0 };
-        I = { 1, 0,
+        I = { 1, 0, 0, 1 };
-              0, 1 };
         Q = G * ~G * accelUncertainty * accelUncertainty;
         R = { barometerUncertainty * barometerUncertainty };
-        P = { 0, 0,
+        P = { 0, 0, 0, 0 };
-              0, 0 };
+        S = { 0, 0 };
-        S = { 0,
-              0 };
     }
 
-    void filter(const float &AccZInertial,
+    void filter(const float &AccZInertial, const float &AltitudeBarometer, float &AltitudeKalman, float &VelocityVerticalKalman) {
-                const float &AltitudeBarometer,
-                float &AltitudeKalman,
-                float &VelocityVerticalKalman) {
         Acc = { AccZInertial };
         S = F * S + G * Acc;
         P = F * P * ~F + Q;

Logic/FlightController.cpp

@@ -0,0 +1,104 @@
+#include "FlightController.hpp"
+
+FlightController::FlightController(Sensors *sensors, PIDController *pid1,
+                                   PIDController *pid2, PIDController *pid3, bool unlimitedBattery) {
+    assert(sensors != nullptr);
+    assert(pid1 != nullptr);
+    assert(pid2 != nullptr);
+    assert(pid3 != nullptr);
+    _sensors = sensors;
+    _unlimitedBattery = unlimitedBattery;
+
+    _status = DeviceStatus::Idle;
+    _updateBatteryCharge();
+    ESP_LOGI(_tag, "Flight controller initialized");
+}
+
+FlightController::~FlightController() {
+    delete _sensors;
+    delete _pid1;
+    delete _pid2;
+    delete _pid3;
+}
+
+void FlightController::tick() {
+    bool hasUpdates = _sensors->tick();
+    if (hasUpdates) {
+        _updateBatteryCharge();
+    }
+    switch (_status) {
+    case DeviceStatus::Idle: break;
+    case DeviceStatus::ChargeRequired: break;
+    case DeviceStatus::IsPreparingForTakeoff: break;
+    case DeviceStatus::IsBoarding:
+        /* TODO: implement boarding */
+        break;
+    case DeviceStatus::IsFlying:
+        /* TODO: implement flying */
+        break;
+    }
+}
+
+ReportedDeviceState FlightController::currentDeviceState() const {
+    return { .batteryCharge = _sensors->batteryCharge(),
+             .flightHeight = _sensors->flightHeight(),
+             .status = _status,
+             .mpuState = _sensors->mpuData() };
+}
+
+void FlightController::startTakeoff(OnMeasuringFinishedCb onTakeoffStarted) {
+    /* Start preparation for takeoff */
+    _updateBatteryCharge();
+    if (_status == DeviceStatus::ChargeRequired)
+        return;
+
+    _status = DeviceStatus::IsPreparingForTakeoff;
+    _sensors->onMeasuaringAltitudeFinished([this]() {
+        if (_status == DeviceStatus::IsPreparingForTakeoff) {
+            // Preparation for takeoff is done
+            _takeoff();
+        }
+    });
+    _sensors->measureBaseAltitudeAsync();
+}
+
+void FlightController::startBoarding() {
+    if (_status == DeviceStatus::IsFlying) {
+        _status = DeviceStatus::IsBoarding;
+    }
+}
+
+void FlightController::_updateBatteryCharge() {
+    if(_unlimitedBattery) return;
+    if (_sensors->batteryCharge() <= _batteryCriticalCharge) {
+        if (_status == DeviceStatus::IsFlying or _status == DeviceStatus::IsBoarding) {
+            // TODO: board the device
+            _status = DeviceStatus::IsBoarding;
+        } else {
+            _status = DeviceStatus::ChargeRequired;
+        }
+    }
+}
+
+void FlightController::_takeoff() {
+    // TODO: implement takeoff
+    _status = DeviceStatus::IsFlying;
+    if(_onTakeoffStarted) {
+        _onTakeoffStarted();
+    }
+}
+
+void FlightController::_boarding() {
+    if (_sensors->flightHeight() <= _defaultStopMotorHeight) {
+        // TODO: stop motors and setpoints
+        _status = DeviceStatus::Idle;
+    } else {
+        // TODO: implement boarding
+    }
+}
+
+void FlightController::startImuCalibration() {
+    _sensors->onMpuCalibrationFinished([this]() {
+        this->_status = DeviceStatus::Idle;
+    });
+}

Logic/FlightController.hpp

@@ -0,0 +1,54 @@
+#include "PID.hpp"
+#include "Sensors/Sensors.hpp"
+
+enum DeviceStatus {
+    Idle = 0,
+    IsPreparingForTakeoff,
+    IsFlying,
+    IsBoarding,
+    IsImuCalibration,
+    ChargeRequired
+};
+
+struct ReportedDeviceState {
+    int batteryCharge;
+    float flightHeight;
+    DeviceStatus status;
+    MpuData mpuState;
+};
+
+struct FlightParams {
+    float height;         // [cm]
+    float yaw;      // [degrees]
+    float pitch;  // [degrees]
+};
+
+class FlightController {
+  public:
+    FlightController(Sensors *sensors, PIDController *pid1, PIDController *pid2, PIDController *pid3, bool unlimitedBattery = false);
+    ~FlightController();
+    ReportedDeviceState currentDeviceState() const;
+    void startTakeoff(OnMeasuringFinishedCb onTakeoffStarted);
+    void startBoarding();
+    void startImuCalibration();
+    void tick();
+  private:
+    void _updateBatteryCharge();
+    void _takeoff();
+    void _boarding();
+
+    Sensors *_sensors = nullptr;
+    PIDController *_pid1 = nullptr;
+    PIDController *_pid2 = nullptr;
+    PIDController *_pid3 = nullptr;
+    DeviceStatus _status;
+    OnMeasuringFinishedCb _onTakeoffStarted = nullptr;
+
+    static constexpr const char *_tag = "FlightController";
+    static constexpr uint8_t _batteryCriticalCharge = 15;  // [%]
+    static constexpr float _defaultFlightHeight = 30.f;    // [cm]
+    static constexpr float _defaultHorizontAngle = 90.f;   // [degrees]
+    static constexpr float _defaultStopMotorHeight = 5;    // [cm]
+
+    bool _unlimitedBattery = false; // for debug purposes
+};

Logic/FlightDispatcher.cpp

@@ -0,0 +1,80 @@
+#include "FlightDispatcher.hpp"
+
+FlightDispatcher::FlightDispatcher(BluetoothDispatcher *bluetoothDispatcher,
+                                   FlightController *flightController, bool loop_on_fail) {
+    assert(bluetoothDispatcher != nullptr);
+    assert(flightController != nullptr);
+
+    _bluetoothDispatcher = bluetoothDispatcher;
+    _flightController = flightController;
+
+    bluetoothDispatcher->onNewDeviceConnected([this](BTAddress device) {
+        this->_onNewDeviceConnected(device);
+    });
+    bluetoothDispatcher->onNewPackageReceived([this](char *package) {
+        this->_onNewMessageReceived(package);
+    });
+
+    if (_bluetoothDispatcher->initialize()) {
+        ESP_LOGI(_tag, "Bluetooth initialized successfully.");
+    } else {
+        ESP_LOGE(_tag, "Failed to initialize Bluetooth dispatcher!");
+        while (loop_on_fail)
+            ;
+    }
+    _telemetryTimer = millis();
+}
+
+FlightDispatcher::~FlightDispatcher() {
+    delete _bluetoothDispatcher;
+    delete _flightController;
+}
+
+void FlightDispatcher::tick() {
+    _flightController->tick();
+    _bluetoothDispatcher->tick();
+    if (millis() - _telemetryTimer >= _telemetryTimeIntervalMS) {
+        _sendTelemetry();
+        _telemetryTimer = millis();
+    }
+}
+
+void FlightDispatcher::_onNewDeviceConnected(BTAddress device) {
+    auto currentState = _flightController->currentDeviceState();
+    gson::string package;
+    package.beginObj();
+    package["batteryCharge"] = currentState.batteryCharge;
+    package["flightHeight"] = currentState.flightHeight;
+    package["status"] = currentState.status;
+    package.endObj();
+    package.end();
+    package += "\r\n";
+    auto pkg = package.s.c_str();
+    _bluetoothDispatcher->sendPackage(pkg, strlen(pkg));
+}
+
+void FlightDispatcher::_onNewMessageReceived(char *package) {
+    ESP_LOGI(_tag, "Received new package: %s", package);
+    gson::Doc pkg;
+    pkg.parse(package, _jsonMaxDepth);
+    // TODO: process package
+}
+
+void FlightDispatcher::_sendTelemetry() {
+    /* Notify mobile client about device state */
+    auto state = _flightController->currentDeviceState();
+    gson::string package;
+    package.beginObj();
+    package["batteryCharge"] = state.batteryCharge;
+    package["flightHeight"] = state.flightHeight;
+    package["status"] = state.status;
+    package["y"] = state.mpuState.yaw;
+    package["p"] = state.mpuState.pitch;
+    package["r"] = state.mpuState.roll;
+    package["zIn"] = state.mpuState.zInertial;
+    package.endObj();
+    package.end();
+    package += "\r\n";
+
+    _bluetoothDispatcher->sendPackage(package.s.c_str(), strlen((package.s.c_str())));
+}

Logic/FlightDispatcher.hpp

@@ -0,0 +1,24 @@
+#include "FlightController.hpp"
+#include "GSON.h"
+#include "RF/BluetoothDispatcher.hpp"
+
+class FlightDispatcher {
+  /* Deserialize state and update it in FlightController. */
+  public:
+    FlightDispatcher(BluetoothDispatcher *bluetoothDispatcher,
+                     FlightController *flightController, bool loop_on_fail = true);
+    ~FlightDispatcher();
+    void tick();
+  private:
+    void _onNewDeviceConnected(BTAddress device);
+    void _onNewMessageReceived(char *package);
+    void _sendTelemetry();
+
+    static constexpr const char *_tag = "FlightDispatcher";
+    static constexpr const int _telemetryTimeIntervalMS = 200;
+    static constexpr const uint8_t _jsonMaxDepth = 5;
+
+    uint32_t _telemetryTimer = millis();
+    BluetoothDispatcher *_bluetoothDispatcher;
+    FlightController *_flightController;
+};

Logic/PID.hpp

@@ -0,0 +1,50 @@
+#include "GyverPID.h"
+#include "Motor/BrushedMotor.hpp"
+#include "Preferences.h"
+#include "esp_log.h"
+#include "mString.h"
+
+class PIDController : public GyverPID {
+  public:
+    PIDController(float p, float i, float d, BrushedMotor *rotor, int rotorNumber = 1, uint16_t dt = 10)
+      : GyverPID(p, i, d, dt) {
+        assert(rotor != nullptr);
+        _rotor = rotor;
+        _rotorNumber = rotorNumber;
+        setLimits(0, _rotor->maxDuty());
+        _preferences.begin((String("r") + String(rotorNumber)).c_str());
+        _loadPreferences(p, i, d);
+        pid_timer = millis();
+        ESP_LOGI(_tag, "PID №%d initialized with params (%f, %f, %f)", _rotorNumber, Kp, Ki, Kd);
+    }
+
+    void tick() {
+        if (millis() - pid_timer >= _dt) {
+            _rotor->setDuty(getResultTimer());
+            pid_timer = millis();
+        }
+    }
+
+    void save() {
+        _preferences.putFloat("p", Kp);
+        _preferences.putFloat("i", Ki);
+        _preferences.putFloat("d", Kd);
+        ESP_LOGI(_tag, "PID №%d saved with params (%f, %f, %f)", _rotorNumber, Kp, Ki, Kd);
+    }
+
+  private:
+    Preferences _preferences;
+    int _rotorNumber = 1;
+    uint16_t _dt;
+    BrushedMotor *_rotor = nullptr;
+    uint32_t pid_timer = 0;
+    static constexpr const char *_tag = "PIDController";
+
+    void _loadPreferences(float pDefault = 0, float iDefault = 0, float dDefault = 0) {
+        if (_preferences.isKey("p") and _preferences.isKey("i") and _preferences.isKey("d")) {
+            Kp = _preferences.getFloat("p", pDefault);
+            Ki = _preferences.getFloat("i", iDefault);
+            Kd = _preferences.getFloat("d", dDefault);
+        }
+    }
+};

Motor/BrushedMotor.cpp

@@ -0,0 +1,22 @@
+#include "BrushedMotor.hpp"
+
+BrushedMotor::BrushedMotor(uint32_t PwmAGpioNum, uint32_t PwmBGpioNum, uint32_t PwmFreqHz,
+                           uint32_t McpwmResolutionHz, int McpwmGroupId) {}
+
+void BrushedMotor::enable() {}
+
+void BrushedMotor::disable() {}
+
+void BrushedMotor::setDuty(uint16_t duty) {}
+
+uint16_t BrushedMotor::maxDuty() {
+    return 0;
+}
+
+void BrushedMotor::forward() {}
+
+void BrushedMotor::reverse() {}
+
+void BrushedMotor::coast() {}
+
+void BrushedMotor::brake() {}

Motor/BrushedMotor.hpp

@@ -0,0 +1,29 @@
+#include <stdint.h>
+
+// TODO: implement class
+
+class BrushedMotor {
+    /* Driver for native MCPWM controller. */
+    // TODO: define default values
+  public:
+    BrushedMotor(uint32_t PwmAGpioNum, uint32_t PwmBGpioNum, uint32_t PwmFreqHz,
+                 uint32_t McpwmResolutionHz, int McpwmGroupId = 0);
+    void setDuty(uint16_t duty);
+    uint16_t maxDuty();
+    void enable();
+    void disable();
+    void forward();
+    void reverse();
+    void coast();
+    void brake();
+
+  protected:
+    /* MCPWM group */
+    int _mcpwmGroupId = 0;
+    uint32_t _mcpwmResolutionHz;
+
+    /* Brushed motor properties */
+    uint32_t _pwmAGpioNum;
+    uint32_t _pwmBGpioNum;
+    uint32_t _pwmFreqHz;
+};

RF/BluetoothDispatcher.cpp

@@ -3,41 +3,58 @@
 static DeviceConnectedCb deviceConnectedCallback = nullptr;
 static void deviceConnectedStaticCallback(esp_spp_cb_event_t event, esp_spp_cb_param_t *param);
 
-BluetoothDispatcher::BluetoothDispatcher(BluetoothSerial *controller, const char *device_name, HardwareSerial *serial) {
+BluetoothDispatcher::BluetoothDispatcher(BluetoothSerial *controller, const char *device_name) {
+    assert(controller != nullptr);
+    assert(device_name != nullptr);
+
     _device_name = device_name;
     _controller = controller;
-    _serial = serial;
 }
 
-bool BluetoothDispatcher::initialize() {
+bool BluetoothDispatcher::initialize(bool loop_on_fail, int readTimeoutMS) {
     _controller->enableSSP();
     _controller->onConfirmRequest([this](uint16_t pin) {
-        _confirmRequestCallback(pin);
+        _onConfirmRequest(pin);
     });
     _controller->onAuthComplete([this](boolean success) {
-        _authCompleteCallback(success);
+        _onAuthComplete(success);
     });
     _controller->register_callback(deviceConnectedStaticCallback);
-    deviceConnectedCallback = [this](esp_bd_addr_t bt_addr) {
+    deviceConnectedCallback = [this](BTAddress device) {
-        _deviceConnectedCallback(bt_addr);
+        _onDeviceConnected(device);
     };
     _controller->setTimeout(2);
-    return _controller->begin(_device_name);
+    bool success = _controller->begin(_device_name, false);
+    if (!success) {
+        ESP_LOGI(_tag, "Failed to initialize Bluetooth controller!");
+        while (loop_on_fail)
+            ;
+        return false;
+    } else {
+        ESP_LOGI(_tag, "Bluetooth host initialized");
+        return true;
+    }
+}
+
+void BluetoothDispatcher::onNewPackageReceived(NewPackageCallback newPackageReceivedCb) {
+    _newPackageReceivedCb = newPackageReceivedCb;
 }
 
 void BluetoothDispatcher::tick() {
+    /* Call the callback, if new package received */
     while (_controller->available() and _buffer.length() <= _buffer_size) {
         _buffer += (char)_controller->read();
     }
     if (_buffer.endsWith("\r")) {
         char buffer[_buffer_size];
         _buffer.substring(0, _buffer.lastIndexOf('\r'), buffer);
-        _serial->println(buffer);  // print the payload
+        ESP_LOGD(_tag, "Received new buffer %s", buffer);
+        if (_newPackageReceivedCb) {
+            _newPackageReceivedCb(buffer);
+        }
         _buffer.clear();
-        _controller->write((uint8_t *)"Hello, world!", strlen("Hello, world!"));
-        // TODO: add callback, that receive new state
     }
-    if (_buffer.length() > _buffer_size) {
+    if (_buffer.length() > _available_buffer_size) {
         _buffer.clear();
     }
 }
@@ -47,30 +64,39 @@ BluetoothDispatcher::~BluetoothDispatcher() {
     delete _controller;
 }
 
-void BluetoothDispatcher::_confirmRequestCallback(uint16_t pin) {
+void BluetoothDispatcher::_onConfirmRequest(uint16_t pin) {
-    _confirmRequestDone = true;
+    ESP_LOGI(_tag, "The Bluetooth PIN is: %06lu", (long unsigned int)pin);
-    _serial->print("The PIN is: ");
-    _serial->println(pin);
     _controller->confirmReply(true);
 }
 
-void BluetoothDispatcher::_authCompleteCallback(boolean success) {
+void BluetoothDispatcher::_onAuthComplete(boolean success) const {
     if (success) {
-        _confirmRequestDone = true;
+        ESP_LOGI(_tag, "Pairing success!");
-        _serial->println("Pairing success!");
     } else {
-        _serial->println("Pairing failed, rejected by user!");
+        ESP_LOGI(_tag, "Pairing failed, rejected by user!");
     }
 }
 
-void BluetoothDispatcher::_deviceConnectedCallback(esp_bd_addr_t bt_addr) {
+void BluetoothDispatcher::_onDeviceConnected(BTAddress device) const {
-    _serial->print("New connection opened: ");
+    ESP_LOGI(_tag, "New device connected: %s", device.toString(true).c_str());
-    _serial->println(BTAddress(bt_addr).toString(true));
+    if (_deviceConnectedCallback) {
+        _deviceConnectedCallback(device);
+    }
+}
+
+void BluetoothDispatcher::onNewDeviceConnected(DeviceConnectedCb deviceConnectedCb) {
+    /* Callback called if device connected successfully. */
+    _deviceConnectedCallback = deviceConnectedCb;
+}
+
+void BluetoothDispatcher::sendPackage(const char *package, size_t size) {
+    _controller->write((uint8_t *)package, size);
 }
 
 
 static void deviceConnectedStaticCallback(esp_spp_cb_event_t event, esp_spp_cb_param_t *param) {
-    if (event == ESP_SPP_SRV_OPEN_EVT and param->srv_open.status == ESP_SPP_SUCCESS and deviceConnectedCallback) {
+    if (event == ESP_SPP_SRV_OPEN_EVT and param->srv_open.status == ESP_SPP_SUCCESS
-        deviceConnectedCallback(param->srv_open.rem_bda);
+        and deviceConnectedCallback) {
+        deviceConnectedCallback(BTAddress(param->srv_open.rem_bda));
     }
 }

RF/BluetoothDispatcher.hpp

@@ -1,5 +1,6 @@
 #include "BluetoothSerial.h"
 #include "HardwareSerial.h"
+#include "esp_log.h"
 #include "mString.h"
 
 /* Check the ESP configuration */
@@ -18,24 +19,31 @@
 #endif
 /* ************************* */
 
-typedef std::function<void(esp_bd_addr_t bt_addr)> DeviceConnectedCb;
+typedef std::function<void(BTAddress device)> DeviceConnectedCb;
+typedef std::function<void(char *package)> NewPackageCallback;
 
 class BluetoothDispatcher {
   public:
-    BluetoothDispatcher(BluetoothSerial *controller, const char *device_name = "Helicopter", HardwareSerial *serial = &Serial);
+    BluetoothDispatcher(BluetoothSerial *controller, const char *device_name = "Helicopter");
-    bool initialize();
+    bool initialize(bool loop_on_fail = true, int readTimeoutMS = 2);
     void tick();
+    void onNewPackageReceived(NewPackageCallback newPackageReceivedCb);
+    void onNewDeviceConnected(DeviceConnectedCb deviceConnectedCb);
+    void sendPackage(const char *package, size_t size);
     ~BluetoothDispatcher();
 
   private:
-    void _confirmRequestCallback(uint16_t pin);
+    void _onConfirmRequest(uint16_t pin);
-    void _authCompleteCallback(boolean success);
+    void _onAuthComplete(boolean success) const;
-    void _deviceConnectedCallback(esp_bd_addr_t bt_addr);
+    void _onDeviceConnected(BTAddress device) const;
 
     const char *_device_name = nullptr;
-    bool _confirmRequestDone = false;
     BluetoothSerial *_controller = nullptr;
-    HardwareSerial *_serial = nullptr;
+    DeviceConnectedCb _deviceConnectedCallback = nullptr;
-    static constexpr uint16_t _buffer_size = 256;
+    NewPackageCallback _newPackageReceivedCb = nullptr;
+    constexpr static const char *_tag = "BluetoothDispatcher";
+
+    static constexpr uint16_t _buffer_size = 522;
+    static constexpr uint16_t _available_buffer_size = 512;
     mString<_buffer_size> _buffer;
 };

Sensors/Barometer.hpp

@@ -1,5 +1,7 @@
 #include "GyverBME280.h"
 
+typedef std::function<void()> OnMeasuringFinishedCb;
+
 class Barometer {
   public:
     Barometer(GyverBME280 *bme) {
@@ -28,7 +30,7 @@ class Barometer {
     }
 
     void measureBaseAltitudeAsync(void) {
-        _isStartCalibration = true;
+        _isCalibration = true;
     }
 
     float altitude() /* [cm] */ {
@@ -39,14 +41,21 @@ class Barometer {
         return altitude() - _startedAltitude;
     }
 
+    void onMeasuaringHeightFinished(OnMeasuringFinishedCb callback) {
+        _measuaringHeightFinishedCb = callback;
+    }
+
     void tick() {
-        if (_isStartCalibration) {
+        if (_isCalibration) {
-            if (_isStartCalibration && millis() - _calibrationTimer >= _calibrationIterationDelay) {
+            if (millis() - _calibrationTimer >= _calibrationIterationDelay) {
                 _startedAltitude += altitude();
             }
             if (++_calibrationIterationsCounter >= _calibrationIterationsCount) {
                 _startedAltitude /= _calibrationIterationsCount;
-                _isStartCalibration = false;
+                _isCalibration = false;
+                if (_measuaringHeightFinishedCb) {
+                    _measuaringHeightFinishedCb();
+                }
             }
         }
     }
@@ -54,9 +63,10 @@ class Barometer {
   private:
     GyverBME280 *_bme;
     float _startedAltitude = 0;
-    bool _isStartCalibration = false;
+    bool _isCalibration = false;
-    int _calibrationTimer;
+    uint32_t _calibrationTimer;
     int _calibrationIterationsCounter = 0;
+    OnMeasuringFinishedCb _measuaringHeightFinishedCb = nullptr;
     static constexpr int _calibrationIterationsCount = 1500;
     static constexpr int _calibrationIterationDelay = 1;  // [ms]
 };

Sensors/MPU.hpp

@@ -1,4 +1,5 @@
 #include "MPU6050_6Axis_MotionApps20.h"
+#include "Preferences.h"
 #include "board_pins.h"
 #include "esp_log.h"
 
@@ -7,6 +8,8 @@ static void IRAM_ATTR _dmpInterruption() {
     _isDMPDataReady = true;
 }
 
+typedef std::function<void()> OnCalibrationFinishedCb;
+
 class MPU {
   public:
     MPU(MPU6050_6Axis_MotionApps20 *mpu) {
@@ -20,6 +23,13 @@ class MPU {
 
     bool initialize() {
         _mpu->initialize();
+
+        if (!_preferences.begin("imu")) {
+            ESP_LOGE(_TAG, "Failed to open the preferences!");
+        } else {
+            _loadOffsets();
+        }
+
         delay(250);
         if (!_mpu->testConnection()) {
             ESP_LOGE(_TAG, "MPU6050 test connection failed!");
@@ -52,12 +62,14 @@ class MPU {
         Quaternion q;
         VectorFloat gravity;
         VectorInt16 accel;
+        VectorInt16 gyro;
         VectorInt16 accelReal;
 
         _mpu->dmpGetQuaternion(&q, _fifoBuffer);
         _mpu->dmpGetGravity(&gravity, &q);
         _mpu->dmpGetYawPitchRoll(_ypr, &q, &gravity);
         _mpu->dmpGetAccel(&accel, _fifoBuffer);
+        _mpu->dmpGetGyro(&gyro, _fifoBuffer);
         _mpu->dmpGetLinearAccel(&accelReal, &accel, &gravity);
         _isDMPDataReady = false;
 
@@ -65,51 +77,121 @@ class MPU {
         _ay = accel.y / 8192.f;
         _az = accel.z / 8192.f;
 
-        _gx = q.x / 32768.f * 250.f;
+        _gx = gyro.x / 131.f;
-        _gy = q.y / 32768.f * 250.f;
+        _gy = gyro.y / 131.f;
-        _gz = q.z / 32768.f * 250.f;
+        _gz = gyro.z / 131.f;
 
         _calculateZInertial(gravity);
+
+        if (_isCalibration) {
+            if (_calibrationsIterCounter >= _calibrationIterCount) {
+                _axOffset /= _calibrationsIterCounter;
+                _ayOffset /= _calibrationsIterCounter;
+                _azOffset /= _calibrationsIterCounter;
+
+                _gxOffset /= _calibrationsIterCounter;
+                _gyOffset /= _calibrationsIterCounter;
+                _gzOffset /= _calibrationsIterCounter;
+
+                _prOffset[0] /= _calibrationsIterCounter;
+                _prOffset[1] /= _calibrationsIterCounter;
+
+                _isCalibration = false;
+                _updateOffsets();
+                if (_calibrationFinishedCb) {
+                    _calibrationFinishedCb();
+                }
+            } else {
+                _axOffset += _ax;
+                _ayOffset += _ay;
+                _azOffset += _az;
+                _gxOffset += _gx;
+                _gyOffset += _gy;
+                _gzOffset += _gz;
+                _prOffset[0] += _ypr[1];
+                _prOffset[1] += _ypr[2];
+            }
+        }
+
         return true;
     }
 
     /* getters */
-    float accZInertial() {
+    float accZInertial() const {
         return _AccZInertial;
     }
-    float yaw() {
+    float yaw() const {
         return degrees(_ypr[0]);
-    };
-    float pitch() {
-        return degrees(_ypr[1]);
-    };
-    float roll() {
-        return degrees(_ypr[2]);
-    };
-
-    float ax() {
-        return _ax;
     }
-    float ay() {
+    float pitch() const {
-        return _ay;
+        if (_isCalibration) {
+            return degrees(_ypr[1]);
+        } else {
+            return degrees(_ypr[1] - _yprOffset[1]);
+        }
     }
-    float az() {
+    float roll() const {
-        return _az;
+        if (_isCalibration) {
+            return degrees(_ypr[2]);
+        } else {
+            return degrees(_ypr[2] - _yprOffset[2]);
+        }
     }
 
-    float gravityZ() {
+    float ax() const {
+        if (_isCalibration) {
+            return _ax;
+        } else {
+            return _ax - _axOffset;
+        }
+    }
+    float ay() const {
+        if (_isCalibration) {
+            return _ay;
+        } else {
+            return _ay - _ayOffset;
+        }
+    }
+    float az() const {
+        if (_isCalibration) {
+            return _az;
+        } else {
+            return _az - _azOffset;
+        }
+    }
+
+    float gravityZ() const {
         return _gravity;
     }
 
+    float gx() const {
+        if (_isCalibration) {
+            return _gx;
+        } else {
+            return _gx - _gxOffset;
+        }
+    }
+    float gy() const {
+        if (_isCalibration) {
+            return _gy;
+        } else {
+            return _gy - _gyOffset;
+        }
+    }
+    float gz() const {
+        if (_isCalibration) {
+            return _gz;
+        } else {
+            return _gz - _gzOffset;
+        }
+    }
 
-    float gx() {
+    void startCalibration() {
-        return _gx;
+        _isCalibration = true;
     }
-    float gy() {
+
-        return _gy;
+    void onCalibrationFinished(OnCalibrationFinishedCb callback) {
-    }
+        _calibrationFinishedCb = callback;
-    float gz() {
-        return _gz;
     }
 
   private:
@@ -119,16 +201,50 @@ class MPU {
     float _gx, _gy, _gz;
     float _gravity;
     float _AccZInertial = 0;
+
+    float _axOffset = 0, _ayOffset = 0, _azOffset = 0;
+    float _gxOffset = 0, _gyOffset = 0, _gzOffset = 0;
+    float _prOffset[2]; // yaw isn't used
+
     uint8_t _fifoBuffer[45];
+    Preferences _preferences;
 
     const char *_TAG = "MPU6050 module";
+    static constexpr const int _calibrationIterCount = 250;
+
+    bool _isCalibration = false;
+    int _calibrationsIterCounter = 0;
+    OnCalibrationFinishedCb _calibrationFinishedCb = nullptr;
 
     void _calculateZInertial(VectorFloat &gravity) {
         const float anglePitch = _ypr[1];
         const float angleRoll = _ypr[2];
         _gravity = gravity.z;
 
-        _AccZInertial = -sin(anglePitch) * _ax + cos(anglePitch) * sin(angleRoll) * _ay + cos(anglePitch) * cos(angleRoll) * _az;
+        _AccZInertial = -sin(anglePitch) * _ax + cos(anglePitch) * sin(angleRoll) * _ay
-        _AccZInertial = (_AccZInertial - 1) * gravity.z * 100;
+                        + cos(anglePitch) * cos(angleRoll) * _az;
+        _AccZInertial = (_AccZInertial - 1) * gravity.z * 100.f;
+    }
+
+    void _loadOffsets() {
+        _axOffset = _preferences.getFloat("ax", 0.f);
+        _ayOffset = _preferences.getFloat("ax", 0.f);
+        _azOffset = _preferences.getFloat("az", 0.f);
+        _gxOffset = _preferences.getFloat("gx", 0.f);
+        _gyOffset = _preferences.getFloat("gy", 0.f);
+        _gzOffset = _preferences.getFloat("gz", 0.f);
+        _ypr[1] = _preferences.getFloat("pitch", 0.f);
+        _ypr[2] = _preferences.getFloat("roll", 0.f);
+    }
+
+    void _updateOffsets() {
+        _preferences.putFloat("ax", _axOffset);
+        _preferences.putFloat("ay", _ayOffset);
+        _preferences.putFloat("az", _azOffset);
+        _preferences.putFloat("gx", _gxOffset);
+        _preferences.putFloat("gy", _gyOffset);
+        _preferences.putFloat("gz", _gzOffset);
+        _preferences.putFloat("pitch", _prOffset[0]);
+        _preferences.putFloat("roll", _prOffset[1]);
     }
 };

Sensors/Sensors.cpp

@@ -1,10 +1,15 @@
 #include "Sensors.hpp"
 
-Sensors::Sensors(Barometer &barometer, MPU &mpu, Kalman2DFilter &filter, BatteryController &battery, bool loop_on_fail) {
+Sensors::Sensors(Barometer *barometer, MPU *mpu, Kalman2DFilter *filter, BatteryController *battery, bool loop_on_fail) {
-    _barometer = &barometer;
+    assert(barometer != nullptr);
-    _mpu = &mpu;
+    assert(mpu != nullptr);
-    _2d_filter = &filter;
+    assert(filter != nullptr);
-    _battery = &battery;
+    assert(battery != nullptr);
+
+    _barometer = barometer;
+    _mpu = mpu;
+    _2d_filter = filter;
+    _battery = battery;
 
     ESP_LOGI(_tag, "Initialize BMP280...");
     if (_barometer->initialize()) {
@@ -44,12 +49,12 @@ void Sensors::measureBaseAltitudeAsync(void) {
     _barometer->measureBaseAltitudeAsync();
 }
 
-float Sensors::rawFlightHeight(void) {
+float Sensors::rawFlightHeight(void) const {
     /* Returns flight height from barometer immediatly */
     return _barometer->flightHeight();
 }
 
-float Sensors::flightHeight(void) {
+float Sensors::flightHeight(void) const {
     /* Returns flight height from cache immediatly */
     if (_flightHeightFromBarometer == NAN) {
         ESP_LOGW(_tag, "flightHeight called, but tick() has called never");
@@ -59,8 +64,8 @@ float Sensors::flightHeight(void) {
     }
 }
 
-MpuData Sensors::mpuData() {
+MpuData Sensors::mpuData() const {
-    if (_mpuData.ax == NAN and _mpuData.gravityZ == NAN) {
+    if (_mpuData.ax == NAN and _mpuData.gravity == NAN) {
         ESP_LOGW(_tag, "MPU data looks like .tick() has never called");
     }
     return _mpuData;
@@ -69,6 +74,7 @@ MpuData Sensors::mpuData() {
 bool Sensors::tick(void) {
     /* Super loop iteraion */
     /* Return true on update */
+    _barometer->tick();
     bool err;
     bool isMpuDataReady = _mpu->tick(err);
     if (isMpuDataReady and !err) {
@@ -82,7 +88,8 @@ bool Sensors::tick(void) {
                      .yaw = _mpu->yaw(),
                      .pitch = _mpu->pitch(),
                      .roll = _mpu->roll(),
-                     .gravityZ = _mpu->gravityZ() };
+                     .gravity = _mpu->gravityZ(),
+                     .zInertial = _mpu->accZInertial() };
         return true;
     } else if (err) {
         ESP_LOGE(_tag, "Failed to get DMP data!");
@@ -90,6 +97,18 @@ bool Sensors::tick(void) {
     return false;
 }
 
-int Sensors::batteryCharge(void) {
+int Sensors::batteryCharge(void) const {
     return _battery->percent();
+}
+
+void Sensors::onMpuCalibrationFinished(OnCalibrationFinishedCb callback) {
+    _mpu->onCalibrationFinished(callback);
+}
+
+void Sensors::startMpuCalibration() {
+    _mpu->startCalibration();
+}
+
+void Sensors::onMeasuaringAltitudeFinished(OnMeasuringFinishedCb callback) {
+    _barometer->onMeasuaringHeightFinished(callback);
 }

Sensors/Sensors.hpp

@@ -10,23 +10,28 @@ struct MpuData {
     float ax, ay, az;
     float gx, gy, gz;
     float yaw, pitch, roll;
-    float gravityZ;
+    float gravity;
+    float zInertial;
 };
 
 class Sensors {
   public:
-    Sensors(Barometer &barometer, MPU &mpu, Kalman2DFilter &filter, BatteryController &battery, bool loop_on_fail = true);
+    Sensors(Barometer *barometer, MPU *mpu, Kalman2DFilter *filter, BatteryController *battery, bool loop_on_fail = true);
 
     ~Sensors();
 
     void measureBaseAltitudeSync(void);
     void measureBaseAltitudeAsync(void);
-    float rawFlightHeight(void);
+    void onMeasuaringAltitudeFinished(OnMeasuringFinishedCb callback);
-    float flightHeight(void);
+    float rawFlightHeight(void) const;
+    float flightHeight(void) const;
 
-    MpuData mpuData(void);
+    void startMpuCalibration();
+    void onMpuCalibrationFinished(OnCalibrationFinishedCb callback);
 
-    int batteryCharge(void);  // [%]
+    MpuData mpuData(void) const;
+
+    int batteryCharge(void) const;  // [%]
 
     bool tick(void);
 
@@ -39,7 +44,7 @@ class Sensors {
     /* cached filtered values */
     float _flightHeightFromBarometer = NAN;
     float _zVelocityAltitude = NAN;
-    MpuData _mpuData = { NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN };
+    MpuData _mpuData = { NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN };
 
 
     static constexpr const char *_tag = "Sensors";

main.cpp

@@ -1,52 +1,36 @@
-#include "Filters/Kalman2DFilter.hpp"
+#include "App.hpp"
-#include "GyverBME280.h"
+#include "BoardI2C.hpp"
-#include "I2Cdev.h"
+//#include "MPU6050_6Axis_MotionApps20.h"
-#include "MPU6050_6Axis_MotionApps20.h"
-#include "RF/BluetoothDispatcher.hpp"
-#include "Sensors/Barometer.hpp"
-#include "Sensors/BatteryController.hpp"
-#include "Sensors/MPU.hpp"
-#include "Wire.h"
-#include "board_pins.h"
-#include <Arduino.h>
 
-TwoWire i2c = TwoWire(0);
+BoardI2C i2c;
-Barometer barometer(new GyverBME280(i2c));
+/*
-MPU mpu(new MPU6050(MPU6050_DEFAULT_ADDRESS, &i2c));
-Kalman2DFilter kalman2d(10.f, 1.f, 1.8f);
-BatteryController battery;
 BluetoothDispatcher bluetoothDispatcher(new BluetoothSerial());
+Sensors *sensors = nullptr; */
+
+static Application *app = nullptr;
 
 void setup() {
     Serial.begin(115200);
-
+    app = new Application(new FlightDispatcher(
-    Serial.print("Ininitialize I2C...");
+      new BluetoothDispatcher(new BluetoothSerial()),
-    Serial.println(i2c.begin(I2C_SDA_PIN, I2C_SCL_PIN, 400000));
+      new FlightController(
-
+        new Sensors(new Barometer(new GyverBME280(i2c)), new MPU(new MPU6050(MPU6050_DEFAULT_ADDRESS, &i2c)),
-    Serial.print("Ininitialize BMP280...");
+                    new Kalman2DFilter(10.f, 1.f, 1.8f), new BatteryController()),
-    Serial.println(barometer.initialize());
+        new PIDController(1, 1, 1, new BrushedMotor(1, 2, 3, 4, 1), 1),
-
+        new PIDController(1, 1, 1, new BrushedMotor(1, 2, 3, 4, 2), 2),
-    Serial.print("Ininitialize MPU6050...");
+        new PIDController(1, 1, 1, new BrushedMotor(1, 2, 3, 4, 3), 3))));
-    Serial.println(mpu.initialize());
+    /*sensors = new Sensors(
-
+    new Barometer(new GyverBME280(i2c)), new MPU(new
-    Serial.print("Ininitialize Bluetooth...");
+  MPU6050(MPU6050_DEFAULT_ADDRESS, &i2c)), new Kalman2DFilter(10.f, 1.f, 1.8f),
-    Serial.println(bluetoothDispatcher.initialize());
+  new BatteryController()); sensors->measureBaseAltitudeSync();
-
+  bluetoothDispatcher.initialize();
-    battery.initialize();
+  Serial.println("System initialized");*/
-    barometer.measureBaseAltitudeSync();
-
-    Serial.println("System initialized");
 }
 
 void loop() {
-    bool err;
+    app->tick();
-    barometer.tick();
+    /*
-    if (mpu.tick(err) && !err) {
+  sensors->tick();
-        float kalmanAltitude, ZVelocityAltitude;
+  bluetoothDispatcher.tick();
-        float barometerAltitude = barometer.flightHeight();
+  delay(1);*/
-        kalman2d.filter(mpu.accZInertial(), barometerAltitude, kalmanAltitude, ZVelocityAltitude);
-        //Serial.print(barometerAltitude);
-    }
-    bluetoothDispatcher.tick();
-    delay(1);
 }