Project structure redefined

src/.clang-format

@@ -0,0 +1,160 @@
+# Source: https://github.com/arduino/tooling-project-assets/tree/main/other/clang-format-configuration
+---
+TabWidth: 2
+AccessModifierOffset: -2
+AlignAfterOpenBracket: Align
+AlignConsecutiveAssignments: None
+AlignConsecutiveBitFields: None
+AlignConsecutiveDeclarations: None
+AlignConsecutiveMacros: None
+AlignEscapedNewlines: DontAlign
+AlignOperands: Align
+AlignTrailingComments: true
+AllowAllArgumentsOnNextLine: true
+AllowAllConstructorInitializersOnNextLine: true
+AllowAllParametersOfDeclarationOnNextLine: true
+AllowShortBlocksOnASingleLine: Always
+AllowShortCaseLabelsOnASingleLine: true
+AllowShortEnumsOnASingleLine: true
+AllowShortFunctionsOnASingleLine: Empty
+AllowShortLambdasOnASingleLine: Empty
+AllowShortLoopsOnASingleLine: true
+AlwaysBreakAfterDefinitionReturnType: None
+AlwaysBreakAfterReturnType: None
+AlwaysBreakBeforeMultilineStrings: false
+AlwaysBreakTemplateDeclarations: No
+AttributeMacros:
+  - __capability
+BinPackArguments: true
+BinPackParameters: true
+BitFieldColonSpacing: Both
+BraceWrapping:
+  AfterCaseLabel: false
+  AfterClass: false
+  AfterControlStatement: Never
+  AfterEnum: false
+  AfterFunction: false
+  AfterNamespace: false
+  AfterObjCDeclaration: false
+  AfterStruct: false
+  AfterUnion: false
+  AfterExternBlock: false
+  BeforeCatch: false
+  BeforeElse: false
+  BeforeLambdaBody: false
+  BeforeWhile: false
+  IndentBraces: false
+  SplitEmptyFunction: true
+  SplitEmptyRecord: true
+  SplitEmptyNamespace: true
+BreakAfterJavaFieldAnnotations: false
+BreakBeforeBinaryOperators: NonAssignment
+BreakBeforeBraces: Attach
+BreakBeforeConceptDeclarations: false
+BreakBeforeInheritanceComma: false
+BreakBeforeTernaryOperators: true
+BreakConstructorInitializers: BeforeColon
+BreakConstructorInitializersBeforeComma: false
+BreakInheritanceList: BeforeColon
+BreakStringLiterals: false
+ColumnLimit: 90
+CommentPragmas: ''
+CompactNamespaces: false
+ConstructorInitializerAllOnOneLineOrOnePerLine: false
+ConstructorInitializerIndentWidth: 2
+ContinuationIndentWidth: 2
+Cpp11BracedListStyle: false
+DeriveLineEnding: true
+DerivePointerAlignment: true
+DisableFormat: false
+EmptyLineBeforeAccessModifier: Leave
+ExperimentalAutoDetectBinPacking: false
+FixNamespaceComments: false
+ForEachMacros:
+  - foreach
+  - Q_FOREACH
+  - BOOST_FOREACH
+IncludeBlocks: Preserve
+IncludeCategories:
+  - Regex: '^"(llvm|llvm-c|clang|clang-c)/'
+    Priority: 2
+    SortPriority: 0
+    CaseSensitive: false
+  - Regex: '^(<|"(gtest|gmock|isl|json)/)'
+    Priority: 3
+    SortPriority: 0
+    CaseSensitive: false
+  - Regex: '.*'
+    Priority: 1
+    SortPriority: 0
+    CaseSensitive: false
+IncludeIsMainRegex: ''
+IncludeIsMainSourceRegex: ''
+IndentGotoLabels: false
+IndentPPDirectives: None
+IndentRequires: true
+IndentWidth: 2
+IndentWrappedFunctionNames: false
+InsertTrailingCommas: None
+JavaScriptQuotes: Leave
+JavaScriptWrapImports: true
+KeepEmptyLinesAtTheStartOfBlocks: true
+Language: Cpp
+MacroBlockBegin: ''
+MacroBlockEnd: ''
+MaxEmptyLinesToKeep: 100000
+NamespaceIndentation: None
+ObjCBinPackProtocolList: Auto
+ObjCBlockIndentWidth: 2
+ObjCBreakBeforeNestedBlockParam: true
+ObjCSpaceAfterProperty: false
+ObjCSpaceBeforeProtocolList: true
+PenaltyBreakAssignment: 1
+PenaltyBreakBeforeFirstCallParameter: 1
+PenaltyBreakComment: 1
+PenaltyBreakFirstLessLess: 1
+PenaltyBreakString: 1
+PenaltyBreakTemplateDeclaration: 1
+PenaltyExcessCharacter: 1
+PenaltyIndentedWhitespace: 1
+PenaltyReturnTypeOnItsOwnLine: 1
+PointerAlignment: Right
+ReflowComments: false
+SortJavaStaticImport: Before
+SortUsingDeclarations: false
+SpaceAfterCStyleCast: false
+SpaceAfterLogicalNot: false
+SpaceAfterTemplateKeyword: false
+SpaceAroundPointerQualifiers: Default
+SpaceBeforeAssignmentOperators: true
+SpaceBeforeCaseColon: false
+SpaceBeforeCpp11BracedList: false
+SpaceBeforeCtorInitializerColon: true
+SpaceBeforeInheritanceColon: true
+SpaceBeforeParens: ControlStatements
+SpaceBeforeRangeBasedForLoopColon: true
+SpaceBeforeSquareBrackets: false
+SpaceInEmptyBlock: false
+SpaceInEmptyParentheses: false
+SpacesBeforeTrailingComments: 2
+SpacesInCStyleCastParentheses: false
+SpacesInConditionalStatement: false
+SpacesInContainerLiterals: false
+SpacesInParentheses: false
+SpacesInSquareBrackets: false
+Standard: Auto
+StatementAttributeLikeMacros:
+  - Q_EMIT
+StatementMacros:
+  - Q_UNUSED
+  - QT_REQUIRE_VERSION
+TabWidth: 2
+UseCRLF: false
+UseTab: Never
+WhitespaceSensitiveMacros:
+  - STRINGIZE
+  - PP_STRINGIZE
+  - BOOST_PP_STRINGIZE
+  - NS_SWIFT_NAME
+  - CF_SWIFT_NAME
+IndentWidth: 4

