PID controllers hierarchy revision

.idea/.gitignore

@@ -0,0 +1,8 @@
+# Default ignored files
+/shelf/
+/workspace.xml
+# Editor-based HTTP Client requests
+/httpRequests/
+# Datasource local storage ignored files
+/dataSources/
+/dataSources.local.xml

.idea/codeStyles/codeStyleConfig.xml

@@ -0,0 +1,5 @@
+<component name="ProjectCodeStyleConfiguration">
+  <state>
+    <option name="USE_PER_PROJECT_SETTINGS" value="true" />
+  </state>
+</component>

.idea/misc.xml

@@ -0,0 +1,17 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ExternalStorageConfigurationManager" enabled="true" />
+  <component name="PlatformIOSettings">
+    <option name="linkedExternalProjectsSettings">
+      <PlatformioProjectSettings>
+        <option name="externalProjectPath" value="$PROJECT_DIR$" />
+        <option name="modules">
+          <set>
+            <option value="$PROJECT_DIR$" />
+          </set>
+        </option>
+      </PlatformioProjectSettings>
+    </option>
+  </component>
+  <component name="PlatformIOWorkspace" PROJECT_DIR="$PROJECT_DIR$" />
+</project>

.idea/vcs.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="VcsDirectoryMappings">
+    <mapping directory="" vcs="Git" />
+  </component>
+</project>

src/.clang-format

@@ -92,7 +92,6 @@ IncludeIsMainRegex: ''
 IncludeIsMainSourceRegex: ''
 IndentGotoLabels: false
 IndentPPDirectives: None
-IndentRequires: true
 IndentWidth: 2
 IndentWrappedFunctionNames: false
 InsertTrailingCommas: None
@@ -148,7 +147,7 @@ StatementAttributeLikeMacros:
 StatementMacros:
   - Q_UNUSED
   - QT_REQUIRE_VERSION
-TabWidth: 2
+#TabWidth: 2
 UseCRLF: false
 UseTab: Never
 WhitespaceSensitiveMacros:

src/BoardI2C.hpp

@@ -1,6 +1,7 @@
 // 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
 
+#define CONFIG_DISABLE_HAL_LOCKS 0
 #include "Wire.h"
 #include "board_pins.h"
 #include "esp_log.h"
