From 69c736b1eb5c65f2cac39c11b23772ae2b37ff21 Mon Sep 17 00:00:00 2001 From: gogacoder Date: Fri, 1 Mar 2024 22:58:59 +0700 Subject: [PATCH] Project structure redefinition --- platformio.ini | 2 +- src/App.hpp | 3 +++ src/BoardI2C.hpp | 3 +++ src/Filters/Kalman2DFilter.hpp | 6 ++++- src/Logic/FlightController.cpp | 20 +++++++++------- src/Logic/FlightController.hpp | 15 +++++++++--- src/Logic/FlightDispatcher.cpp | 40 ++++++++++++------------------- src/Logic/FlightDispatcher.hpp | 9 +++++-- src/Logic/PID.hpp | 3 +++ src/Motor/BrushedMotor.hpp | 3 +++ src/RF/BluetoothDispatcher.cpp | 20 +++++++++------- src/RF/BluetoothDispatcher.hpp | 9 ++++--- src/Sensors/Barometer.hpp | 3 +++ src/Sensors/BatteryController.hpp | 32 ++++++++++++++++++++----- src/Sensors/MPU.hpp | 6 +++++ src/Sensors/Sensors.hpp | 3 +++ src/board_pins.h | 3 +++ src/main.cpp | 9 ++++--- 18 files changed, 126 insertions(+), 63 deletions(-) diff --git a/platformio.ini b/platformio.ini index 3e41bd4..e079902 100644 --- a/platformio.ini +++ b/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 diff --git a/src/App.hpp b/src/App.hpp index c597f7d..3e3c948 100644 --- a/src/App.hpp +++ b/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" diff --git a/src/BoardI2C.hpp b/src/BoardI2C.hpp index f9f3a93..ab7d4d9 100644 --- a/src/BoardI2C.hpp +++ b/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" diff --git a/src/Filters/Kalman2DFilter.hpp b/src/Filters/Kalman2DFilter.hpp index e3e8ee2..8f15a74 100644 --- a/src/Filters/Kalman2DFilter.hpp +++ b/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 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; diff --git a/src/Logic/FlightController.cpp b/src/Logic/FlightController.cpp index 8252e4b..41eb672 100644 --- a/src/Logic/FlightController.cpp +++ b/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 { - 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; +std::unique_ptr FlightController::pidSettings() const { + std::unique_ptr settings = std::make_unique(); + 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); } \ No newline at end of file diff --git a/src/Logic/FlightController.hpp b/src/Logic/FlightController.hpp index 85f80b8..3d915db 100644 --- a/src/Logic/FlightController.hpp +++ b/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 #include "Sensors/Sensors.hpp" +#include 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() const; private: void _updateBatteryCharge(); [[deprecated]] void _takeoff(); diff --git a/src/Logic/FlightDispatcher.cpp b/src/Logic/FlightDispatcher.cpp index e5abe33..fdf0b1c 100644 --- a/src/Logic/FlightDispatcher.cpp +++ b/src/Logic/FlightDispatcher.cpp @@ -43,22 +43,12 @@ void FlightDispatcher::tick() { } 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)); + _sendTelemetry(); } 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["i"] = settings[0].i; - pkg["d"] = settings[0].d; + pkg["p"] = settings->pid1.p; + pkg["i"] = settings->pid1.i; + pkg["d"] = settings->pid1.d; pkg.endObj(); pkg.beginObj("p2"); - pkg["p"] = settings[1].p; - pkg["i"] = settings[1].i; - pkg["d"] = settings[1].d; + pkg["p"] = settings->pid2.p; + pkg["i"] = settings->pid2.i; + pkg["d"] = settings->pid2.d; pkg.endObj(); pkg.beginObj("p3"); - pkg["p"] = settings[2].p; - pkg["i"] = settings[2].i; - pkg["d"] = settings[2].d; + pkg["p"] = settings->pid3.p; + pkg["i"] = settings->pid3.i; + 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()))); } diff --git a/src/Logic/FlightDispatcher.hpp b/src/Logic/FlightDispatcher.hpp index b061278..18a0751 100644 --- a/src/Logic/FlightDispatcher.hpp +++ b/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 +// 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> _routes; }; \ No newline at end of file diff --git a/src/Logic/PID.hpp b/src/Logic/PID.hpp index 08dd358..9b62542 100644 --- a/src/Logic/PID.hpp +++ b/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" diff --git a/src/Motor/BrushedMotor.hpp b/src/Motor/BrushedMotor.hpp index ecee0cd..20e9a7f 100644 --- a/src/Motor/BrushedMotor.hpp +++ b/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 // TODO: implement class diff --git a/src/RF/BluetoothDispatcher.cpp b/src/RF/BluetoothDispatcher.cpp index 946b1da..98ebb37 100644 --- a/src/RF/BluetoothDispatcher.cpp +++ b/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) { - _buffer += (char)_controller->read(); - } - if (_buffer.endsWith("\n\r")) { + while (_controller->available()) { _buffer += (char)_controller->read(); } + if (_buffer.endsWith(message_delimeter)) { 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); } diff --git a/src/RF/BluetoothDispatcher.hpp b/src/RF/BluetoothDispatcher.hpp index c55efc6..e33641e 100644 --- a/src/RF/BluetoothDispatcher.hpp +++ b/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; }; \ No newline at end of file diff --git a/src/Sensors/Barometer.hpp b/src/Sensors/Barometer.hpp index e92fb83..714df5e 100644 --- a/src/Sensors/Barometer.hpp +++ b/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 OnMeasuringFinishedCb; diff --git a/src/Sensors/BatteryController.hpp b/src/Sensors/BatteryController.hpp index a116530..35b5d4f 100644 --- a/src/Sensors/BatteryController.hpp +++ b/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); - digitalWrite(BATTERY_DATA_SWITCH_PIN, HIGH); + pinMode(_battery_data_switch_pin, OUTPUT); + 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) { - return map(int(measureVoltage() * 1000), minVoltage, maxVoltage, 0, 100); + 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)); + } + + 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; } }; \ No newline at end of file diff --git a/src/Sensors/MPU.hpp b/src/Sensors/MPU.hpp index fd89e81..a3d7231 100644 --- a/src/Sensors/MPU.hpp +++ b/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++; } } diff --git a/src/Sensors/Sensors.hpp b/src/Sensors/Sensors.hpp index b548787..bc54481 100644 --- a/src/Sensors/Sensors.hpp +++ b/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" diff --git a/src/board_pins.h b/src/board_pins.h index 20ee6f7..925c278 100644 --- a/src/board_pins.h +++ b/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 diff --git a/src/main.cpp b/src/main.cpp index 1d9334f..3bbeba1 100644 --- a/src/main.cpp +++ b/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 Kalman2DFilter(10.f, 1.f, 1.8f), new BatteryController()), + 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(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() {