src/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";
+};

src/BoardI2C.hpp

@@ -0,0 +1,16 @@
+#include "Wire.h"
+#include "board_pins.h"
+#include "esp_log.h"
+
+class BoardI2C : public TwoWire {
+  public:
+    BoardI2C(volatile bool loop_on_fail = true) : TwoWire(0) {
+        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!");
+            while (loop_on_fail)
+                ;
+        }
+    }
+};

src/Filters/Kalman2DFilter.hpp

@@ -0,0 +1,43 @@
+#include <BasicLinearAlgebra.h>
+
+class Kalman2DFilter {
+  public:
+    Kalman2DFilter(float dt = 4.f, float accelUncertainty = 10.f, float barometerUncertainty = 100.f) {
+        dt /= 1000.f;
+        F = { 1, dt, 0, 1 };
+        G = { 0.5f * dt * dt, dt };
+        H = { 1, 0 };
+        I = { 1, 0, 0, 1 };
+        Q = G * ~G * accelUncertainty * accelUncertainty;
+        R = { barometerUncertainty * barometerUncertainty };
+        P = { 0, 0, 0, 0 };
+        S = { 0, 0 };
+    }
+
+    void filter(const float &AccZInertial, const float &AltitudeBarometer, float &AltitudeKalman, float &VelocityVerticalKalman) {
+        Acc = { AccZInertial };
+        S = F * S + G * Acc;
+        P = F * P * ~F + Q;
+        L = H * P * ~H + R;
+        K = P * ~H * Inverse(L);
+        M = { AltitudeBarometer };
+        S = S + K * (M - H * S);
+        AltitudeKalman = S(0, 0);
+        VelocityVerticalKalman = S(1, 0);
+        P = (I - K * H) * P;
+    }
+
+  private:
+    BLA::Matrix<2, 2> F;
+    BLA::Matrix<2, 1> G;
+    BLA::Matrix<2, 2> P;
+    BLA::Matrix<2, 2> Q;
+    BLA::Matrix<2, 1> S;
+    BLA::Matrix<1, 2> H;
+    BLA::Matrix<2, 2> I;
+    BLA::Matrix<1, 1> Acc;
+    BLA::Matrix<2, 1> K;
+    BLA::Matrix<1, 1> R;
+    BLA::Matrix<1, 1> L;
+    BLA::Matrix<1, 1> M;
+};

src/Filters/kalman.hpp

@@ -0,0 +1,46 @@
+// упрощённый Калман для одномерного случая
+
+#ifndef _GKalman_h
+#define _GKalman_h
+
+class GKalman {
+public:
+    // разброс измерения, разброс оценки, скорость изменения значений
+    GKalman(float mea_e, float est_e, float q) {
+        setParameters(mea_e, est_e, q);
+    }
+    
+    // разброс измерения, скорость изменения значений (разброс измерения принимается равным разбросу оценки)
+    GKalman(float mea_e, float q) {
+        setParameters(mea_e, mea_e, q);
+    }
+    
+    // разброс измерения, разброс оценки, скорость изменения значений
+    void setParameters(float mea_e, float est_e, float q) {
+        _err_measure = mea_e;
+        _err_estimate = est_e;
+        _q = q;
+    }
+    
+    // разброс измерения, скорость изменения значений (разброс измерения принимается равным разбросу оценки)
+    void setParameters(float mea_e, float q) {
+        setParameters(mea_e, mea_e, q);
+    }
+    
+    // возвращает фильтрованное значение
+    float filtered(float value) {		
+        float _kalman_gain, _current_estimate;
+        _kalman_gain = _err_estimate / (_err_estimate + _err_measure);
+        _current_estimate = _last_estimate + _kalman_gain * (value - _last_estimate);
+        _err_estimate =  (1.0 - _kalman_gain)*_err_estimate + fabs(_last_estimate-_current_estimate)*_q;
+        _last_estimate=_current_estimate;
+        return _current_estimate;
+    }
+
+private:
+    float _err_measure = 0.0;
+    float _err_estimate = 0.0;
+    float _q = 0.0;
+    float _last_estimate = 0.0;
+};
+#endif