@@ -9,6 +10,7 @@ class BoardI2C : public TwoWire {
   public:
     BoardI2C(volatile bool loop_on_fail = true) : TwoWire(0) {
         if (begin(I2C_SDA_PIN, I2C_SCL_PIN, 100000)) {
+            setTimeOut(50);
             ESP_LOGI("I2CBus", "Bus initialized");
         } else {
             ESP_LOGE("I2CBus", "Failed to initialize the i2c software bus!");

src/Logic/FlightController.cpp

@@ -1,48 +1,59 @@
 #include "FlightController.hpp"
 
-FlightController::FlightController(Sensors *sensors, PIDController *pid1, PIDController *pid2,
-                                   PIDController *pid3, bool unlimitedBattery) {
+FlightController::FlightController(
+        Sensors *sensors, SavedPidRegulator *heightController, SavedPidRegulator *yawController,
+        BrushedMotor *rotor1, BrushedMotor *rotor2,
+        PIDController *tailRotorController, bool unlimitedBattery) {
     assert(sensors != nullptr);
-    assert(pid1 != nullptr);
-    assert(pid2 != nullptr);
-    assert(pid3 != nullptr);
+    assert(heightController != nullptr);
+    assert(yawController != nullptr);
+    assert(tailRotorController != nullptr);
+    assert(rotor1 != nullptr);
+    assert(rotor2 != nullptr);
     _sensors = sensors;
     _unlimitedBattery = unlimitedBattery;
-    _pid1 = pid1;
-    _pid2 = pid2;
-    _pid3 = pid3;
+    _heightController = heightController;
+    _yawController = yawController;
+    _tailRotorController = tailRotorController;
+    _rotor1 = rotor1;
+    _rotor2 = rotor2;
 
     _status = DeviceStatus::Idle;
-    _updateBatteryCharge();
+    _checkBatteryCharge();
+    stopAllRotors();
     ESP_LOGI(_tag, "Flight controller initialized");
 }
 
 FlightController::~FlightController() {
     delete _sensors;
-    delete _pid1;
-    delete _pid2;
-    delete _pid3;
+    delete _heightController;
+    delete _yawController;
+    delete _tailRotorController;
+    delete _rotor1;
+    delete _rotor2;
 }
 
 void FlightController::tick() {
     bool hasUpdates = _sensors->tick();
     if (hasUpdates) {
-        _updateBatteryCharge();
+        _checkBatteryCharge();
     }
     switch (_status) {
-    case DeviceStatus::Idle: break;
-    case DeviceStatus::ChargeRequired: break;
-    case DeviceStatus::IsPreparingForTakeoff: break;
-    case DeviceStatus::IsImuCalibration: break;
+        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();
+                _tailRotorController->tick();
             }
             break;
     }
@@ -57,7 +68,7 @@ ReportedDeviceState FlightController::currentDeviceState() const {
 
 void FlightController::startTakeoff() {
     /* Start preparation for takeoff */
-    _updateBatteryCharge();
+    _checkBatteryCharge();
     if (_status != DeviceStatus::Idle)
         return;
 
@@ -77,7 +88,7 @@ void FlightController::startBoarding() {
     }
 }
 
-void FlightController::_updateBatteryCharge() {
+void FlightController::_checkBatteryCharge() {
     if (_unlimitedBattery)
         return;
     if (_sensors->batteryCharge() <= _batteryCriticalCharge) {
@@ -92,16 +103,18 @@ void FlightController::_updateBatteryCharge() {
 void FlightController::_takeoff() {
     // TODO: implement takeoff
     _status = DeviceStatus::IsFlying;
-    if (_onTakeoffStarted) {
-        _onTakeoffStarted();
-    }
+    _tailRotorController->enable();
+    _rotor1->enable();
+    _rotor2->enable();
 }
 
 void FlightController::_boarding() {
     if (_sensors->flightHeight() <= _defaultStopMotorHeight) {
         // TODO: stop motors and setpoints
         _status = DeviceStatus::Idle;
-
+        _rotor1->disable();
+        _rotor2->disable();
+        _tailRotorController->disable();
     } else {
         // TODO: implement boarding
     }
@@ -134,63 +147,61 @@ void FlightController::newPitchStickPosition(int16_t newYawStickPostition) {
     // TODO: implement stick input
 }
 
-void FlightController::newRotor1Duty(uint32_t rotor1Duty) {
+void FlightController::setRotor1Duty(uint32_t rotor1Duty) {
     if (_status == DeviceStatus::Idle) {
-        _pid1->setRotorDuty(rotor1Duty);
+        _rotor1->setDuty(rotor1Duty);
     }
 }
 
-void FlightController::newRotor2Duty(uint32_t rotor2Duty) {
+void FlightController::setRotor2Duty(uint32_t rotor2Duty) {
     if (_status == DeviceStatus::Idle) {
-        _pid2->setRotorDuty(rotor2Duty);
+        _rotor2->setDuty(rotor2Duty);
     }
 }
 
-void FlightController::newRotor3Duty(uint32_t rotor3Duty) {
+void FlightController::setRotor3Duty(uint32_t rotor3Duty) {
     if (_status == DeviceStatus::Idle) {
-        _pid3->setRotorDuty(rotor3Duty);
+        _tailRotorController->setRotorDuty(rotor3Duty);
     }
 }
 
 void FlightController::stopAllRotors() {
-    _pid1->setRotorDuty(0);
-    _pid2->setRotorDuty(0);
-    _pid3->setRotorDuty(0);
+    setRotor1Duty(0);
+    setRotor2Duty(0);
+    setRotor3Duty(0);
 }
 
-void FlightController::setPid1Params(float p, float i, float d) {
+void FlightController::setHeightControllerParams(float p, float i, float d) {
     if (_status == DeviceStatus::Idle) {
-        _pid1->Kp = p;
-        _pid1->Ki = i;
-        _pid1->Kd = d;
-        _pid1->save();
+        _heightController->Kp = p;
+        _heightController->Ki = i;
+        _heightController->Kd = d;
+        _heightController->save();
     }
 }
 
-void FlightController::setPid2Params(float p, float i, float d) {
+void FlightController::setYawControllerParams(float p, float i, float d) {
     if (_status == DeviceStatus::Idle) {
-        _pid2->Kp = p;
-        _pid2->Ki = i;
-        _pid2->Kd = d;
-        _pid1->save();
+        _yawController->Kp = p;
+        _yawController->Ki = i;
+        _yawController->Kd = d;
+        _heightController->save();
     }
 }
 
-void FlightController::setPid3Params(float p, float i, float d) {
+void FlightController::setPitchControllerParams(float p, float i, float d) {
     if (_status == DeviceStatus::Idle) {
-        _pid3->Kp = p;
-        _pid3->Ki = i;
-        _pid3->Kd = d;
-        _pid1->save();
+        _tailRotorController->Kp = p;
+        _tailRotorController->Ki = i;
+        _tailRotorController->Kd = d;
+        _heightController->save();
     }
 }
 
-std::unique_ptr<PidSettings> FlightController::pidSettings() const {
-    std::unique_ptr<PidSettings> settings = std::make_unique<PidSettings>();
-    settings->pid1 = { .p = _pid1->Kp, .i = _pid1->Ki, .d = _pid1->Kd };
-    settings->pid2 = { .p = _pid2->Kp, .i = _pid2->Ki, .d = _pid2->Kd };
-    settings->pid3 = { .p = _pid3->Kp, .i = _pid3->Ki, .d = _pid3->Kd };
-    ESP_LOGI(_tag, "PID №1 settings: (%f, %f, %f)", settings->pid1.p, settings->pid1.i,
-             settings->pid1.d);
-    return std::move(settings);
+PidSettings FlightController::pidSettings() const {
+    PidSettings settings;
+    settings.flightController = {.p = _heightController->Kp, .i = _heightController->Ki, .d = _heightController->Kd};
+    settings.yawController = {.p = _yawController->Kp, .i = _yawController->Ki, .d = _yawController->Kd};
+    settings.pitchController = {.p = _tailRotorController->Kp, .i = _tailRotorController->Ki, .d = _tailRotorController->Kd};
+    return settings;
 }

src/Logic/FlightController.hpp

@@ -1,7 +1,7 @@
 // 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 "PID.hpp"
+#include "PIDController.hpp"
 #include "Sensors/Sensors.hpp"
 #include <memory>
 
@@ -27,51 +27,67 @@ struct FlightParams {
     float pitch;   // [degrees]
 };
 
-struct PidParam {
-    float p;
-    float i;
-    float d;
-};
-
 struct PidSettings {
-    PidParam pid1;
-    PidParam pid2;
-    PidParam pid3;
+    PidParams flightController;
+    PidParams yawController;
+    PidParams pitchController;
 };
 
 class FlightController {
 public:
-    FlightController(Sensors *sensors, PIDController *pid1, PIDController *pid2,
-                     PIDController *pid3, bool unlimitedBattery = false);
+    FlightController(Sensors *sensors, SavedPidRegulator *heightController, SavedPidRegulator *yawController,
+                     BrushedMotor *_rotor1, BrushedMotor *_rotor2,
+                     PIDController *tailRotor, bool unlimitedBattery);
+
     ~FlightController();
-    ReportedDeviceState currentDeviceState() const;
+
+    [[nodiscard]] 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 setRotor1Duty(uint32_t rotor1Duty);
+
+    void setRotor2Duty(uint32_t rotor2Duty);
+
+    void setRotor3Duty(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);
-    std::unique_ptr<PidSettings> pidSettings() const;
+
+    void setHeightControllerParams(float p, float i, float d);
+
+    void setYawControllerParams(float p, float i, float d);
+
+    void setPitchControllerParams(float p, float i, float d);
+
+    [[nodiscard]] PidSettings pidSettings() const;
+
 private:
-    void _updateBatteryCharge();
+    void _checkBatteryCharge();
+
     [[deprecated]] void _takeoff();
+
     [[deprecated]] void _boarding();
 
     Sensors *_sensors = nullptr;
-    PIDController *_pid1 = nullptr;
-    PIDController *_pid2 = nullptr;
-    PIDController *_pid3 = nullptr;
+    SavedPidRegulator *_heightController = nullptr;
+    SavedPidRegulator *_yawController = nullptr;
+    PIDController *_tailRotorController = nullptr;
+    BrushedMotor *_rotor1 = nullptr;
+    BrushedMotor *_rotor2 = nullptr;
     DeviceStatus _status;
-    OnMeasuringFinishedCb _onTakeoffStarted = nullptr;
 
     static constexpr const char *_tag = "FlightController";
     static constexpr uint8_t _batteryCriticalCharge = 15;  // [%]

src/Logic/FlightDispatcher.cpp

@@ -22,8 +22,7 @@ FlightDispatcher::FlightDispatcher(BluetoothDispatcher *bluetoothDispatcher,
         ESP_LOGI(_tag, "Bluetooth initialized successfully.");
     } else {
         ESP_LOGE(_tag, "Failed to initialize Bluetooth dispatcher!");
-        while (loop_on_fail)
-            ;
+        while (loop_on_fail);
     }
     _telemetryTimer = millis();
 }
@@ -66,44 +65,59 @@ void FlightDispatcher::_onNewMessageReceived(char *package) {
             }
 
             switch (pkg.keyHash(keyIndex)) {
-            case SH("status"): _changeStatus((DeviceStatus)value.toInt32()); break;
+                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("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("r1"):
+                    _flightController->setRotor1Duty(value.toInt32());
+                    break;
+                case SH("r2"):
+                    _flightController->setRotor2Duty(value.toInt32());
+                    break;
+                case SH("r3"):
+                    _flightController->setRotor3Duty(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(
+                        _flightController->setHeightControllerParams(
                                 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(
+                        _flightController->setYawControllerParams(
                                 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(
+                        _flightController->setPitchControllerParams(
                                 pair[SH("p")].toFloat(), pair[SH("i")].toFloat(), pair[SH("d")].toFloat());
                     }
                     break;
-            case SH("pidSettingOpened"): _pidSettingsOpened(); break;
-            default: break;
+                case SH("pidSettingOpened"):
+                    _pidSettingsOpened();
+                    break;
+                default:
+                    break;
             }
         }
     }
@@ -111,13 +125,20 @@ void FlightDispatcher::_onNewMessageReceived(char *package) {
 
 void FlightDispatcher::_changeStatus(const DeviceStatus &newStatus) {
     switch (newStatus) {
-    case DeviceStatus::IsImuCalibration: _flightController->startImuCalibration(); break;
-    case DeviceStatus::IsFlying:
-        _changeStatus(DeviceStatus::IsPreparingForTakeoff);
+        case DeviceStatus::IsImuCalibration:
+            _flightController->startImuCalibration();
+            break;
+        case DeviceStatus::IsFlying:
+            _flightController->startTakeoff();
+            break;
+        case DeviceStatus::IsPreparingForTakeoff:
+            _flightController->startTakeoff();
+            break;
+        case DeviceStatus::IsBoarding:
+            _flightController->startBoarding();
+            break;
+        default:
             break;
-    case DeviceStatus::IsPreparingForTakeoff: _flightController->startTakeoff(); break;
-    case DeviceStatus::IsBoarding: _flightController->startBoarding(); break;
-    default: break;
     }
 }
 
@@ -127,21 +148,21 @@ void FlightDispatcher::_pidSettingsOpened() {
     pkg.beginObj();
 
     pkg.beginObj("p1");
-    pkg["p"] = settings->pid1.p;
-    pkg["i"] = settings->pid1.i;
-    pkg["d"] = settings->pid1.d;
+    pkg["p"] = settings.flightController.p;
+    pkg["i"] = settings.flightController.i;
+    pkg["d"] = settings.flightController.d;
     pkg.endObj();
 
     pkg.beginObj("p2");
-    pkg["p"] = settings->pid2.p;
-    pkg["i"] = settings->pid2.i;
-    pkg["d"] = settings->pid2.d;
+    pkg["p"] = settings.yawController.p;
+    pkg["i"] = settings.yawController.i;
+    pkg["d"] = settings.yawController.d;
     pkg.endObj();
 
     pkg.beginObj("p3");
-    pkg["p"] = settings->pid3.p;
-    pkg["i"] = settings->pid3.i;
-    pkg["d"] = settings->pid3.d;
+    pkg["p"] = settings.pitchController.p;
+    pkg["i"] = settings.pitchController.i;
+    pkg["d"] = settings.pitchController.d;
     pkg.endObj();
 
     pkg.endObj();

src/Logic/PID.hpp

@@ -1,59 +0,0 @@
-// 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 "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/Logic/PIDController.hpp

@@ -0,0 +1,46 @@
+// 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 "Motor/BrushedMotor.hpp"
+#include "SavedPidRegulator.hpp"
+#include "esp_log.h"
+
+
+class PIDController : public SavedPidRegulator {
+public:
+    PIDController(float p, float i, float d, BrushedMotor *rotor,
+                  const char *name = "PID", uint16_t dt = 10)
+            : SavedPidRegulator(p, i, d, name, dt) {
+        assert(rotor != nullptr);
+        _rotor = rotor;
+        this->_dt = dt;
+        setLimits(0, _rotor->maxDuty());
+        _pid_timer = millis();
+        ESP_LOGI(_tag, "PID %s initialized with params (%f, %f, %f)", name, Kp, Ki, Kd);
+    }
+
+    void enable() {
+        _rotor->enable();
+    }
+
+    void disable() {
+        _rotor->disable();
+    }
+
+    void tick() {
+        if (millis() - _pid_timer >= _dt) {
+            _rotor->setDuty((uint32_t) getResultTimer());
+            _pid_timer = millis();
+        }
+    }
+
+    void setRotorDuty(uint32_t duty) {
+        /* Used only for test purposes */
+        _rotor->setDuty(duty);
+    }
+
+private:
+    uint16_t _dt;
+    BrushedMotor *_rotor = nullptr;
+    uint32_t _pid_timer = 0;
+};

src/Logic/SavedPidRegulator.hpp

@@ -0,0 +1,71 @@
+#include "GyverPID.h"
+#include "Preferences.h"
+#include "esp_log.h"
+#include <memory>
+
+struct PidParams {
+    float p;
+    float i;
+    float d;
+};
+
+class SavedPidRegulator : public GyverPID {
+public:
+    SavedPidRegulator(float p, float i, float d, const char *name = "saved_pid", uint16_t dt = 10)
+            : GyverPID(p, i, d, (int16_t) dt) {
+        _name = name;
+        _preferences.begin(name);
+        _pDefault = p;
+        _iDefault = i;
+        _dDefault = d;
+        _loadPreferences();
+        ESP_LOGI(_tag, "PID %s initialized with params (%f, %f, %f)", _name.c_str(), Kp, Ki, Kd);
+    }
+
+    void setParams(PidParams params) {
+        this->Kp = params.p;
+        this->Ki = params.i;
+        this->Kd = params.d;
+    }
+
+    std::shared_ptr<PidParams> getParams() {
+        std::shared_ptr params = std::make_unique<PidParams>();
+        params->p = Kp;
+        params->i = Ki;
+        params->d = Kd;
+        return params;
+    }
+
+    void resetToDefault() {
+        this->Kp = _pDefault;
+        this->Ki = _iDefault;
+        this->Kd = _dDefault;
+        save();
+    }
+
+    void save() {
+        _preferences.putFloat("p", Kp);
+        _preferences.putFloat("i", Ki);
+        _preferences.putFloat("d", Kd);
+        ESP_LOGI(_tag, "PID %s saved with params (%f, %f, %f)", _name.c_str(), Kp, Ki, Kd);
+    }
+
+protected:
+    static constexpr const char *_tag = "PIDController";
+
+private:
+    float _pDefault = 0.f;
+    float _iDefault = 0.f;
+    float _dDefault = 0.f;
+
+    String _name = "PID";
+    Preferences _preferences;
+
+    void _loadPreferences() {
+        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.hpp

@@ -11,13 +11,21 @@ class BrushedMotor {
 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:

src/RF/BluetoothDispatcher.cpp

@@ -4,6 +4,7 @@
 #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) {
@@ -26,12 +27,11 @@ bool BluetoothDispatcher::initialize(volatile bool loop_on_fail, int readTimeout
     deviceConnectedCallback = [this](BTAddress device) {
         _onDeviceConnected(device);
     };
-    _controller->setTimeout(2);
+    _controller->setTimeout(50);
     bool success = _controller->begin(_device_name, false);
     if (!success) {
         ESP_LOGI(_tag, "Failed to initialize Bluetooth controller!");
-        while (loop_on_fail)
-            ;
+        while (loop_on_fail);
         return false;
     } else {
         ESP_LOGI(_tag, "Bluetooth host initialized");
@@ -43,12 +43,12 @@ void BluetoothDispatcher::onNewPackageReceived(NewPackageCallback newPackageRece
     _newPackageReceivedCb = newPackageReceivedCb;
 }
 
-void BluetoothDispatcher::tick(const char *message_delimeter) {
+void BluetoothDispatcher::tick(const char message_delimeter) {
     /* Call the callback, if new package received */
     while (_controller->available()) { _buffer += (char) _controller->read(); }
-    if (_buffer.endsWith(message_delimeter)) {
+    if (_buffer.lastIndexOf(message_delimeter) != -1) {
         char buffer[_buffer_size];
-        auto messageEnd = _buffer.lastIndexOf('}');
+        auto messageEnd = _buffer.indexOf(message_delimeter) - 1;
         if (messageEnd == -1) {
             _buffer.clear();
         } else {

src/RF/BluetoothDispatcher.hpp

@@ -29,7 +29,7 @@ class BluetoothDispatcher {
   public:
     BluetoothDispatcher(BluetoothSerial *controller, const char *device_name = "Helicopter");
     bool initialize(volatile bool loop_on_fail = true, int readTimeoutMS = 2);
-    void tick(const char *message_delimeter = "\n");
+    void tick(const char message_delimeter = '\n');
     void onNewPackageReceived(NewPackageCallback newPackageReceivedCb);
     void onNewDeviceConnected(DeviceConnectedCb deviceConnectedCb);
     void sendPackage(const char *package, size_t size);

src/Sensors/BatteryController.hpp

@@ -25,15 +25,12 @@ class BatteryController {
     int percent(const float minVoltage = 7.2f, const float maxVoltage = 8.4f,
                 const float r1 = 3.3f, const float r2 = 2.f, const float k = 2.87f) {
         auto batteryVoltage = measureVoltage() * ((r1 + r2) / k);
-        return round(_map(batteryVoltage, minVoltage, maxVoltage, 5.f, 100.f));
+        auto percent = ((batteryVoltage - minVoltage) / (maxVoltage - minVoltage)) * 100;
+        return constrain(round(percent), 0, 100);
     }
 
   private:
     uint16_t _battery_pin;
     uint16_t _battery_data_switch_pin;
     static constexpr const char *_tag = "BatteryController";
-
-    float _map(float x, float in_min, float in_max, float out_min, float out_max) {
-        return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
-    }
 };

src/main.cpp

@@ -14,11 +14,15 @@ void setup() {
             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 MPU(new MPU6050(MPU6050_DEFAULT_ADDRESS, &i2c)),
+                                new Kalman2DFilter(10.f, 1.f, 1.8f),
                                 new BatteryController(BATTERY_DATA_PIN, BATTERY_DATA_SWITCH_PIN)),
-        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), true)));
+                    new SavedPidRegulator(3.5f, 0.0015f, 0.01f, "HeightController"),  // height
+                    new SavedPidRegulator(2.f, 12.f, 0.f, "YawController"),  // yaw
+                    new BrushedMotor(1, 2, 3, 4, 1),
+                    new BrushedMotor(1, 2, 3, 4, 1),
+                    new PIDController(0.6f, 3.5f, 0.03f, new BrushedMotor(1, 2, 3, 4, 3), "RollController"),  // pitch
+                    true)));
 }
 
 void loop() {