Project structure redefinition

platformio.ini

@@ -24,7 +24,7 @@ lib_deps_external =
     https://github.com/tomstewart89/BasicLinearAlgebra.git @ 4.3
     https://github.com/GyverLibs/mString.git @ 1.7
     https://github.com/GyverLibs/GyverPID @ 3.3
-
+    https://github.com/pololu/vl53l0x-arduino @ 1.3.1
 
 [env:esp32dev]
 platform = espressif32

src/App.hpp

@@ -1,3 +1,6 @@
+// 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 "Logic/FlightDispatcher.hpp"
 #include "esp_log.h"
 

src/BoardI2C.hpp

@@ -1,3 +1,6 @@
+// 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 "Wire.h"
 #include "board_pins.h"
 #include "esp_log.h"

src/Filters/Kalman2DFilter.hpp

@@ -1,3 +1,6 @@
+// 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 <BasicLinearAlgebra.h>
 
 class Kalman2DFilter {
@@ -14,7 +17,8 @@ class Kalman2DFilter {
         S = { 0, 0 };
     }
 
-    void filter(const float &AccZInertial, const float &AltitudeBarometer, float &AltitudeKalman, float &VelocityVerticalKalman) {
+    void filter(const float &AccZInertial, const float &AltitudeBarometer,
+                float &AltitudeKalman, float &VelocityVerticalKalman) {
         Acc = { AccZInertial };
         S = F * S + G * Acc;
         P = F * P * ~F + Q;

src/Logic/FlightController.cpp

@@ -1,6 +1,3 @@
-// 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,
@@ -113,6 +110,9 @@ void FlightController::_boarding() {
 void FlightController::startImuCalibration() {
     if (_status != DeviceStatus::Idle)
         return;
+    ESP_LOGI(_tag, "Started IMU calibration");
+    _status = DeviceStatus::IsImuCalibration;
+    _sensors->startMpuCalibration();
     _sensors->onMpuCalibrationFinished([this]() {
         this->_status = DeviceStatus::Idle;
     });
@@ -185,10 +185,12 @@ void FlightController::setPid3Params(float p, float i, float d) {
     }
 }
 
-PidSettings *FlightController::pidSettings() const {
+std::unique_ptr<PidSettings> FlightController::pidSettings() const {
-    PidSettings pid1 = { .p = _pid1->Kp, .i = _pid1->Ki, .d = _pid1->Kd };
+    std::unique_ptr<PidSettings> settings = std::make_unique<PidSettings>();
-    PidSettings pid2 = { .p = _pid2->Kp, .i = _pid2->Ki, .d = _pid2->Kd };
+    settings->pid1 = { .p = _pid1->Kp, .i = _pid1->Ki, .d = _pid1->Kd };
-    PidSettings pid3 = { .p = _pid3->Kp, .i = _pid3->Ki, .d = _pid3->Kd };
+    settings->pid2 = { .p = _pid2->Kp, .i = _pid2->Ki, .d = _pid2->Kd };
-    static PidSettings settings[] = { pid1, pid2, pid3 };
+    settings->pid3 = { .p = _pid3->Kp, .i = _pid3->Ki, .d = _pid3->Kd };
-    return settings;
+    ESP_LOGI(_tag, "PID №1 settings: (%f, %f, %f)", settings->pid1.p, settings->pid1.i,
+             settings->pid1.d);
+    return std::move(settings);
 }

src/Logic/FlightController.hpp

@@ -1,6 +1,9 @@
+// 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 <memory>
 #include "Sensors/Sensors.hpp"
+#include <memory>
 
 enum DeviceStatus {
     Idle = 0,
@@ -24,12 +27,18 @@ struct FlightParams {
     float pitch;   // [degrees]
 };
 
-struct PidSettings {
+struct PidParam {
     float p;
     float i;
     float d;
 };
 
+struct PidSettings {
+    PidParam pid1;
+    PidParam pid2;
+    PidParam pid3;
+};
+
 class FlightController {
   public:
     FlightController(Sensors *sensors, PIDController *pid1, PIDController *pid2,
@@ -51,7 +60,7 @@ class FlightController {
     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;
+    std::unique_ptr<PidSettings> pidSettings() const;
   private:
     void _updateBatteryCharge();
     [[deprecated]] void _takeoff();

src/Logic/FlightDispatcher.cpp

@@ -43,22 +43,12 @@ void FlightDispatcher::tick() {
 }
 
 void FlightDispatcher::_onNewDeviceConnected(BTAddress device) {
-    auto currentState = _flightController->currentDeviceState();
+    _sendTelemetry();
-    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);
+    ESP_LOGI(_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",
@@ -137,27 +127,28 @@ void FlightDispatcher::_pidSettingsOpened() {
     pkg.beginObj();
 
     pkg.beginObj("p1");
-    pkg["p"] = settings[0].p;
+    pkg["p"] = settings->pid1.p;
-    pkg["i"] = settings[0].i;
+    pkg["i"] = settings->pid1.i;
-    pkg["d"] = settings[0].d;
+    pkg["d"] = settings->pid1.d;
     pkg.endObj();
 
     pkg.beginObj("p2");
-    pkg["p"] = settings[1].p;
+    pkg["p"] = settings->pid2.p;
-    pkg["i"] = settings[1].i;
+    pkg["i"] = settings->pid2.i;
-    pkg["d"] = settings[1].d;
+    pkg["d"] = settings->pid2.d;
     pkg.endObj();
 
     pkg.beginObj("p3");
-    pkg["p"] = settings[2].p;
+    pkg["p"] = settings->pid3.p;
-    pkg["i"] = settings[2].i;
+    pkg["i"] = settings->pid3.i;
-    pkg["d"] = settings[2].d;
+    pkg["d"] = settings->pid3.d;
     pkg.endObj();
 
     pkg.endObj();
     pkg.end();
-    pkg += "\r\n";
+    pkg.s = String(MessageType::PidSettings) + ";" + pkg.s + _message_delimeter;
     _bluetoothDispatcher->sendPackage(pkg.s.c_str(), strlen(pkg.s.c_str()));
+    ESP_LOGI(_tag, "PID settings sended %s", pkg.s.c_str());
 }
 
 void FlightDispatcher::_sendTelemetry() {
@@ -165,7 +156,7 @@ void FlightDispatcher::_sendTelemetry() {
     auto state = _flightController->currentDeviceState();
     gson::string package;
     package.beginObj();
-    package["batteryCharge"] = state.batteryCharge;
+    package["charge"] = state.batteryCharge;
     package["flightHeight"] = state.flightHeight;
     package["status"] = state.status;
     package["y"] = state.mpuState.yaw;
@@ -174,7 +165,6 @@ void FlightDispatcher::_sendTelemetry() {
     package["zIn"] = state.mpuState.zInertial;
     package.endObj();
     package.end();
-    package += "\r\n";
+    package.s = String(MessageType::UpdatePackage) + ";" + package.s + _message_delimeter;
-
     _bluetoothDispatcher->sendPackage(package.s.c_str(), strlen((package.s.c_str())));
 }

src/Logic/FlightDispatcher.hpp

@@ -1,8 +1,14 @@
+// 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"
 #include "GSON.h"
 #include "RF/BluetoothDispatcher.hpp"
 #include <map>
 
+// Message type annotation for mobile app
+enum MessageType { UpdatePackage = 0, PidSettings };
+
 class FlightDispatcher {
     /* Deserialize state and update it in FlightController. */
   public:
@@ -26,10 +32,9 @@ class FlightDispatcher {
     static constexpr const char *_tag = "FlightDispatcher";
     static constexpr const int _telemetryTimeIntervalMS = 200;
     static constexpr const uint8_t _jsonMaxDepth = 5;
+    static constexpr const char *_message_delimeter = "\n";
 
     uint32_t _telemetryTimer = millis();
     BluetoothDispatcher *_bluetoothDispatcher;
     FlightController *_flightController;
-
-    std::map<size_t, std::function<void(gson::Entry)>> _routes;
 };

src/Logic/PID.hpp

@@ -1,3 +1,6 @@
+// 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"

src/Motor/BrushedMotor.hpp

@@ -1,3 +1,6 @@
+// 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 <stdint.h>
 
 // TODO: implement class

src/RF/BluetoothDispatcher.cpp

@@ -43,21 +43,24 @@ void BluetoothDispatcher::onNewPackageReceived(NewPackageCallback newPackageRece
     _newPackageReceivedCb = newPackageReceivedCb;
 }
 
-void BluetoothDispatcher::tick() {
+void BluetoothDispatcher::tick(const char *message_delimeter) {
     /* Call the callback, if new package received */
-    while (_controller->available() and _buffer.length() <= _buffer_size) {
+    while (_controller->available()) { _buffer += (char)_controller->read(); }
-        _buffer += (char)_controller->read();
+    if (_buffer.endsWith(message_delimeter)) {
-    }
-    if (_buffer.endsWith("\n\r")) {
         char buffer[_buffer_size];
-        _buffer.substring(0, _buffer.lastIndexOf('}'), buffer);
+        auto messageEnd = _buffer.lastIndexOf('}');
+        if (messageEnd == -1) {
+            _buffer.clear();
+        } else {
+            _buffer.substring(0, messageEnd, buffer);
+        }
         ESP_LOGD(_tag, "Received new buffer %s", buffer);
         if (_newPackageReceivedCb) {
             _newPackageReceivedCb(buffer);
         }
         _buffer.clear();
     }
-    if (_buffer.length() > _available_buffer_size) {
+    if (_buffer.length() > _real_buffer_size) {
         _buffer.clear();
     }
 }
@@ -80,8 +83,9 @@ void BluetoothDispatcher::_onAuthComplete(boolean success) const {
     }
 }
 
-void BluetoothDispatcher::_onDeviceConnected(BTAddress device) const {
+void BluetoothDispatcher::_onDeviceConnected(BTAddress device) {
     ESP_LOGI(_tag, "New device connected: %s", device.toString(true).c_str());
+    _buffer.clear();
     if (_deviceConnectedCallback) {
         _deviceConnectedCallback(device);
     }

src/RF/BluetoothDispatcher.hpp

@@ -1,3 +1,6 @@
+// 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 "BluetoothSerial.h"
 #include "HardwareSerial.h"
 #include "esp_log.h"
@@ -26,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();
+    void tick(const char *message_delimeter = "\n");
     void onNewPackageReceived(NewPackageCallback newPackageReceivedCb);
     void onNewDeviceConnected(DeviceConnectedCb deviceConnectedCb);
     void sendPackage(const char *package, size_t size);
@@ -35,7 +38,7 @@ class BluetoothDispatcher {
   private:
     void _onConfirmRequest(uint16_t pin);
     void _onAuthComplete(boolean success) const;
-    void _onDeviceConnected(BTAddress device) const;
+    void _onDeviceConnected(BTAddress device);
 
     const char *_device_name = nullptr;
     BluetoothSerial *_controller = nullptr;
@@ -44,6 +47,6 @@ class BluetoothDispatcher {
     constexpr static const char *_tag = "BluetoothDispatcher";
 
     static constexpr uint16_t _buffer_size = 522;
-    static constexpr uint16_t _available_buffer_size = 512;
+    static constexpr uint16_t _real_buffer_size = 512;
     mString<_buffer_size> _buffer;
 };

src/Sensors/Barometer.hpp

@@ -1,3 +1,6 @@
+// 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 "GyverBME280.h"
 
 typedef std::function<void()> OnMeasuringFinishedCb;

src/Sensors/BatteryController.hpp

@@ -1,19 +1,39 @@
+// 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 "board_pins.h"
 
 class BatteryController {
   public:
-    BatteryController() {}
+    BatteryController(const uint16_t batteryPin, const uint16_t battery_data_switch_pin) {
+        _battery_pin = batteryPin;
+        _battery_data_switch_pin = battery_data_switch_pin;
+    }
 
     void initialize() {
-        pinMode(BATTERY_DATA_SWITCH_PIN, OUTPUT);
+        pinMode(_battery_data_switch_pin, OUTPUT);
-        digitalWrite(BATTERY_DATA_SWITCH_PIN, HIGH);
+        digitalWrite(_battery_data_switch_pin, HIGH);
+        analogReadResolution(12);
+        analogSetWidth(12);
+        adcAttachPin(_battery_pin);
     }
 
     float measureVoltage() {
-        return analogRead(BATTERY_DATA_PIN) * 3.3 / 4095;
+        return analogRead(BATTERY_DATA_PIN) * 3.3f / 4096.f;
     }
 
-    int percent(int minVoltage = 7200, int maxVoltage = 8400) {
+    int percent(const float minVoltage = 7.2f, const float maxVoltage = 8.4f,
-        return map(int(measureVoltage() * 1000), minVoltage, maxVoltage, 0, 100);
+                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));
+    }
+
+  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/Sensors/MPU.hpp

@@ -1,3 +1,6 @@
+// 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 "MPU6050_6Axis_MotionApps20.h"
 #include "Preferences.h"
 #include "board_pins.h"
@@ -98,9 +101,11 @@ class MPU {
 
                 _isCalibration = false;
                 _updateOffsets();
+                _calibrationsIterCounter = 0;
                 if (_calibrationFinishedCb) {
                     _calibrationFinishedCb();
                 }
+                ESP_LOGI(_TAG, "Calibration finished!");
             } else {
                 _axOffset += _ax;
                 _ayOffset += _ay;
@@ -110,6 +115,7 @@ class MPU {
                 _gzOffset += _gz;
                 _prOffset[0] += _ypr[1];
                 _prOffset[1] += _ypr[2];
+                _calibrationsIterCounter++;
             }
         }
 

src/Sensors/Sensors.hpp

@@ -1,3 +1,6 @@
+// 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 "Barometer.hpp"
 #include "BatteryController.hpp"
 #include "Filters/Kalman2DFilter.hpp"

src/board_pins.h

@@ -1,3 +1,6 @@
+// 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 "driver/adc.h"
 
 #define I2C_SDA_PIN 21

src/main.cpp

@@ -4,8 +4,6 @@
 #include "App.hpp"
 #include "BoardI2C.hpp"
 
-#define IGNORE_CHARGE true
-
 BoardI2C i2c;
 
 static Application *app = nullptr;
@@ -15,11 +13,12 @@ void setup() {
     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 Sensors(new Barometer(new GyverBME280(i2c)),
-                    new Kalman2DFilter(10.f, 1.f, 1.8f), new BatteryController()),
+                    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), IGNORE_CHARGE)));
+        new PIDController(1.f, 1.f, 1.f, new BrushedMotor(1, 2, 3, 4, 3), 3), true)));
 }
 
 void loop() {