src/Filters/median3.hpp

@@ -0,0 +1,19 @@
+// быстрый медианный фильтр 3-го порядка
+
+#ifndef _GMedian3_h
+#define _GMedian3_h
+
+template < typename TYPE >
+class GMedian3 {
+public:
+    TYPE filtered(TYPE value) {	// возвращает фильтрованное значение
+        buf[_counter] = value;
+        if (++_counter > 2) _counter = 0;
+        return (max(buf[0], buf[1]) == max(buf[1], buf[2])) ? max(buf[0], buf[2]) : max(buf[1], min(buf[0], buf[2]));
+    }
+    
+private:
+    TYPE buf[3];
+    uint8_t _counter = 0;
+};
+#endif

src/Filters/runningAverage.hpp

@@ -0,0 +1,49 @@
+// экспоненциальное бегущее среднее
+
+#ifndef _GFilterRA_h
+#define _GFilterRA_h
+
+class GFilterRA {
+public:
+    GFilterRA(){}
+    
+    GFilterRA(float coef, uint16_t interval) {
+        _coef = coef;
+        _prd = interval;
+    }
+
+    GFilterRA(float coef) {
+        _coef = coef;
+    }
+
+    void setCoef(float coef) {
+        _coef = coef;
+    }
+    
+    void setPeriod(uint16_t interval) {
+        _prd = interval;
+    }
+
+    float filteredTime(float value) {
+        if (millis() - _tmr >= _prd) {
+            _tmr += _prd;
+            filtered(value);
+        }
+        return _fil;
+    }
+
+    float filtered(float value) {
+        return _fil += (value - _fil) * _coef;
+    }
+    
+    //
+    void setStep(uint16_t interval) {
+        _prd = interval;
+    }
+    
+private:
+    float _coef = 0.0, _fil = 0.0;
+    uint32_t _tmr = 0;
+    uint16_t _prd = 0;
+};
+#endif

src/Logic/FlightController.cpp

@@ -0,0 +1,194 @@
+// This is a personal academic project. Dear PVS-Studio, please check it.
+// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
+
+#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;
+    _pid1 = pid1;
+    _pid2 = pid2;
+    _pid3 = pid3;
+
+    _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::IsImuCalibration: break;
+    case DeviceStatus::IsBoarding:
+        /* TODO: implement boarding */
+        break;
+    case DeviceStatus::IsFlying:
+        /* TODO: implement flying */
+        if (hasUpdates) {
+            _pid1->tick();
+            _pid2->tick();
+            _pid3->tick();
+        }
+        break;
+    }
+}
+
+ReportedDeviceState FlightController::currentDeviceState() const {
+    return { .batteryCharge = _sensors->batteryCharge(),
+             .flightHeight = _sensors->flightHeight(),
+             .status = _status,
+             .mpuState = _sensors->mpuData() };
+}
+
+void FlightController::startTakeoff() {
+    /* Start preparation for takeoff */
+    _updateBatteryCharge();
+    if (_status != DeviceStatus::Idle)
+        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) {
+            startBoarding();
+        } 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() {
+    if (_status != DeviceStatus::Idle)
+        return;
+    _sensors->onMpuCalibrationFinished([this]() {
+        this->_status = DeviceStatus::Idle;
+    });
+}
+
+void FlightController::moveToFlightHeight(float newFlightHeight) {
+    // TODO: implement setting flight height
+}
+
+void FlightController::newFlightHeightStickPosition(int16_t newFlightHeightStickPostition) {
+    // TODO: implement stick input
+}
+
+void FlightController::newYawStickPosition(int16_t newYawStickPostition) {
+    // TODO: implement stick input
+}
+
+void FlightController::newPitchStickPosition(int16_t newYawStickPostition) {
+    // TODO: implement stick input
+}
+
+void FlightController::newRotor1Duty(uint32_t rotor1Duty) {
+    if (_status == DeviceStatus::Idle) {
+        _pid1->setRotorDuty(rotor1Duty);
+    }
+}
+
+void FlightController::newRotor2Duty(uint32_t rotor2Duty) {
+    if (_status == DeviceStatus::Idle) {
+        _pid2->setRotorDuty(rotor2Duty);
+    }
+}
+
+void FlightController::newRotor3Duty(uint32_t rotor3Duty) {
+    if (_status == DeviceStatus::Idle) {
+        _pid3->setRotorDuty(rotor3Duty);
+    }
+}
+
+void FlightController::stopAllRotors() {
+    _pid1->setRotorDuty(0);
+    _pid2->setRotorDuty(0);
+    _pid3->setRotorDuty(0);
+}
+
+void FlightController::setPid1Params(float p, float i, float d) {
+    if (_status == DeviceStatus::Idle) {
+        _pid1->Kp = p;
+        _pid1->Ki = i;
+        _pid1->Kd = d;
+        _pid1->save();
+    }
+}
+
+void FlightController::setPid2Params(float p, float i, float d) {
+    if (_status == DeviceStatus::Idle) {
+        _pid2->Kp = p;
+        _pid2->Ki = i;
+        _pid2->Kd = d;
+        _pid1->save();
+    }
+}
+
+void FlightController::setPid3Params(float p, float i, float d) {
+    if (_status == DeviceStatus::Idle) {
+        _pid3->Kp = p;
+        _pid3->Ki = i;
+        _pid3->Kd = d;
+        _pid1->save();
+    }
+}
+
+PidSettings *FlightController::pidSettings() const {
+    PidSettings pid1 = { .p = _pid1->Kp, .i = _pid1->Ki, .d = _pid1->Kd };
+    PidSettings pid2 = { .p = _pid2->Kp, .i = _pid2->Ki, .d = _pid2->Kd };
+    PidSettings pid3 = { .p = _pid3->Kp, .i = _pid3->Ki, .d = _pid3->Kd };
+    static PidSettings settings[] = { pid1, pid2, pid3 };
+    return settings;
+}

