From 941aedd79faa8174a31cf8e618825ada5bf52812 Mon Sep 17 00:00:00 2001 From: gogacoder Date: Sat, 27 Jan 2024 22:07:53 +0700 Subject: [PATCH] moved all existing files to new 'src' directory --- .clang-format | 160 ---------------------- .gitignore | 39 +----- App.hpp | 23 ---- BoardI2C.hpp | 16 --- Filters/Kalman2DFilter.hpp | 43 ------ Filters/kalman.hpp | 46 ------- Filters/median3.hpp | 19 --- Filters/runningAverage.hpp | 49 ------- Logic/FlightController.cpp | 194 -------------------------- Logic/FlightController.hpp | 74 ---------- Logic/FlightDispatcher.cpp | 180 ------------------------ Logic/FlightDispatcher.hpp | 35 ----- Logic/PID.hpp | 56 -------- Motor/BrushedMotor.cpp | 30 ---- Motor/BrushedMotor.hpp | 29 ---- RF/BluetoothDispatcher.cpp | 105 -------------- RF/BluetoothDispatcher.hpp | 49 ------- Sensors/Barometer.hpp | 72 ---------- Sensors/BatteryController.hpp | 19 --- Sensors/MPU.hpp | 250 ---------------------------------- Sensors/Sensors.cpp | 119 ---------------- Sensors/Sensors.hpp | 52 ------- board_pins.h | 8 -- main.cpp | 27 ---- 24 files changed, 5 insertions(+), 1689 deletions(-) delete mode 100644 .clang-format delete mode 100644 App.hpp delete mode 100644 BoardI2C.hpp delete mode 100644 Filters/Kalman2DFilter.hpp delete mode 100644 Filters/kalman.hpp delete mode 100644 Filters/median3.hpp delete mode 100644 Filters/runningAverage.hpp delete mode 100644 Logic/FlightController.cpp delete mode 100644 Logic/FlightController.hpp delete mode 100644 Logic/FlightDispatcher.cpp delete mode 100644 Logic/FlightDispatcher.hpp delete mode 100644 Logic/PID.hpp delete mode 100644 Motor/BrushedMotor.cpp delete mode 100644 Motor/BrushedMotor.hpp delete mode 100644 RF/BluetoothDispatcher.cpp delete mode 100644 RF/BluetoothDispatcher.hpp delete mode 100644 Sensors/Barometer.hpp delete mode 100644 Sensors/BatteryController.hpp delete mode 100644 Sensors/MPU.hpp delete mode 100644 Sensors/Sensors.cpp delete mode 100644 Sensors/Sensors.hpp delete mode 100644 board_pins.h delete mode 100644 main.cpp diff --git a/.clang-format b/.clang-format deleted file mode 100644 index 9f3311b..0000000 --- a/.clang-format +++ /dev/null @@ -1,160 +0,0 @@ -# Source: https://github.com/arduino/tooling-project-assets/tree/main/other/clang-format-configuration ---- -TabWidth: 2 -AccessModifierOffset: -2 -AlignAfterOpenBracket: Align -AlignConsecutiveAssignments: None -AlignConsecutiveBitFields: None -AlignConsecutiveDeclarations: None -AlignConsecutiveMacros: None -AlignEscapedNewlines: DontAlign -AlignOperands: Align -AlignTrailingComments: true -AllowAllArgumentsOnNextLine: true -AllowAllConstructorInitializersOnNextLine: true -AllowAllParametersOfDeclarationOnNextLine: true -AllowShortBlocksOnASingleLine: Always -AllowShortCaseLabelsOnASingleLine: true -AllowShortEnumsOnASingleLine: true -AllowShortFunctionsOnASingleLine: Empty -AllowShortLambdasOnASingleLine: Empty -AllowShortLoopsOnASingleLine: true -AlwaysBreakAfterDefinitionReturnType: None -AlwaysBreakAfterReturnType: None -AlwaysBreakBeforeMultilineStrings: false -AlwaysBreakTemplateDeclarations: No -AttributeMacros: - - __capability -BinPackArguments: true -BinPackParameters: true -BitFieldColonSpacing: Both -BraceWrapping: - AfterCaseLabel: false - AfterClass: false - AfterControlStatement: Never - AfterEnum: false - AfterFunction: false - AfterNamespace: false - AfterObjCDeclaration: false - AfterStruct: false - AfterUnion: false - AfterExternBlock: false - BeforeCatch: false - BeforeElse: false - BeforeLambdaBody: false - BeforeWhile: false - IndentBraces: false - SplitEmptyFunction: true - SplitEmptyRecord: true - SplitEmptyNamespace: true -BreakAfterJavaFieldAnnotations: false -BreakBeforeBinaryOperators: NonAssignment -BreakBeforeBraces: Attach -BreakBeforeConceptDeclarations: false -BreakBeforeInheritanceComma: false -BreakBeforeTernaryOperators: true -BreakConstructorInitializers: BeforeColon -BreakConstructorInitializersBeforeComma: false -BreakInheritanceList: BeforeColon -BreakStringLiterals: false -ColumnLimit: 90 -CommentPragmas: '' -CompactNamespaces: false -ConstructorInitializerAllOnOneLineOrOnePerLine: false -ConstructorInitializerIndentWidth: 2 -ContinuationIndentWidth: 2 -Cpp11BracedListStyle: false -DeriveLineEnding: true -DerivePointerAlignment: true -DisableFormat: false -EmptyLineBeforeAccessModifier: Leave -ExperimentalAutoDetectBinPacking: false -FixNamespaceComments: false -ForEachMacros: - - foreach - - Q_FOREACH - - BOOST_FOREACH -IncludeBlocks: Preserve -IncludeCategories: - - Regex: '^"(llvm|llvm-c|clang|clang-c)/' - Priority: 2 - SortPriority: 0 - CaseSensitive: false - - Regex: '^(<|"(gtest|gmock|isl|json)/)' - Priority: 3 - SortPriority: 0 - CaseSensitive: false - - Regex: '.*' - Priority: 1 - SortPriority: 0 - CaseSensitive: false -IncludeIsMainRegex: '' -IncludeIsMainSourceRegex: '' -IndentGotoLabels: false -IndentPPDirectives: None -IndentRequires: true -IndentWidth: 2 -IndentWrappedFunctionNames: false -InsertTrailingCommas: None -JavaScriptQuotes: Leave -JavaScriptWrapImports: true -KeepEmptyLinesAtTheStartOfBlocks: true -Language: Cpp -MacroBlockBegin: '' -MacroBlockEnd: '' -MaxEmptyLinesToKeep: 100000 -NamespaceIndentation: None -ObjCBinPackProtocolList: Auto -ObjCBlockIndentWidth: 2 -ObjCBreakBeforeNestedBlockParam: true -ObjCSpaceAfterProperty: false -ObjCSpaceBeforeProtocolList: true -PenaltyBreakAssignment: 1 -PenaltyBreakBeforeFirstCallParameter: 1 -PenaltyBreakComment: 1 -PenaltyBreakFirstLessLess: 1 -PenaltyBreakString: 1 -PenaltyBreakTemplateDeclaration: 1 -PenaltyExcessCharacter: 1 -PenaltyIndentedWhitespace: 1 -PenaltyReturnTypeOnItsOwnLine: 1 -PointerAlignment: Right -ReflowComments: false -SortJavaStaticImport: Before -SortUsingDeclarations: false -SpaceAfterCStyleCast: false -SpaceAfterLogicalNot: false -SpaceAfterTemplateKeyword: false -SpaceAroundPointerQualifiers: Default -SpaceBeforeAssignmentOperators: true -SpaceBeforeCaseColon: false -SpaceBeforeCpp11BracedList: false -SpaceBeforeCtorInitializerColon: true -SpaceBeforeInheritanceColon: true -SpaceBeforeParens: ControlStatements -SpaceBeforeRangeBasedForLoopColon: true -SpaceBeforeSquareBrackets: false -SpaceInEmptyBlock: false -SpaceInEmptyParentheses: false -SpacesBeforeTrailingComments: 2 -SpacesInCStyleCastParentheses: false -SpacesInConditionalStatement: false -SpacesInContainerLiterals: false -SpacesInParentheses: false -SpacesInSquareBrackets: false -Standard: Auto -StatementAttributeLikeMacros: - - Q_EMIT -StatementMacros: - - Q_UNUSED - - QT_REQUIRE_VERSION -TabWidth: 2 -UseCRLF: false -UseTab: Never -WhitespaceSensitiveMacros: - - STRINGIZE - - PP_STRINGIZE - - BOOST_PP_STRINGIZE - - NS_SWIFT_NAME - - CF_SWIFT_NAME -IndentWidth: 4 \ No newline at end of file diff --git a/.gitignore b/.gitignore index e257658..89cc49c 100644 --- a/.gitignore +++ b/.gitignore @@ -1,34 +1,5 @@ -# ---> C++ -# Prerequisites -*.d - -# Compiled Object files -*.slo -*.lo -*.o -*.obj - -# Precompiled Headers -*.gch -*.pch - -# Compiled Dynamic libraries -*.so -*.dylib -*.dll - -# Fortran module files -*.mod -*.smod - -# Compiled Static libraries -*.lai -*.la -*.a -*.lib - -# Executables -*.exe -*.out -*.app - +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch diff --git a/App.hpp b/App.hpp deleted file mode 100644 index c597f7d..0000000 --- a/App.hpp +++ /dev/null @@ -1,23 +0,0 @@ -#include "Logic/FlightDispatcher.hpp" -#include "esp_log.h" - -class Application { - public: - Application(FlightDispatcher *dispatcher) { - assert(dispatcher != nullptr); - _dispatcher = dispatcher; - ESP_LOGI(_tag, "Application startup complete"); - } - - void tick() { - _dispatcher->tick(); - } - - ~Application() { - ESP_LOGI(_tag, "Application destroyed"); - delete _dispatcher; - } - private: - FlightDispatcher *_dispatcher = nullptr; - static constexpr const char *_tag = "Application"; -}; \ No newline at end of file diff --git a/BoardI2C.hpp b/BoardI2C.hpp deleted file mode 100644 index f9f3a93..0000000 --- a/BoardI2C.hpp +++ /dev/null @@ -1,16 +0,0 @@ -#include "Wire.h" -#include "board_pins.h" -#include "esp_log.h" - -class BoardI2C : public TwoWire { - public: - BoardI2C(volatile bool loop_on_fail = true) : TwoWire(0) { - if (begin(I2C_SDA_PIN, I2C_SCL_PIN, 100000)) { - ESP_LOGI("I2CBus", "Bus initialized"); - } else { - ESP_LOGE("I2CBus", "Failed to initialize the i2c software bus!"); - while (loop_on_fail) - ; - } - } -}; \ No newline at end of file diff --git a/Filters/Kalman2DFilter.hpp b/Filters/Kalman2DFilter.hpp deleted file mode 100644 index e3e8ee2..0000000 --- a/Filters/Kalman2DFilter.hpp +++ /dev/null @@ -1,43 +0,0 @@ -#include - -class Kalman2DFilter { - public: - Kalman2DFilter(float dt = 4.f, float accelUncertainty = 10.f, float barometerUncertainty = 100.f) { - dt /= 1000.f; - F = { 1, dt, 0, 1 }; - G = { 0.5f * dt * dt, dt }; - H = { 1, 0 }; - I = { 1, 0, 0, 1 }; - Q = G * ~G * accelUncertainty * accelUncertainty; - R = { barometerUncertainty * barometerUncertainty }; - P = { 0, 0, 0, 0 }; - S = { 0, 0 }; - } - - void filter(const float &AccZInertial, const float &AltitudeBarometer, float &AltitudeKalman, float &VelocityVerticalKalman) { - Acc = { AccZInertial }; - S = F * S + G * Acc; - P = F * P * ~F + Q; - L = H * P * ~H + R; - K = P * ~H * Inverse(L); - M = { AltitudeBarometer }; - S = S + K * (M - H * S); - AltitudeKalman = S(0, 0); - VelocityVerticalKalman = S(1, 0); - P = (I - K * H) * P; - } - - private: - BLA::Matrix<2, 2> F; - BLA::Matrix<2, 1> G; - BLA::Matrix<2, 2> P; - BLA::Matrix<2, 2> Q; - BLA::Matrix<2, 1> S; - BLA::Matrix<1, 2> H; - BLA::Matrix<2, 2> I; - BLA::Matrix<1, 1> Acc; - BLA::Matrix<2, 1> K; - BLA::Matrix<1, 1> R; - BLA::Matrix<1, 1> L; - BLA::Matrix<1, 1> M; -}; \ No newline at end of file diff --git a/Filters/kalman.hpp b/Filters/kalman.hpp deleted file mode 100644 index 373fe88..0000000 --- a/Filters/kalman.hpp +++ /dev/null @@ -1,46 +0,0 @@ -// упрощённый Калман для одномерного случая - -#ifndef _GKalman_h -#define _GKalman_h - -class GKalman { -public: - // разброс измерения, разброс оценки, скорость изменения значений - GKalman(float mea_e, float est_e, float q) { - setParameters(mea_e, est_e, q); - } - - // разброс измерения, скорость изменения значений (разброс измерения принимается равным разбросу оценки) - GKalman(float mea_e, float q) { - setParameters(mea_e, mea_e, q); - } - - // разброс измерения, разброс оценки, скорость изменения значений - void setParameters(float mea_e, float est_e, float q) { - _err_measure = mea_e; - _err_estimate = est_e; - _q = q; - } - - // разброс измерения, скорость изменения значений (разброс измерения принимается равным разбросу оценки) - void setParameters(float mea_e, float q) { - setParameters(mea_e, mea_e, q); - } - - // возвращает фильтрованное значение - float filtered(float value) { - float _kalman_gain, _current_estimate; - _kalman_gain = _err_estimate / (_err_estimate + _err_measure); - _current_estimate = _last_estimate + _kalman_gain * (value - _last_estimate); - _err_estimate = (1.0 - _kalman_gain)*_err_estimate + fabs(_last_estimate-_current_estimate)*_q; - _last_estimate=_current_estimate; - return _current_estimate; - } - -private: - float _err_measure = 0.0; - float _err_estimate = 0.0; - float _q = 0.0; - float _last_estimate = 0.0; -}; -#endif \ No newline at end of file diff --git a/Filters/median3.hpp b/Filters/median3.hpp deleted file mode 100644 index 5c59c6b..0000000 --- a/Filters/median3.hpp +++ /dev/null @@ -1,19 +0,0 @@ -// быстрый медианный фильтр 3-го порядка - -#ifndef _GMedian3_h -#define _GMedian3_h - -template < typename TYPE > -class GMedian3 { -public: - TYPE filtered(TYPE value) { // возвращает фильтрованное значение - buf[_counter] = value; - if (++_counter > 2) _counter = 0; - return (max(buf[0], buf[1]) == max(buf[1], buf[2])) ? max(buf[0], buf[2]) : max(buf[1], min(buf[0], buf[2])); - } - -private: - TYPE buf[3]; - uint8_t _counter = 0; -}; -#endif \ No newline at end of file diff --git a/Filters/runningAverage.hpp b/Filters/runningAverage.hpp deleted file mode 100644 index fd338a6..0000000 --- a/Filters/runningAverage.hpp +++ /dev/null @@ -1,49 +0,0 @@ -// экспоненциальное бегущее среднее - -#ifndef _GFilterRA_h -#define _GFilterRA_h - -class GFilterRA { -public: - GFilterRA(){} - - GFilterRA(float coef, uint16_t interval) { - _coef = coef; - _prd = interval; - } - - GFilterRA(float coef) { - _coef = coef; - } - - void setCoef(float coef) { - _coef = coef; - } - - void setPeriod(uint16_t interval) { - _prd = interval; - } - - float filteredTime(float value) { - if (millis() - _tmr >= _prd) { - _tmr += _prd; - filtered(value); - } - return _fil; - } - - float filtered(float value) { - return _fil += (value - _fil) * _coef; - } - - // - void setStep(uint16_t interval) { - _prd = interval; - } - -private: - float _coef = 0.0, _fil = 0.0; - uint32_t _tmr = 0; - uint16_t _prd = 0; -}; -#endif \ No newline at end of file diff --git a/Logic/FlightController.cpp b/Logic/FlightController.cpp deleted file mode 100644 index 8252e4b..0000000 --- a/Logic/FlightController.cpp +++ /dev/null @@ -1,194 +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 "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; - _pid1 = pid1; - _pid2 = pid2; - _pid3 = pid3; - - _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::IsImuCalibration: break; - case DeviceStatus::IsBoarding: - /* TODO: implement boarding */ - break; - case DeviceStatus::IsFlying: - /* TODO: implement flying */ - if (hasUpdates) { - _pid1->tick(); - _pid2->tick(); - _pid3->tick(); - } - break; - } -} - -ReportedDeviceState FlightController::currentDeviceState() const { - return { .batteryCharge = _sensors->batteryCharge(), - .flightHeight = _sensors->flightHeight(), - .status = _status, - .mpuState = _sensors->mpuData() }; -} - -void FlightController::startTakeoff() { - /* Start preparation for takeoff */ - _updateBatteryCharge(); - if (_status != DeviceStatus::Idle) - 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) { - startBoarding(); - } 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() { - if (_status != DeviceStatus::Idle) - return; - _sensors->onMpuCalibrationFinished([this]() { - this->_status = DeviceStatus::Idle; - }); -} - -void FlightController::moveToFlightHeight(float newFlightHeight) { - // TODO: implement setting flight height -} - -void FlightController::newFlightHeightStickPosition(int16_t newFlightHeightStickPostition) { - // TODO: implement stick input -} - -void FlightController::newYawStickPosition(int16_t newYawStickPostition) { - // TODO: implement stick input -} - -void FlightController::newPitchStickPosition(int16_t newYawStickPostition) { - // TODO: implement stick input -} - -void FlightController::newRotor1Duty(uint32_t rotor1Duty) { - if (_status == DeviceStatus::Idle) { - _pid1->setRotorDuty(rotor1Duty); - } -} - -void FlightController::newRotor2Duty(uint32_t rotor2Duty) { - if (_status == DeviceStatus::Idle) { - _pid2->setRotorDuty(rotor2Duty); - } -} - -void FlightController::newRotor3Duty(uint32_t rotor3Duty) { - if (_status == DeviceStatus::Idle) { - _pid3->setRotorDuty(rotor3Duty); - } -} - -void FlightController::stopAllRotors() { - _pid1->setRotorDuty(0); - _pid2->setRotorDuty(0); - _pid3->setRotorDuty(0); -} - -void FlightController::setPid1Params(float p, float i, float d) { - if (_status == DeviceStatus::Idle) { - _pid1->Kp = p; - _pid1->Ki = i; - _pid1->Kd = d; - _pid1->save(); - } -} - -void FlightController::setPid2Params(float p, float i, float d) { - if (_status == DeviceStatus::Idle) { - _pid2->Kp = p; - _pid2->Ki = i; - _pid2->Kd = d; - _pid1->save(); - } -} - -void FlightController::setPid3Params(float p, float i, float d) { - if (_status == DeviceStatus::Idle) { - _pid3->Kp = p; - _pid3->Ki = i; - _pid3->Kd = d; - _pid1->save(); - } -} - -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; -} \ No newline at end of file diff --git a/Logic/FlightController.hpp b/Logic/FlightController.hpp deleted file mode 100644 index 85f80b8..0000000 --- a/Logic/FlightController.hpp +++ /dev/null @@ -1,74 +0,0 @@ -#include "PID.hpp" -#include -#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] -}; - -struct PidSettings { - float p; - float i; - float d; -}; - -class FlightController { - public: - FlightController(Sensors *sensors, PIDController *pid1, PIDController *pid2, - PIDController *pid3, bool unlimitedBattery = false); - ~FlightController(); - 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 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); - PidSettings *pidSettings() const; - private: - void _updateBatteryCharge(); - [[deprecated]] void _takeoff(); - [[deprecated]] 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 -}; \ No newline at end of file diff --git a/Logic/FlightDispatcher.cpp b/Logic/FlightDispatcher.cpp deleted file mode 100644 index e5abe33..0000000 --- a/Logic/FlightDispatcher.cpp +++ /dev/null @@ -1,180 +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 "FlightDispatcher.hpp" - -FlightDispatcher::FlightDispatcher(BluetoothDispatcher *bluetoothDispatcher, - FlightController *flightController, volatile 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) { - using sutil::SH; - ESP_LOGD(_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", - pkg.readError(), pkg.errorIndex(), package); - } else { - pkg.hashKeys(); - for (int keyIndex = 0; keyIndex < pkg.length(); ++keyIndex) { - auto pair = pkg[pkg.keyHash(keyIndex)]; - auto value = pair.value(); - ESP_LOGD(_tag, "Key: %zu", pkg.keyHash(keyIndex)); - - if (!pair.valid()) { - ESP_LOGW(_tag, "Pair isn't valid and was skipped"); - continue; - } - - switch (pkg.keyHash(keyIndex)) { - 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("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("p1"): - if (pair.type() == gson::Type::Object and pair.includes(SH("p")) - and pair.includes(SH("i")) and pair.includes(SH("d"))) { - _flightController->setPid1Params( - 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( - 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( - pair[SH("p")].toFloat(), pair[SH("i")].toFloat(), pair[SH("d")].toFloat()); - } - break; - case SH("pidSettingOpened"): _pidSettingsOpened(); break; - default: break; - } - } - } -} - -void FlightDispatcher::_changeStatus(const DeviceStatus &newStatus) { - switch (newStatus) { - case DeviceStatus::IsImuCalibration: _flightController->startImuCalibration(); break; - case DeviceStatus::IsFlying: - _changeStatus(DeviceStatus::IsPreparingForTakeoff); - break; - case DeviceStatus::IsPreparingForTakeoff: _flightController->startTakeoff(); break; - case DeviceStatus::IsBoarding: _flightController->startBoarding(); break; - default: break; - } -} - -void FlightDispatcher::_pidSettingsOpened() { - auto settings = _flightController->pidSettings(); - gson::string pkg; - pkg.beginObj(); - - pkg.beginObj("p1"); - pkg["p"] = settings[0].p; - pkg["i"] = settings[0].i; - pkg["d"] = settings[0].d; - pkg.endObj(); - - pkg.beginObj("p2"); - pkg["p"] = settings[1].p; - pkg["i"] = settings[1].i; - pkg["d"] = settings[1].d; - pkg.endObj(); - - pkg.beginObj("p3"); - pkg["p"] = settings[2].p; - pkg["i"] = settings[2].i; - pkg["d"] = settings[2].d; - pkg.endObj(); - - pkg.endObj(); - pkg.end(); - pkg += "\r\n"; - _bluetoothDispatcher->sendPackage(pkg.s.c_str(), strlen(pkg.s.c_str())); -} - -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()))); -} diff --git a/Logic/FlightDispatcher.hpp b/Logic/FlightDispatcher.hpp deleted file mode 100644 index b061278..0000000 --- a/Logic/FlightDispatcher.hpp +++ /dev/null @@ -1,35 +0,0 @@ -#include "FlightController.hpp" -#include "GSON.h" -#include "RF/BluetoothDispatcher.hpp" -#include - -class FlightDispatcher { - /* Deserialize state and update it in FlightController. */ - public: - FlightDispatcher(BluetoothDispatcher *bluetoothDispatcher, - FlightController *flightController, volatile bool loop_on_fail = true); - ~FlightDispatcher(); - void tick(); - private: - /* Telemetry flow */ - void _sendTelemetry(); - - /* Events handlers */ - void _onNewDeviceConnected(BTAddress device); - void _onNewMessageReceived(char *package); - - /* Requests handlers */ - void _changeStatus(const DeviceStatus &newStatus); - void _pidSettingsOpened(); - - /* Compile time settings */ - 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; - - std::map> _routes; -}; \ No newline at end of file diff --git a/Logic/PID.hpp b/Logic/PID.hpp deleted file mode 100644 index 08dd358..0000000 --- a/Logic/PID.hpp +++ /dev/null @@ -1,56 +0,0 @@ -#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); - } - } -}; \ No newline at end of file diff --git a/Motor/BrushedMotor.cpp b/Motor/BrushedMotor.cpp deleted file mode 100644 index 89f3556..0000000 --- a/Motor/BrushedMotor.cpp +++ /dev/null @@ -1,30 +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 "BrushedMotor.hpp" - -BrushedMotor::BrushedMotor(uint32_t PwmAGpioNum, uint32_t PwmBGpioNum, uint32_t PwmFreqHz, - uint32_t McpwmResolutionHz, int McpwmGroupId) { - _mcpwmResolutionHz = McpwmResolutionHz; - _pwmAGpioNum = McpwmGroupId; - _pwmBGpioNum = PwmBGpioNum; - _pwmFreqHz = PwmFreqHz; -} - -void BrushedMotor::enable() {} - -void BrushedMotor::disable() {} - -void BrushedMotor::setDuty(uint16_t duty) {} - -uint16_t BrushedMotor::maxDuty() { - return 0; -} - -void BrushedMotor::forward() {} - -void BrushedMotor::reverse() {} - -void BrushedMotor::coast() {} - -void BrushedMotor::brake() {} \ No newline at end of file diff --git a/Motor/BrushedMotor.hpp b/Motor/BrushedMotor.hpp deleted file mode 100644 index ecee0cd..0000000 --- a/Motor/BrushedMotor.hpp +++ /dev/null @@ -1,29 +0,0 @@ -#include - -// TODO: implement class - -class BrushedMotor { - /* Driver for native MCPWM controller. */ - // TODO: define default values - 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: - /* MCPWM group */ - int _mcpwmGroupId = 0; - uint32_t _mcpwmResolutionHz; - - /* Brushed motor properties */ - uint32_t _pwmAGpioNum; - uint32_t _pwmBGpioNum; - uint32_t _pwmFreqHz; -}; \ No newline at end of file diff --git a/RF/BluetoothDispatcher.cpp b/RF/BluetoothDispatcher.cpp deleted file mode 100644 index 946b1da..0000000 --- a/RF/BluetoothDispatcher.cpp +++ /dev/null @@ -1,105 +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 "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) { - assert(controller != nullptr); - assert(device_name != nullptr); - - _device_name = device_name; - _controller = controller; -} - -bool BluetoothDispatcher::initialize(volatile bool loop_on_fail, int readTimeoutMS) { - _controller->enableSSP(); - _controller->onConfirmRequest([this](uint16_t pin) { - _onConfirmRequest(pin); - }); - _controller->onAuthComplete([this](boolean success) { - _onAuthComplete(success); - }); - _controller->register_callback(deviceConnectedStaticCallback); - deviceConnectedCallback = [this](BTAddress device) { - _onDeviceConnected(device); - }; - _controller->setTimeout(2); - bool success = _controller->begin(_device_name, false); - if (!success) { - ESP_LOGI(_tag, "Failed to initialize Bluetooth controller!"); - while (loop_on_fail) - ; - return false; - } else { - ESP_LOGI(_tag, "Bluetooth host initialized"); - return true; - } -} - -void BluetoothDispatcher::onNewPackageReceived(NewPackageCallback newPackageReceivedCb) { - _newPackageReceivedCb = newPackageReceivedCb; -} - -void BluetoothDispatcher::tick() { - /* Call the callback, if new package received */ - while (_controller->available() and _buffer.length() <= _buffer_size) { - _buffer += (char)_controller->read(); - } - if (_buffer.endsWith("\n\r")) { - char buffer[_buffer_size]; - _buffer.substring(0, _buffer.lastIndexOf('}'), buffer); - ESP_LOGD(_tag, "Received new buffer %s", buffer); - if (_newPackageReceivedCb) { - _newPackageReceivedCb(buffer); - } - _buffer.clear(); - } - if (_buffer.length() > _available_buffer_size) { - _buffer.clear(); - } -} - -BluetoothDispatcher::~BluetoothDispatcher() { - _controller->end(); - delete _controller; -} - -void BluetoothDispatcher::_onConfirmRequest(uint16_t pin) { - ESP_LOGI(_tag, "The Bluetooth PIN is: %06lu", (long unsigned int)pin); - _controller->confirmReply(true); -} - -void BluetoothDispatcher::_onAuthComplete(boolean success) const { - if (success) { - ESP_LOGI(_tag, "Pairing success!"); - } else { - ESP_LOGI(_tag, "Pairing failed, rejected by user!"); - } -} - -void BluetoothDispatcher::_onDeviceConnected(BTAddress device) const { - ESP_LOGI(_tag, "New device connected: %s", device.toString(true).c_str()); - if (_deviceConnectedCallback) { - _deviceConnectedCallback(device); - } -} - -void BluetoothDispatcher::onNewDeviceConnected(DeviceConnectedCb deviceConnectedCb) { - /* Callback called if device connected successfully. */ - _deviceConnectedCallback = deviceConnectedCb; -} - -void BluetoothDispatcher::sendPackage(const char *package, size_t size) { - _controller->write((uint8_t *)package, size); -} - - -static void deviceConnectedStaticCallback(esp_spp_cb_event_t event, esp_spp_cb_param_t *param) { - if (event == ESP_SPP_SRV_OPEN_EVT and param->srv_open.status == ESP_SPP_SUCCESS - and deviceConnectedCallback) { - deviceConnectedCallback(BTAddress(param->srv_open.rem_bda)); - } -} \ No newline at end of file diff --git a/RF/BluetoothDispatcher.hpp b/RF/BluetoothDispatcher.hpp deleted file mode 100644 index c55efc6..0000000 --- a/RF/BluetoothDispatcher.hpp +++ /dev/null @@ -1,49 +0,0 @@ -#include "BluetoothSerial.h" -#include "HardwareSerial.h" -#include "esp_log.h" -#include "mString.h" - -/* Check the ESP configuration */ -#if not defined(CONFIG_BT_ENABLED) || not defined(CONFIG_BLUEDROID_ENABLED) -#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it -#endif - - -#if not defined(CONFIG_BT_SPP_ENABLED) -#error Serial Port Profile for Bluetooth is not available or not enabled. It is only available for the ESP32 chip. -#endif - - -#if not defined(CONFIG_BT_SSP_ENABLED) -#error Simple Secure Pairing for Bluetooth is not available or not enabled. -#endif -/* ************************* */ - -typedef std::function DeviceConnectedCb; -typedef std::function NewPackageCallback; - -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 onNewPackageReceived(NewPackageCallback newPackageReceivedCb); - void onNewDeviceConnected(DeviceConnectedCb deviceConnectedCb); - void sendPackage(const char *package, size_t size); - ~BluetoothDispatcher(); - - private: - void _onConfirmRequest(uint16_t pin); - void _onAuthComplete(boolean success) const; - void _onDeviceConnected(BTAddress device) const; - - const char *_device_name = nullptr; - BluetoothSerial *_controller = nullptr; - DeviceConnectedCb _deviceConnectedCallback = nullptr; - NewPackageCallback _newPackageReceivedCb = nullptr; - constexpr static const char *_tag = "BluetoothDispatcher"; - - static constexpr uint16_t _buffer_size = 522; - static constexpr uint16_t _available_buffer_size = 512; - mString<_buffer_size> _buffer; -}; \ No newline at end of file diff --git a/Sensors/Barometer.hpp b/Sensors/Barometer.hpp deleted file mode 100644 index e92fb83..0000000 --- a/Sensors/Barometer.hpp +++ /dev/null @@ -1,72 +0,0 @@ -#include "GyverBME280.h" - -typedef std::function OnMeasuringFinishedCb; - -class Barometer { - public: - Barometer(GyverBME280 *bme) { - _bme = bme; - }; - - ~Barometer() { - delete this->_bme; - } - - bool initialize(void) { - _bme->setMode(NORMAL_MODE); - _bme->setFilter(FILTER_COEF_16); - _bme->setTempOversampling(OVERSAMPLING_2); - _bme->setPressOversampling(OVERSAMPLING_16); - _bme->setStandbyTime(STANDBY_500US); - return _bme->begin(); - } - - void measureBaseAltitudeSync(void) { - for (int i = 0; i < _calibrationIterationsCount; ++i) { - _startedAltitude += altitude(); - delay(_calibrationIterationDelay); - } - _startedAltitude /= _calibrationIterationsCount; - } - - void measureBaseAltitudeAsync(void) { - _isCalibration = true; - } - - float altitude() /* [cm] */ { - return pressureToAltitude(_bme->readPressure()) * 100; - } - - float flightHeight() /* [cm] */ { - return altitude() - _startedAltitude; - } - - void onMeasuaringHeightFinished(OnMeasuringFinishedCb callback) { - _measuaringHeightFinishedCb = callback; - } - - void tick() { - if (_isCalibration) { - if (millis() - _calibrationTimer >= _calibrationIterationDelay) { - _startedAltitude += altitude(); - } - if (++_calibrationIterationsCounter >= _calibrationIterationsCount) { - _startedAltitude /= _calibrationIterationsCount; - _isCalibration = false; - if (_measuaringHeightFinishedCb) { - _measuaringHeightFinishedCb(); - } - } - } - } - - private: - GyverBME280 *_bme; - float _startedAltitude = 0; - bool _isCalibration = false; - uint32_t _calibrationTimer = 0; - int _calibrationIterationsCounter = 0; - OnMeasuringFinishedCb _measuaringHeightFinishedCb = nullptr; - static constexpr int _calibrationIterationsCount = 1500; - static constexpr int _calibrationIterationDelay = 1; // [ms] -}; \ No newline at end of file diff --git a/Sensors/BatteryController.hpp b/Sensors/BatteryController.hpp deleted file mode 100644 index a116530..0000000 --- a/Sensors/BatteryController.hpp +++ /dev/null @@ -1,19 +0,0 @@ -#include "board_pins.h" - -class BatteryController { - public: - BatteryController() {} - - void initialize() { - pinMode(BATTERY_DATA_SWITCH_PIN, OUTPUT); - digitalWrite(BATTERY_DATA_SWITCH_PIN, HIGH); - } - - float measureVoltage() { - return analogRead(BATTERY_DATA_PIN) * 3.3 / 4095; - } - - int percent(int minVoltage = 7200, int maxVoltage = 8400) { - return map(int(measureVoltage() * 1000), minVoltage, maxVoltage, 0, 100); - } -}; \ No newline at end of file diff --git a/Sensors/MPU.hpp b/Sensors/MPU.hpp deleted file mode 100644 index fd89e81..0000000 --- a/Sensors/MPU.hpp +++ /dev/null @@ -1,250 +0,0 @@ -#include "MPU6050_6Axis_MotionApps20.h" -#include "Preferences.h" -#include "board_pins.h" -#include "esp_log.h" - -static volatile bool _isDMPDataReady = false; -static void IRAM_ATTR _dmpInterruption() { - _isDMPDataReady = true; -} - -typedef std::function OnCalibrationFinishedCb; - -class MPU { - public: - MPU(MPU6050_6Axis_MotionApps20 *mpu) { - assert(mpu != nullptr); - _mpu = mpu; - } - - ~MPU() { - delete _mpu; - } - - bool initialize() { - _mpu->initialize(); - - if (!_preferences.begin("imu")) { - ESP_LOGE(_TAG, "Failed to open the preferences!"); - } else { - _loadOffsets(); - } - - delay(250); - if (!_mpu->testConnection()) { - ESP_LOGE(_TAG, "MPU6050 test connection failed!"); - return false; - } - _mpu->setDLPFMode(MPU6050_DLPF_BW_10); // 10 Hz bandwidth - //mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_500); // set sensivity, not recomended - //mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_8); - - if (_mpu->dmpInitialize()) { - ESP_LOGE(_TAG, "Failed to initialize DMP!"); - return false; - } - _mpu->setDMPEnabled(true); - attachInterrupt(MPU6050_INT_PIN, _dmpInterruption, RISING); - return true; - } - - bool tick(bool &err) { - err = false; - if (!_isDMPDataReady) { - return false; - } - if (!_mpu->dmpGetCurrentFIFOPacket(_fifoBuffer)) { - ESP_LOGE(_TAG, "Failed to get DMP data!"); - err = true; - return false; - } - - Quaternion q; - VectorFloat gravity; - VectorInt16 accel; - VectorInt16 gyro; - VectorInt16 accelReal; - - _mpu->dmpGetQuaternion(&q, _fifoBuffer); - _mpu->dmpGetGravity(&gravity, &q); - _mpu->dmpGetYawPitchRoll(_ypr, &q, &gravity); - _mpu->dmpGetAccel(&accel, _fifoBuffer); - _mpu->dmpGetGyro(&gyro, _fifoBuffer); - _mpu->dmpGetLinearAccel(&accelReal, &accel, &gravity); - _isDMPDataReady = false; - - _ax = accel.x / 8192.f; - _ay = accel.y / 8192.f; - _az = accel.z / 8192.f; - - _gx = gyro.x / 131.f; - _gy = gyro.y / 131.f; - _gz = gyro.z / 131.f; - - _calculateZInertial(gravity); - - if (_isCalibration) { - if (_calibrationsIterCounter >= _calibrationIterCount) { - _axOffset /= _calibrationsIterCounter; - _ayOffset /= _calibrationsIterCounter; - _azOffset /= _calibrationsIterCounter; - - _gxOffset /= _calibrationsIterCounter; - _gyOffset /= _calibrationsIterCounter; - _gzOffset /= _calibrationsIterCounter; - - _prOffset[0] /= _calibrationsIterCounter; - _prOffset[1] /= _calibrationsIterCounter; - - _isCalibration = false; - _updateOffsets(); - if (_calibrationFinishedCb) { - _calibrationFinishedCb(); - } - } else { - _axOffset += _ax; - _ayOffset += _ay; - _azOffset += _az; - _gxOffset += _gx; - _gyOffset += _gy; - _gzOffset += _gz; - _prOffset[0] += _ypr[1]; - _prOffset[1] += _ypr[2]; - } - } - - return true; - } - - /* getters */ - float accZInertial() const { - return _AccZInertial; - } - float yaw() const { - return degrees(_ypr[0]); - } - float pitch() const { - if (_isCalibration) { - return degrees(_ypr[1]); - } else { - return degrees(_ypr[1] - _prOffset[0]); - } - } - float roll() const { - if (_isCalibration) { - return degrees(_ypr[2]); - } else { - return degrees(_ypr[2] - _prOffset[1]); - } - } - - float ax() const { - if (_isCalibration) { - return _ax; - } else { - return _ax - _axOffset; - } - } - float ay() const { - if (_isCalibration) { - return _ay; - } else { - return _ay - _ayOffset; - } - } - float az() const { - if (_isCalibration) { - return _az; - } else { - return _az - _azOffset; - } - } - - float gravityZ() const { - return _gravity; - } - - float gx() const { - if (_isCalibration) { - return _gx; - } else { - return _gx - _gxOffset; - } - } - float gy() const { - if (_isCalibration) { - return _gy; - } else { - return _gy - _gyOffset; - } - } - float gz() const { - if (_isCalibration) { - return _gz; - } else { - return _gz - _gzOffset; - } - } - - void startCalibration() { - _isCalibration = true; - } - - void onCalibrationFinished(OnCalibrationFinishedCb callback) { - _calibrationFinishedCb = callback; - } - - private: - MPU6050_6Axis_MotionApps20 *_mpu = nullptr; - 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] = { 0 }; // yaw isn't used - - uint8_t _fifoBuffer[45] = { 0 }; - Preferences _preferences; - - const char *_TAG = "MPU6050 module"; - static constexpr const int _calibrationIterCount = 250; - - bool _isCalibration = false; - int _calibrationsIterCounter = 0; - OnCalibrationFinishedCb _calibrationFinishedCb = nullptr; - - void _calculateZInertial(VectorFloat &gravity) { - const float anglePitch = _ypr[1]; - const float angleRoll = _ypr[2]; - _gravity = gravity.z; - - _AccZInertial = -sin(anglePitch) * _ax + cos(anglePitch) * sin(angleRoll) * _ay - + cos(anglePitch) * cos(angleRoll) * _az; - _AccZInertial = (_AccZInertial - 1) * gravity.z * 100.f; - } - - void _loadOffsets() { - _axOffset = _preferences.getFloat("ax", 0.f); - _ayOffset = _preferences.getFloat("ax", 0.f); - _azOffset = _preferences.getFloat("az", 0.f); - _gxOffset = _preferences.getFloat("gx", 0.f); - _gyOffset = _preferences.getFloat("gy", 0.f); - _gzOffset = _preferences.getFloat("gz", 0.f); - _ypr[1] = _preferences.getFloat("pitch", 0.f); - _ypr[2] = _preferences.getFloat("roll", 0.f); - } - - void _updateOffsets() { - _preferences.putFloat("ax", _axOffset); - _preferences.putFloat("ay", _ayOffset); - _preferences.putFloat("az", _azOffset); - _preferences.putFloat("gx", _gxOffset); - _preferences.putFloat("gy", _gyOffset); - _preferences.putFloat("gz", _gzOffset); - _preferences.putFloat("pitch", _prOffset[0]); - _preferences.putFloat("roll", _prOffset[1]); - } -}; diff --git a/Sensors/Sensors.cpp b/Sensors/Sensors.cpp deleted file mode 100644 index b1eb7ae..0000000 --- a/Sensors/Sensors.cpp +++ /dev/null @@ -1,119 +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 "Sensors.hpp" - -Sensors::Sensors(Barometer *barometer, MPU *mpu, Kalman2DFilter *filter, - BatteryController *battery, volatile bool loop_on_fail) { - assert(barometer != nullptr); - assert(mpu != nullptr); - assert(filter != nullptr); - assert(battery != nullptr); - - _barometer = barometer; - _mpu = mpu; - _2d_filter = filter; - _battery = battery; - - ESP_LOGI(_tag, "Initialize BMP280..."); - if (_barometer->initialize()) { - ESP_LOGI(_tag, "BMP280 initialized"); - } else { - ESP_LOGI(_tag, "BMP280 initialization failed!"); - while (loop_on_fail) - ; - } - - ESP_LOGI(_tag, "Initialize MPU6050..."); - if (_mpu->initialize()) { - ESP_LOGI(_tag, "MPU6050 initialized"); - } else { - ESP_LOGI(_tag, "MPU6050 initialization failed!"); - while (loop_on_fail) - ; - } - - _battery->initialize(); - ESP_LOGI(_tag, "BatteryController initialized"); - ESP_LOGI(_tag, "Sensors initialized"); -} - -Sensors::~Sensors() { - delete _barometer; - delete _mpu; - delete _2d_filter; - delete _battery; -} - -void Sensors::measureBaseAltitudeSync(void) { - _barometer->measureBaseAltitudeSync(); -} - -void Sensors::measureBaseAltitudeAsync(void) { - _barometer->measureBaseAltitudeAsync(); -} - -float Sensors::rawFlightHeight(void) const { - /* Returns flight height from barometer immediatly */ - return _barometer->flightHeight(); -} - -float Sensors::flightHeight(void) const { - /* Returns flight height from cache immediatly */ - if (_flightHeightFromBarometer == 0.f) { - ESP_LOGW(_tag, "flightHeight called, but tick() has called never"); - return rawFlightHeight(); - } else { - return _flightHeightFromBarometer; - } -} - -MpuData Sensors::mpuData() const { - if (_mpuData.ax == 0.f and _mpuData.gravity == 0.f) { - ESP_LOGW(_tag, "MPU data looks like .tick() has never called"); - } - return _mpuData; -} - -bool Sensors::tick(void) { - /* Super loop iteraion */ - /* Return true on update */ - _barometer->tick(); - bool err; - bool isMpuDataReady = _mpu->tick(err); - if (isMpuDataReady and !err) { - _2d_filter->filter(_mpu->accZInertial(), _barometer->flightHeight(), - _flightHeightFromBarometer, _zVelocityAltitude); - _mpuData = { .ax = _mpu->ax(), - .ay = _mpu->ay(), - .az = _mpu->az(), - .gx = _mpu->gx(), - .gy = _mpu->gy(), - .gz = _mpu->gz(), - .yaw = _mpu->yaw(), - .pitch = _mpu->pitch(), - .roll = _mpu->roll(), - .gravity = _mpu->gravityZ(), - .zInertial = _mpu->accZInertial() }; - return true; - } else if (err) { - ESP_LOGE(_tag, "Failed to get DMP data!"); - } - return false; -} - -int Sensors::batteryCharge(void) const { - return _battery->percent(); -} - -void Sensors::onMpuCalibrationFinished(OnCalibrationFinishedCb callback) { - _mpu->onCalibrationFinished(callback); -} - -void Sensors::startMpuCalibration() { - _mpu->startCalibration(); -} - -void Sensors::onMeasuaringAltitudeFinished(OnMeasuringFinishedCb callback) { - _barometer->onMeasuaringHeightFinished(callback); -} \ No newline at end of file diff --git a/Sensors/Sensors.hpp b/Sensors/Sensors.hpp deleted file mode 100644 index b548787..0000000 --- a/Sensors/Sensors.hpp +++ /dev/null @@ -1,52 +0,0 @@ -#include "Barometer.hpp" -#include "BatteryController.hpp" -#include "Filters/Kalman2DFilter.hpp" -#include "MPU.hpp" -#include "esp_log.h" - -/* Sensors module provides cached and filtered sensors values */ - -struct MpuData { - float ax, ay, az; - float gx, gy, gz; - float yaw, pitch, roll; - float gravity; - float zInertial; -}; - -class Sensors { - public: - Sensors(Barometer *barometer, MPU *mpu, Kalman2DFilter *filter, - BatteryController *battery, bool loop_on_fail = true); - - ~Sensors(); - - void measureBaseAltitudeSync(void); - void measureBaseAltitudeAsync(void); - void onMeasuaringAltitudeFinished(OnMeasuringFinishedCb callback); - float rawFlightHeight(void) const; - float flightHeight(void) const; - - void startMpuCalibration(); - void onMpuCalibrationFinished(OnCalibrationFinishedCb callback); - - MpuData mpuData(void) const; - - int batteryCharge(void) const; // [%] - - bool tick(void); - - private: - Barometer *_barometer = nullptr; - MPU *_mpu = nullptr; - BatteryController *_battery = nullptr; - Kalman2DFilter *_2d_filter = nullptr; - - /* cached filtered values */ - float _flightHeightFromBarometer = 0.f; - float _zVelocityAltitude = 0.f; - MpuData _mpuData = { 0.f }; - - - static constexpr const char *_tag = "Sensors"; -}; \ No newline at end of file diff --git a/board_pins.h b/board_pins.h deleted file mode 100644 index 20ee6f7..0000000 --- a/board_pins.h +++ /dev/null @@ -1,8 +0,0 @@ -#include "driver/adc.h" - -#define I2C_SDA_PIN 21 -#define I2C_SCL_PIN 18 -#define MPU6050_INT_PIN 19 -#define BATTERY_ADC_CHANNEL ADC2_CHANNEL_0 -#define BATTERY_DATA_PIN 4 -#define BATTERY_DATA_SWITCH_PIN 17 \ No newline at end of file diff --git a/main.cpp b/main.cpp deleted file mode 100644 index 1d9334f..0000000 --- a/main.cpp +++ /dev/null @@ -1,27 +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 "App.hpp" -#include "BoardI2C.hpp" - -#define IGNORE_CHARGE true - -BoardI2C i2c; - -static Application *app = nullptr; - -void setup() { - Serial.begin(115200); - 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 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(); -} \ No newline at end of file