All modules and core of the app was created

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