src/Logic/FlightController.hpp

@@ -0,0 +1,74 @@
+#include "PID.hpp"
+#include <memory>
+#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]
+};
+
+struct PidSettings {
+    float p;
+    float i;
+    float d;
+};
+
+class FlightController {
+  public:
+    FlightController(Sensors *sensors, PIDController *pid1, PIDController *pid2,
+                     PIDController *pid3, bool unlimitedBattery = false);
+    ~FlightController();
+    ReportedDeviceState currentDeviceState() const;
+    void startTakeoff();
+    void startBoarding();
+    void startImuCalibration();
+    void tick();
+    [[deprecated]] void moveToFlightHeight(float newFlightHeight);  //[cm]
+    [[deprecated]] void newFlightHeightStickPosition(int16_t newFlightHeightStickPostition);
+    [[deprecated]] void newYawStickPosition(int16_t newYawStickPostition);
+    [[deprecated]] void newPitchStickPosition(int16_t newPitchStickPostition);
+    void newRotor1Duty(uint32_t rotor1Duty);
+    void newRotor2Duty(uint32_t rotor2Duty);
+    void newRotor3Duty(uint32_t rotor3Duty);
+    void stopAllRotors();
+    void setPid1Params(float p, float i, float d);
+    void setPid2Params(float p, float i, float d);
+    void setPid3Params(float p, float i, float d);
+    PidSettings *pidSettings() const;
+  private:
+    void _updateBatteryCharge();
+    [[deprecated]] void _takeoff();
+    [[deprecated]] 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
+};

src/Logic/FlightDispatcher.cpp

@@ -0,0 +1,180 @@
+// This is a personal academic project. Dear PVS-Studio, please check it.
+// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
+
+#include "FlightDispatcher.hpp"
+
+FlightDispatcher::FlightDispatcher(BluetoothDispatcher *bluetoothDispatcher,
+                                   FlightController *flightController, volatile 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) {
+    using sutil::SH;
+    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",
+                 pkg.readError(), pkg.errorIndex(), package);
+    } else {
+        pkg.hashKeys();
+        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;
+            }
+
+            switch (pkg.keyHash(keyIndex)) {
+            case SH("status"): _changeStatus((DeviceStatus)value.toInt32()); break;
+            case SH("flightHeight"):
+                _flightController->moveToFlightHeight(value.toFloat());
+                break;
+            case SH("hS"):
+                _flightController->newFlightHeightStickPosition(value.toInt16());
+                break;
+            case SH("yS"): _flightController->newYawStickPosition(value.toInt16()); break;
+            case SH("pS"):
+                _flightController->newPitchStickPosition(value.toInt16());
+                break;
+            case SH("r1"): _flightController->newRotor1Duty(value.toInt32()); break;
+            case SH("r2"): _flightController->newRotor2Duty(value.toInt32()); break;
+            case SH("r3"): _flightController->newRotor3Duty(value.toInt32()); break;
+            case SH("stop"): _flightController->stopAllRotors(); break;
+            case SH("p1"):
+                if (pair.type() == gson::Type::Object and pair.includes(SH("p"))
+                    and pair.includes(SH("i")) and pair.includes(SH("d"))) {
+                    _flightController->setPid1Params(
+                      pair[SH("p")].toFloat(), pair[SH("i")].toFloat(), pair[SH("d")].toFloat());
+                }
+                break;
+            case SH("p2"):
+                if (pair.type() == gson::Type::Object and pair.includes(SH("p"))
+                    and pair.includes(SH("i")) and pair.includes(SH("d"))) {
+                    _flightController->setPid2Params(
+                      pair[SH("p")].toFloat(), pair[SH("i")].toFloat(), pair[SH("d")].toFloat());
+                }
+                break;
+            case SH("p3"):
+                if (pair.type() == gson::Type::Object and pair.includes(SH("p"))
+                    and pair.includes(SH("i")) and pair.includes(SH("d"))) {
+                    _flightController->setPid3Params(
+                      pair[SH("p")].toFloat(), pair[SH("i")].toFloat(), pair[SH("d")].toFloat());
+                }
+                break;
+            case SH("pidSettingOpened"): _pidSettingsOpened(); break;
+            default: break;
+            }
+        }
+    }
+}
+
+void FlightDispatcher::_changeStatus(const DeviceStatus &newStatus) {
+    switch (newStatus) {
+    case DeviceStatus::IsImuCalibration: _flightController->startImuCalibration(); break;
+    case DeviceStatus::IsFlying:
+        _changeStatus(DeviceStatus::IsPreparingForTakeoff);
+        break;
+    case DeviceStatus::IsPreparingForTakeoff: _flightController->startTakeoff(); break;
+    case DeviceStatus::IsBoarding: _flightController->startBoarding(); break;
+    default: break;
+    }
+}
+
+void FlightDispatcher::_pidSettingsOpened() {
+    auto settings = _flightController->pidSettings();
+    gson::string pkg;
+    pkg.beginObj();
+
+    pkg.beginObj("p1");
+    pkg["p"] = settings[0].p;
+    pkg["i"] = settings[0].i;
+    pkg["d"] = settings[0].d;
+    pkg.endObj();
+
+    pkg.beginObj("p2");
+    pkg["p"] = settings[1].p;
+    pkg["i"] = settings[1].i;
+    pkg["d"] = settings[1].d;
+    pkg.endObj();
+
+    pkg.beginObj("p3");
+    pkg["p"] = settings[2].p;
+    pkg["i"] = settings[2].i;
+    pkg["d"] = settings[2].d;
+    pkg.endObj();
+
+    pkg.endObj();
+    pkg.end();
+    pkg += "\r\n";
+    _bluetoothDispatcher->sendPackage(pkg.s.c_str(), strlen(pkg.s.c_str()));
+}
+
+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())));
+}

