diff --git a/src/.clang-format b/src/.clang-format new file mode 100644 index 0000000..9f3311b --- /dev/null +++ b/src/.clang-format @@ -0,0 +1,160 @@ +# 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/src/App.hpp b/src/App.hpp new file mode 100644 index 0000000..c597f7d --- /dev/null +++ b/src/App.hpp @@ -0,0 +1,23 @@ +#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/src/BoardI2C.hpp b/src/BoardI2C.hpp new file mode 100644 index 0000000..f9f3a93 --- /dev/null +++ b/src/BoardI2C.hpp @@ -0,0 +1,16 @@ +#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/src/Filters/Kalman2DFilter.hpp b/src/Filters/Kalman2DFilter.hpp new file mode 100644 index 0000000..e3e8ee2 --- /dev/null +++ b/src/Filters/Kalman2DFilter.hpp @@ -0,0 +1,43 @@ +#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/src/Filters/kalman.hpp b/src/Filters/kalman.hpp new file mode 100644 index 0000000..373fe88 --- /dev/null +++ b/src/Filters/kalman.hpp @@ -0,0 +1,46 @@ +// упрощённый Калман для одномерного случая + +#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/src/Filters/median3.hpp b/src/Filters/median3.hpp new file mode 100644 index 0000000..5c59c6b --- /dev/null +++ b/src/Filters/median3.hpp @@ -0,0 +1,19 @@ +// быстрый медианный фильтр 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/src/Filters/runningAverage.hpp b/src/Filters/runningAverage.hpp new file mode 100644 index 0000000..fd338a6 --- /dev/null +++ b/src/Filters/runningAverage.hpp @@ -0,0 +1,49 @@ +// экспоненциальное бегущее среднее + +#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/src/Logic/FlightController.cpp b/src/Logic/FlightController.cpp new file mode 100644 index 0000000..8252e4b --- /dev/null +++ b/src/Logic/FlightController.cpp @@ -0,0 +1,194 @@ +// 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/src/Logic/FlightController.hpp b/src/Logic/FlightController.hpp new file mode 100644 index 0000000..85f80b8 --- /dev/null +++ b/src/Logic/FlightController.hpp @@ -0,0 +1,74 @@ +#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/src/Logic/FlightDispatcher.cpp b/src/Logic/FlightDispatcher.cpp new file mode 100644 index 0000000..e5abe33 --- /dev/null +++ b/src/Logic/FlightDispatcher.cpp @@ -0,0 +1,180 @@ +// 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/src/Logic/FlightDispatcher.hpp b/src/Logic/FlightDispatcher.hpp new file mode 100644 index 0000000..b061278 --- /dev/null +++ b/src/Logic/FlightDispatcher.hpp @@ -0,0 +1,35 @@ +#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/src/Logic/PID.hpp b/src/Logic/PID.hpp new file mode 100644 index 0000000..08dd358 --- /dev/null +++ b/src/Logic/PID.hpp @@ -0,0 +1,56 @@ +#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/src/Motor/BrushedMotor.cpp b/src/Motor/BrushedMotor.cpp new file mode 100644 index 0000000..89f3556 --- /dev/null +++ b/src/Motor/BrushedMotor.cpp @@ -0,0 +1,30 @@ +// 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/src/Motor/BrushedMotor.hpp b/src/Motor/BrushedMotor.hpp new file mode 100644 index 0000000..ecee0cd --- /dev/null +++ b/src/Motor/BrushedMotor.hpp @@ -0,0 +1,29 @@ +#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/src/RF/BluetoothDispatcher.cpp b/src/RF/BluetoothDispatcher.cpp new file mode 100644 index 0000000..946b1da --- /dev/null +++ b/src/RF/BluetoothDispatcher.cpp @@ -0,0 +1,105 @@ +// 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/src/RF/BluetoothDispatcher.hpp b/src/RF/BluetoothDispatcher.hpp new file mode 100644 index 0000000..c55efc6 --- /dev/null +++ b/src/RF/BluetoothDispatcher.hpp @@ -0,0 +1,49 @@ +#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/src/Sensors/Barometer.hpp b/src/Sensors/Barometer.hpp new file mode 100644 index 0000000..e92fb83 --- /dev/null +++ b/src/Sensors/Barometer.hpp @@ -0,0 +1,72 @@ +#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/src/Sensors/BatteryController.hpp b/src/Sensors/BatteryController.hpp new file mode 100644 index 0000000..a116530 --- /dev/null +++ b/src/Sensors/BatteryController.hpp @@ -0,0 +1,19 @@ +#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/src/Sensors/MPU.hpp b/src/Sensors/MPU.hpp new file mode 100644 index 0000000..fd89e81 --- /dev/null +++ b/src/Sensors/MPU.hpp @@ -0,0 +1,250 @@ +#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/src/Sensors/Sensors.cpp b/src/Sensors/Sensors.cpp new file mode 100644 index 0000000..b1eb7ae --- /dev/null +++ b/src/Sensors/Sensors.cpp @@ -0,0 +1,119 @@ +// 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/src/Sensors/Sensors.hpp b/src/Sensors/Sensors.hpp new file mode 100644 index 0000000..b548787 --- /dev/null +++ b/src/Sensors/Sensors.hpp @@ -0,0 +1,52 @@ +#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/src/board_pins.h b/src/board_pins.h new file mode 100644 index 0000000..20ee6f7 --- /dev/null +++ b/src/board_pins.h @@ -0,0 +1,8 @@ +#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/src/main.cpp b/src/main.cpp new file mode 100644 index 0000000..1d9334f --- /dev/null +++ b/src/main.cpp @@ -0,0 +1,27 @@ +// 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