From fa0a664e43bb2582ffc021f7df8cdcd4ff8325d4 Mon Sep 17 00:00:00 2001 From: gogacoder Date: Sat, 27 Jan 2024 21:35:57 +0700 Subject: [PATCH] Code refactoring --- BoardI2C.hpp | 2 +- Logic/FlightController.cpp | 5 ++++- Logic/FlightController.hpp | 1 + Logic/FlightDispatcher.cpp | 9 +++++++-- Logic/FlightDispatcher.hpp | 2 +- Logic/PID.hpp | 1 + Motor/BrushedMotor.cpp | 10 +++++++++- RF/BluetoothDispatcher.cpp | 5 ++++- RF/BluetoothDispatcher.hpp | 2 +- Sensors/Barometer.hpp | 2 +- Sensors/MPU.hpp | 12 ++++++------ Sensors/Sensors.cpp | 13 +++++++++---- Sensors/Sensors.hpp | 9 +++++---- main.cpp | 24 +++++++----------------- 14 files changed, 57 insertions(+), 40 deletions(-) diff --git a/BoardI2C.hpp b/BoardI2C.hpp index d6a98ba..f9f3a93 100644 --- a/BoardI2C.hpp +++ b/BoardI2C.hpp @@ -4,7 +4,7 @@ class BoardI2C : public TwoWire { public: - BoardI2C(bool loop_on_fail = true) : TwoWire(0) { + BoardI2C(volatile bool loop_on_fail = true) : TwoWire(0) { if (begin(I2C_SDA_PIN, I2C_SCL_PIN, 100000)) { ESP_LOGI("I2CBus", "Bus initialized"); } else { diff --git a/Logic/FlightController.cpp b/Logic/FlightController.cpp index 59eb649..8252e4b 100644 --- a/Logic/FlightController.cpp +++ b/Logic/FlightController.cpp @@ -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 "FlightController.hpp" FlightController::FlightController(Sensors *sensors, PIDController *pid1, PIDController *pid2, @@ -186,6 +189,6 @@ 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 }; - PidSettings settings[] = { pid1, pid2, pid3 }; + static PidSettings settings[] = { pid1, pid2, pid3 }; return settings; } \ No newline at end of file diff --git a/Logic/FlightController.hpp b/Logic/FlightController.hpp index 812ed49..85f80b8 100644 --- a/Logic/FlightController.hpp +++ b/Logic/FlightController.hpp @@ -1,4 +1,5 @@ #include "PID.hpp" +#include #include "Sensors/Sensors.hpp" enum DeviceStatus { diff --git a/Logic/FlightDispatcher.cpp b/Logic/FlightDispatcher.cpp index afa8bf2..e5abe33 100644 --- a/Logic/FlightDispatcher.cpp +++ b/Logic/FlightDispatcher.cpp @@ -1,7 +1,10 @@ +// 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, bool loop_on_fail) { + FlightController *flightController, volatile bool loop_on_fail) { assert(bluetoothDispatcher != nullptr); assert(flightController != nullptr); @@ -119,7 +122,9 @@ void FlightDispatcher::_onNewMessageReceived(char *package) { void FlightDispatcher::_changeStatus(const DeviceStatus &newStatus) { switch (newStatus) { case DeviceStatus::IsImuCalibration: _flightController->startImuCalibration(); break; - case DeviceStatus::IsFlying: _flightController->startTakeoff(); break; + case DeviceStatus::IsFlying: + _changeStatus(DeviceStatus::IsPreparingForTakeoff); + break; case DeviceStatus::IsPreparingForTakeoff: _flightController->startTakeoff(); break; case DeviceStatus::IsBoarding: _flightController->startBoarding(); break; default: break; diff --git a/Logic/FlightDispatcher.hpp b/Logic/FlightDispatcher.hpp index af6b1d6..b061278 100644 --- a/Logic/FlightDispatcher.hpp +++ b/Logic/FlightDispatcher.hpp @@ -7,7 +7,7 @@ class FlightDispatcher { /* Deserialize state and update it in FlightController. */ public: FlightDispatcher(BluetoothDispatcher *bluetoothDispatcher, - FlightController *flightController, bool loop_on_fail = true); + FlightController *flightController, volatile bool loop_on_fail = true); ~FlightDispatcher(); void tick(); private: diff --git a/Logic/PID.hpp b/Logic/PID.hpp index d022306..08dd358 100644 --- a/Logic/PID.hpp +++ b/Logic/PID.hpp @@ -11,6 +11,7 @@ class PIDController : public GyverPID { 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); diff --git a/Motor/BrushedMotor.cpp b/Motor/BrushedMotor.cpp index f949957..89f3556 100644 --- a/Motor/BrushedMotor.cpp +++ b/Motor/BrushedMotor.cpp @@ -1,7 +1,15 @@ +// 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) {} + uint32_t McpwmResolutionHz, int McpwmGroupId) { + _mcpwmResolutionHz = McpwmResolutionHz; + _pwmAGpioNum = McpwmGroupId; + _pwmBGpioNum = PwmBGpioNum; + _pwmFreqHz = PwmFreqHz; +} void BrushedMotor::enable() {} diff --git a/RF/BluetoothDispatcher.cpp b/RF/BluetoothDispatcher.cpp index fc185b6..946b1da 100644 --- a/RF/BluetoothDispatcher.cpp +++ b/RF/BluetoothDispatcher.cpp @@ -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 "BluetoothDispatcher.hpp" static DeviceConnectedCb deviceConnectedCallback = nullptr; @@ -11,7 +14,7 @@ BluetoothDispatcher::BluetoothDispatcher(BluetoothSerial *controller, const char _controller = controller; } -bool BluetoothDispatcher::initialize(bool loop_on_fail, int readTimeoutMS) { +bool BluetoothDispatcher::initialize(volatile bool loop_on_fail, int readTimeoutMS) { _controller->enableSSP(); _controller->onConfirmRequest([this](uint16_t pin) { _onConfirmRequest(pin); diff --git a/RF/BluetoothDispatcher.hpp b/RF/BluetoothDispatcher.hpp index aff5d47..c55efc6 100644 --- a/RF/BluetoothDispatcher.hpp +++ b/RF/BluetoothDispatcher.hpp @@ -25,7 +25,7 @@ typedef std::function NewPackageCallback; class BluetoothDispatcher { public: BluetoothDispatcher(BluetoothSerial *controller, const char *device_name = "Helicopter"); - bool initialize(bool loop_on_fail = true, int readTimeoutMS = 2); + bool initialize(volatile bool loop_on_fail = true, int readTimeoutMS = 2); void tick(); void onNewPackageReceived(NewPackageCallback newPackageReceivedCb); void onNewDeviceConnected(DeviceConnectedCb deviceConnectedCb); diff --git a/Sensors/Barometer.hpp b/Sensors/Barometer.hpp index d683050..e92fb83 100644 --- a/Sensors/Barometer.hpp +++ b/Sensors/Barometer.hpp @@ -64,7 +64,7 @@ class Barometer { GyverBME280 *_bme; float _startedAltitude = 0; bool _isCalibration = false; - uint32_t _calibrationTimer; + uint32_t _calibrationTimer = 0; int _calibrationIterationsCounter = 0; OnMeasuringFinishedCb _measuaringHeightFinishedCb = nullptr; static constexpr int _calibrationIterationsCount = 1500; diff --git a/Sensors/MPU.hpp b/Sensors/MPU.hpp index be952bc..fd89e81 100644 --- a/Sensors/MPU.hpp +++ b/Sensors/MPU.hpp @@ -196,17 +196,17 @@ class MPU { private: MPU6050_6Axis_MotionApps20 *_mpu = nullptr; - float _ypr[3]; - float _ax, _ay, _az; - float _gx, _gy, _gz; - float _gravity; + 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]; // yaw isn't used + float _prOffset[2] = { 0 }; // yaw isn't used - uint8_t _fifoBuffer[45]; + uint8_t _fifoBuffer[45] = { 0 }; Preferences _preferences; const char *_TAG = "MPU6050 module"; diff --git a/Sensors/Sensors.cpp b/Sensors/Sensors.cpp index 3de981b..b1eb7ae 100644 --- a/Sensors/Sensors.cpp +++ b/Sensors/Sensors.cpp @@ -1,6 +1,10 @@ +// 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, bool loop_on_fail) { +Sensors::Sensors(Barometer *barometer, MPU *mpu, Kalman2DFilter *filter, + BatteryController *battery, volatile bool loop_on_fail) { assert(barometer != nullptr); assert(mpu != nullptr); assert(filter != nullptr); @@ -56,7 +60,7 @@ float Sensors::rawFlightHeight(void) const { float Sensors::flightHeight(void) const { /* Returns flight height from cache immediatly */ - if (_flightHeightFromBarometer == NAN) { + if (_flightHeightFromBarometer == 0.f) { ESP_LOGW(_tag, "flightHeight called, but tick() has called never"); return rawFlightHeight(); } else { @@ -65,7 +69,7 @@ float Sensors::flightHeight(void) const { } MpuData Sensors::mpuData() const { - if (_mpuData.ax == NAN and _mpuData.gravity == NAN) { + if (_mpuData.ax == 0.f and _mpuData.gravity == 0.f) { ESP_LOGW(_tag, "MPU data looks like .tick() has never called"); } return _mpuData; @@ -78,7 +82,8 @@ bool Sensors::tick(void) { bool err; bool isMpuDataReady = _mpu->tick(err); if (isMpuDataReady and !err) { - _2d_filter->filter(_mpu->accZInertial(), _barometer->flightHeight(), _flightHeightFromBarometer, _zVelocityAltitude); + _2d_filter->filter(_mpu->accZInertial(), _barometer->flightHeight(), + _flightHeightFromBarometer, _zVelocityAltitude); _mpuData = { .ax = _mpu->ax(), .ay = _mpu->ay(), .az = _mpu->az(), diff --git a/Sensors/Sensors.hpp b/Sensors/Sensors.hpp index 0edad5a..b548787 100644 --- a/Sensors/Sensors.hpp +++ b/Sensors/Sensors.hpp @@ -16,7 +16,8 @@ struct MpuData { class Sensors { public: - Sensors(Barometer *barometer, MPU *mpu, Kalman2DFilter *filter, BatteryController *battery, bool loop_on_fail = true); + Sensors(Barometer *barometer, MPU *mpu, Kalman2DFilter *filter, + BatteryController *battery, bool loop_on_fail = true); ~Sensors(); @@ -42,9 +43,9 @@ class Sensors { Kalman2DFilter *_2d_filter = nullptr; /* cached filtered values */ - float _flightHeightFromBarometer = NAN; - float _zVelocityAltitude = NAN; - MpuData _mpuData = { NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN }; + float _flightHeightFromBarometer = 0.f; + float _zVelocityAltitude = 0.f; + MpuData _mpuData = { 0.f }; static constexpr const char *_tag = "Sensors"; diff --git a/main.cpp b/main.cpp index dd2f438..1d9334f 100644 --- a/main.cpp +++ b/main.cpp @@ -1,12 +1,12 @@ +// 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" -//#include "MPU6050_6Axis_MotionApps20.h" + #define IGNORE_CHARGE true BoardI2C i2c; -/* -BluetoothDispatcher bluetoothDispatcher(new BluetoothSerial()); -Sensors *sensors = nullptr; */ static Application *app = nullptr; @@ -17,21 +17,11 @@ void setup() { 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, 1, 1, new BrushedMotor(1, 2, 3, 4, 1), 1), - new PIDController(1, 1, 1, new BrushedMotor(1, 2, 3, 4, 2), 2), - new PIDController(1, 1, 1, new BrushedMotor(1, 2, 3, 4, 3), 3), IGNORE_CHARGE))); - /*sensors = 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()); sensors->measureBaseAltitudeSync(); - bluetoothDispatcher.initialize(); - Serial.println("System initialized");*/ + 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(); - /* - sensors->tick(); - bluetoothDispatcher.tick(); - delay(1);*/ } \ No newline at end of file