src/Logic/FlightDispatcher.hpp

@@ -0,0 +1,35 @@
+#include "FlightController.hpp"
+#include "GSON.h"
+#include "RF/BluetoothDispatcher.hpp"
+#include <map>
+
+class FlightDispatcher {
+    /* Deserialize state and update it in FlightController. */
+  public:
+    FlightDispatcher(BluetoothDispatcher *bluetoothDispatcher,
+                     FlightController *flightController, volatile bool loop_on_fail = true);
+    ~FlightDispatcher();
+    void tick();
+  private:
+    /* Telemetry flow */
+    void _sendTelemetry();
+
+    /* Events handlers */
+    void _onNewDeviceConnected(BTAddress device);
+    void _onNewMessageReceived(char *package);
+
+    /* Requests handlers */
+    void _changeStatus(const DeviceStatus &newStatus);
+    void _pidSettingsOpened();
+
+    /* Compile time settings */
+    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;
+
+    std::map<size_t, std::function<void(gson::Entry)>> _routes;
+};

src/Logic/PID.hpp

@@ -0,0 +1,56 @@
+#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;
+        this->_dt = dt;
+        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 setRotorDuty(uint32_t duty) {
+        /* Used only for test purposes */
+        _rotor->setDuty(duty);
+    }
+
+    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);
+        }
+    }
+};

src/Motor/BrushedMotor.cpp

@@ -0,0 +1,30 @@
+// This is a personal academic project. Dear PVS-Studio, please check it.
+// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
+
+#include "BrushedMotor.hpp"
+
+BrushedMotor::BrushedMotor(uint32_t PwmAGpioNum, uint32_t PwmBGpioNum, uint32_t PwmFreqHz,
+                           uint32_t McpwmResolutionHz, int McpwmGroupId) {
+    _mcpwmResolutionHz = McpwmResolutionHz;
+    _pwmAGpioNum = McpwmGroupId;
+    _pwmBGpioNum = PwmBGpioNum;
+    _pwmFreqHz = PwmFreqHz;
+}
+
+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() {}

src/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;
+};

src/RF/BluetoothDispatcher.cpp

@@ -0,0 +1,105 @@
+// This is a personal academic project. Dear PVS-Studio, please check it.
+// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
+
+#include "BluetoothDispatcher.hpp"
+
+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) {
+    assert(controller != nullptr);
+    assert(device_name != nullptr);
+
+    _device_name = device_name;
+    _controller = controller;
+}
+
+bool BluetoothDispatcher::initialize(volatile bool loop_on_fail, int readTimeoutMS) {
+    _controller->enableSSP();
+    _controller->onConfirmRequest([this](uint16_t pin) {
+        _onConfirmRequest(pin);
+    });
+    _controller->onAuthComplete([this](boolean success) {
+        _onAuthComplete(success);
+    });
+    _controller->register_callback(deviceConnectedStaticCallback);
+    deviceConnectedCallback = [this](BTAddress device) {
+        _onDeviceConnected(device);
+    };
+    _controller->setTimeout(2);
+    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("\n\r")) {
+        char buffer[_buffer_size];
+        _buffer.substring(0, _buffer.lastIndexOf('}'), buffer);
+        ESP_LOGD(_tag, "Received new buffer %s", buffer);
+        if (_newPackageReceivedCb) {
+            _newPackageReceivedCb(buffer);
+        }
+        _buffer.clear();
+    }
+    if (_buffer.length() > _available_buffer_size) {
+        _buffer.clear();
+    }
+}
+
+BluetoothDispatcher::~BluetoothDispatcher() {
+    _controller->end();
+    delete _controller;
+}
+
+void BluetoothDispatcher::_onConfirmRequest(uint16_t pin) {
+    ESP_LOGI(_tag, "The Bluetooth PIN is: %06lu", (long unsigned int)pin);
+    _controller->confirmReply(true);
+}
+
+void BluetoothDispatcher::_onAuthComplete(boolean success) const {
+    if (success) {
+        ESP_LOGI(_tag, "Pairing success!");
+    } else {
+        ESP_LOGI(_tag, "Pairing failed, rejected by user!");
+    }
+}
+
+void BluetoothDispatcher::_onDeviceConnected(BTAddress device) const {
+    ESP_LOGI(_tag, "New device connected: %s", device.toString(true).c_str());
+    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) {
+        deviceConnectedCallback(BTAddress(param->srv_open.rem_bda));
+    }
+}

src/RF/BluetoothDispatcher.hpp

@@ -0,0 +1,49 @@
+#include "BluetoothSerial.h"
+#include "HardwareSerial.h"
+#include "esp_log.h"
+#include "mString.h"
+
+/* Check the ESP configuration */
+#if not defined(CONFIG_BT_ENABLED) || not defined(CONFIG_BLUEDROID_ENABLED)
+#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
+#endif
+
+
+#if not defined(CONFIG_BT_SPP_ENABLED)
+#error Serial Port Profile for Bluetooth is not available or not enabled. It is only available for the ESP32 chip.
+#endif
+
+
+#if not defined(CONFIG_BT_SSP_ENABLED)
+#error Simple Secure Pairing for Bluetooth is not available or not enabled.
+#endif
+/* ************************* */
+
+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");
+    bool initialize(volatile 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 _onConfirmRequest(uint16_t pin);
+    void _onAuthComplete(boolean success) const;
+    void _onDeviceConnected(BTAddress device) const;
+
+    const char *_device_name = nullptr;
+    BluetoothSerial *_controller = nullptr;
+    DeviceConnectedCb _deviceConnectedCallback = nullptr;
+    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;
+};

src/Sensors/Barometer.hpp

@@ -0,0 +1,72 @@
+#include "GyverBME280.h"
+
+typedef std::function<void()> OnMeasuringFinishedCb;
+
+class Barometer {
+  public:
+    Barometer(GyverBME280 *bme) {
+        _bme = bme;
+    };
+
+    ~Barometer() {
+        delete this->_bme;
+    }
+
+    bool initialize(void) {
+        _bme->setMode(NORMAL_MODE);
+        _bme->setFilter(FILTER_COEF_16);
+        _bme->setTempOversampling(OVERSAMPLING_2);
+        _bme->setPressOversampling(OVERSAMPLING_16);
+        _bme->setStandbyTime(STANDBY_500US);
+        return _bme->begin();
+    }
+
+    void measureBaseAltitudeSync(void) {
+        for (int i = 0; i < _calibrationIterationsCount; ++i) {
+            _startedAltitude += altitude();
+            delay(_calibrationIterationDelay);
+        }
+        _startedAltitude /= _calibrationIterationsCount;
+    }
+
+    void measureBaseAltitudeAsync(void) {
+        _isCalibration = true;
+    }
+
+    float altitude() /* [cm] */ {
+        return pressureToAltitude(_bme->readPressure()) * 100;
+    }
+
+    float flightHeight() /* [cm] */ {
+        return altitude() - _startedAltitude;
+    }
+
+    void onMeasuaringHeightFinished(OnMeasuringFinishedCb callback) {
+        _measuaringHeightFinishedCb = callback;
+    }
+
+    void tick() {
+        if (_isCalibration) {
+            if (millis() - _calibrationTimer >= _calibrationIterationDelay) {
+                _startedAltitude += altitude();
+            }
+            if (++_calibrationIterationsCounter >= _calibrationIterationsCount) {
+                _startedAltitude /= _calibrationIterationsCount;
+                _isCalibration = false;
+                if (_measuaringHeightFinishedCb) {
+                    _measuaringHeightFinishedCb();
+                }
+            }
+        }
+    }
+
+  private:
+    GyverBME280 *_bme;
+    float _startedAltitude = 0;
+    bool _isCalibration = false;
+    uint32_t _calibrationTimer = 0;
+    int _calibrationIterationsCounter = 0;
+    OnMeasuringFinishedCb _measuaringHeightFinishedCb = nullptr;
+    static constexpr int _calibrationIterationsCount = 1500;
+    static constexpr int _calibrationIterationDelay = 1;  // [ms]
+};

src/Sensors/BatteryController.hpp

@@ -0,0 +1,19 @@
+#include "board_pins.h"
+
+class BatteryController {
+  public:
+    BatteryController() {}
+
+    void initialize() {
+        pinMode(BATTERY_DATA_SWITCH_PIN, OUTPUT);
+        digitalWrite(BATTERY_DATA_SWITCH_PIN, HIGH);
+    }
+
+    float measureVoltage() {
+        return analogRead(BATTERY_DATA_PIN) * 3.3 / 4095;
+    }
+
+    int percent(int minVoltage = 7200, int maxVoltage = 8400) {
+        return map(int(measureVoltage() * 1000), minVoltage, maxVoltage, 0, 100);
+    }
+};

src/Sensors/MPU.hpp

@@ -0,0 +1,250 @@
+#include "MPU6050_6Axis_MotionApps20.h"
+#include "Preferences.h"
+#include "board_pins.h"
+#include "esp_log.h"
+
+static volatile bool _isDMPDataReady = false;
+static void IRAM_ATTR _dmpInterruption() {
+    _isDMPDataReady = true;
+}
+
+typedef std::function<void()> OnCalibrationFinishedCb;
+
+class MPU {
+  public:
+    MPU(MPU6050_6Axis_MotionApps20 *mpu) {
+        assert(mpu != nullptr);
+        _mpu = mpu;
+    }
+
+    ~MPU() {
+        delete _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!");
+            return false;
+        }
+        _mpu->setDLPFMode(MPU6050_DLPF_BW_10);  // 10 Hz bandwidth
+        //mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_500); // set sensivity, not recomended
+        //mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_8);
+
+        if (_mpu->dmpInitialize()) {
+            ESP_LOGE(_TAG, "Failed to initialize DMP!");
+            return false;
+        }
+        _mpu->setDMPEnabled(true);
+        attachInterrupt(MPU6050_INT_PIN, _dmpInterruption, RISING);
+        return true;
+    }
+
+    bool tick(bool &err) {
+        err = false;
+        if (!_isDMPDataReady) {
+            return false;
+        }
+        if (!_mpu->dmpGetCurrentFIFOPacket(_fifoBuffer)) {
+            ESP_LOGE(_TAG, "Failed to get DMP data!");
+            err = true;
+            return false;
+        }
+
+        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;
+
+        _ax = accel.x / 8192.f;
+        _ay = accel.y / 8192.f;
+        _az = accel.z / 8192.f;
+
+        _gx = gyro.x / 131.f;
+        _gy = gyro.y / 131.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() const {
+        return _AccZInertial;
+    }
+    float yaw() const {
+        return degrees(_ypr[0]);
+    }
+    float pitch() const {
+        if (_isCalibration) {
+            return degrees(_ypr[1]);
+        } else {
+            return degrees(_ypr[1] - _prOffset[0]);
+        }
+    }
+    float roll() const {
+        if (_isCalibration) {
+            return degrees(_ypr[2]);
+        } else {
+            return degrees(_ypr[2] - _prOffset[1]);
+        }
+    }
+
+    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;
+        }
+    }
+
+    void startCalibration() {
+        _isCalibration = true;
+    }
+
+    void onCalibrationFinished(OnCalibrationFinishedCb callback) {
+        _calibrationFinishedCb = callback;
+    }
+
+  private:
+    MPU6050_6Axis_MotionApps20 *_mpu = nullptr;
+    float _ypr[3] = { 0 };
+    float _ax = 0, _ay = 0, _az = 0;
+    float _gx = 0, _gy = 0, _gz = 0;
+    float _gravity = 0;
+    float _AccZInertial = 0;
+
+    float _axOffset = 0, _ayOffset = 0, _azOffset = 0;
+    float _gxOffset = 0, _gyOffset = 0, _gzOffset = 0;
+    float _prOffset[2] = { 0 };  // yaw isn't used
+
+    uint8_t _fifoBuffer[45] = { 0 };
+    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 = (_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]);
+    }
+};

src/Sensors/Sensors.cpp

@@ -0,0 +1,119 @@
+// This is a personal academic project. Dear PVS-Studio, please check it.
+// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
+
+#include "Sensors.hpp"
+
+Sensors::Sensors(Barometer *barometer, MPU *mpu, Kalman2DFilter *filter,
+                 BatteryController *battery, volatile bool loop_on_fail) {
+    assert(barometer != nullptr);
+    assert(mpu != nullptr);
+    assert(filter != nullptr);
+    assert(battery != nullptr);
+
+    _barometer = barometer;
+    _mpu = mpu;
+    _2d_filter = filter;
+    _battery = battery;
+
+    ESP_LOGI(_tag, "Initialize BMP280...");
+    if (_barometer->initialize()) {
+        ESP_LOGI(_tag, "BMP280 initialized");
+    } else {
+        ESP_LOGI(_tag, "BMP280 initialization failed!");
+        while (loop_on_fail)
+            ;
+    }
+
+    ESP_LOGI(_tag, "Initialize MPU6050...");
+    if (_mpu->initialize()) {
+        ESP_LOGI(_tag, "MPU6050 initialized");
+    } else {
+        ESP_LOGI(_tag, "MPU6050 initialization failed!");
+        while (loop_on_fail)
+            ;
+    }
+
+    _battery->initialize();
+    ESP_LOGI(_tag, "BatteryController initialized");
+    ESP_LOGI(_tag, "Sensors initialized");
+}
+
+Sensors::~Sensors() {
+    delete _barometer;
+    delete _mpu;
+    delete _2d_filter;
+    delete _battery;
+}
+
+void Sensors::measureBaseAltitudeSync(void) {
+    _barometer->measureBaseAltitudeSync();
+}
+
+void Sensors::measureBaseAltitudeAsync(void) {
+    _barometer->measureBaseAltitudeAsync();
+}
+
+float Sensors::rawFlightHeight(void) const {
+    /* Returns flight height from barometer immediatly */
+    return _barometer->flightHeight();
+}
+
+float Sensors::flightHeight(void) const {
+    /* Returns flight height from cache immediatly */
+    if (_flightHeightFromBarometer == 0.f) {
+        ESP_LOGW(_tag, "flightHeight called, but tick() has called never");
+        return rawFlightHeight();
+    } else {
+        return _flightHeightFromBarometer;
+    }
+}
+
+MpuData Sensors::mpuData() const {
+    if (_mpuData.ax == 0.f and _mpuData.gravity == 0.f) {
+        ESP_LOGW(_tag, "MPU data looks like .tick() has never called");
+    }
+    return _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) {
+        _2d_filter->filter(_mpu->accZInertial(), _barometer->flightHeight(),
+                           _flightHeightFromBarometer, _zVelocityAltitude);
+        _mpuData = { .ax = _mpu->ax(),
+                     .ay = _mpu->ay(),
+                     .az = _mpu->az(),
+                     .gx = _mpu->gx(),
+                     .gy = _mpu->gy(),
+                     .gz = _mpu->gz(),
+                     .yaw = _mpu->yaw(),
+                     .pitch = _mpu->pitch(),
+                     .roll = _mpu->roll(),
+                     .gravity = _mpu->gravityZ(),
+                     .zInertial = _mpu->accZInertial() };
+        return true;
+    } else if (err) {
+        ESP_LOGE(_tag, "Failed to get DMP data!");
+    }
+    return false;
+}
+
+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);
+}

src/Sensors/Sensors.hpp

@@ -0,0 +1,52 @@
+#include "Barometer.hpp"
+#include "BatteryController.hpp"
+#include "Filters/Kalman2DFilter.hpp"
+#include "MPU.hpp"
+#include "esp_log.h"
+
+/* Sensors module provides cached and filtered sensors values */
+
+struct MpuData {
+    float ax, ay, az;
+    float gx, gy, gz;
+    float yaw, pitch, roll;
+    float gravity;
+    float zInertial;
+};
+
+class Sensors {
+  public:
+    Sensors(Barometer *barometer, MPU *mpu, Kalman2DFilter *filter,
+            BatteryController *battery, bool loop_on_fail = true);
+
+    ~Sensors();
+
+    void measureBaseAltitudeSync(void);
+    void measureBaseAltitudeAsync(void);
+    void onMeasuaringAltitudeFinished(OnMeasuringFinishedCb callback);
+    float rawFlightHeight(void) const;
+    float flightHeight(void) const;
+
+    void startMpuCalibration();
+    void onMpuCalibrationFinished(OnCalibrationFinishedCb callback);
+
+    MpuData mpuData(void) const;
+
+    int batteryCharge(void) const;  // [%]
+
+    bool tick(void);
+
+  private:
+    Barometer *_barometer = nullptr;
+    MPU *_mpu = nullptr;
+    BatteryController *_battery = nullptr;
+    Kalman2DFilter *_2d_filter = nullptr;
+
+    /* cached filtered values */
+    float _flightHeightFromBarometer = 0.f;
+    float _zVelocityAltitude = 0.f;
+    MpuData _mpuData = { 0.f };
+
+
+    static constexpr const char *_tag = "Sensors";
+};

src/board_pins.h

@@ -0,0 +1,8 @@
+#include "driver/adc.h"
+
+#define I2C_SDA_PIN 21
+#define I2C_SCL_PIN 18
+#define MPU6050_INT_PIN 19
+#define BATTERY_ADC_CHANNEL ADC2_CHANNEL_0
+#define BATTERY_DATA_PIN 4
+#define BATTERY_DATA_SWITCH_PIN 17

src/main.cpp

@@ -0,0 +1,27 @@
+// This is a personal academic project. Dear PVS-Studio, please check it.
+// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
+
+#include "App.hpp"
+#include "BoardI2C.hpp"
+
+#define IGNORE_CHARGE true
+
+BoardI2C i2c;
+
+static Application *app = nullptr;
+
+void setup() {
+    Serial.begin(115200);
+    app = new Application(new FlightDispatcher(
+      new BluetoothDispatcher(new BluetoothSerial()),
+      new FlightController(
+        new Sensors(new Barometer(new GyverBME280(i2c)), new MPU(new MPU6050(MPU6050_DEFAULT_ADDRESS, &i2c)),
+                    new Kalman2DFilter(10.f, 1.f, 1.8f), new BatteryController()),
+        new PIDController(1.f, 1.f, 1.f, new BrushedMotor(1, 2, 3, 4, 1), 1),
+        new PIDController(1.f, 1.f, 1.f, new BrushedMotor(1, 2, 3, 4, 2), 2),
+        new PIDController(1.f, 1.f, 1.f, new BrushedMotor(1, 2, 3, 4, 3), 3), IGNORE_CHARGE)));
+}
+
+void loop() {
+    app->tick();
+}