diff --git a/.idea/.gitignore b/.idea/.gitignore
new file mode 100644
index 0000000..13566b8
--- /dev/null
+++ b/.idea/.gitignore
@@ -0,0 +1,8 @@
+# Default ignored files
+/shelf/
+/workspace.xml
+# Editor-based HTTP Client requests
+/httpRequests/
+# Datasource local storage ignored files
+/dataSources/
+/dataSources.local.xml
diff --git a/.idea/codeStyles/codeStyleConfig.xml b/.idea/codeStyles/codeStyleConfig.xml
new file mode 100644
index 0000000..79ee123
--- /dev/null
+++ b/.idea/codeStyles/codeStyleConfig.xml
@@ -0,0 +1,5 @@
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/misc.xml b/.idea/misc.xml
new file mode 100644
index 0000000..d858eb1
--- /dev/null
+++ b/.idea/misc.xml
@@ -0,0 +1,17 @@
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/vcs.xml b/.idea/vcs.xml
new file mode 100644
index 0000000..35eb1dd
--- /dev/null
+++ b/.idea/vcs.xml
@@ -0,0 +1,6 @@
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/.clang-format b/src/.clang-format
index 9f3311b..2dc47ad 100644
--- a/src/.clang-format
+++ b/src/.clang-format
@@ -92,7 +92,6 @@ IncludeIsMainRegex: ''
IncludeIsMainSourceRegex: ''
IndentGotoLabels: false
IndentPPDirectives: None
-IndentRequires: true
IndentWidth: 2
IndentWrappedFunctionNames: false
InsertTrailingCommas: None
@@ -148,7 +147,7 @@ StatementAttributeLikeMacros:
StatementMacros:
- Q_UNUSED
- QT_REQUIRE_VERSION
-TabWidth: 2
+#TabWidth: 2
UseCRLF: false
UseTab: Never
WhitespaceSensitiveMacros:
diff --git a/src/BoardI2C.hpp b/src/BoardI2C.hpp
index ab7d4d9..a2a7b68 100644
--- a/src/BoardI2C.hpp
+++ b/src/BoardI2C.hpp
@@ -1,6 +1,7 @@
// 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
+#define CONFIG_DISABLE_HAL_LOCKS 0
#include "Wire.h"
#include "board_pins.h"
#include "esp_log.h"
@@ -9,6 +10,7 @@ class BoardI2C : public TwoWire {
public:
BoardI2C(volatile bool loop_on_fail = true) : TwoWire(0) {
if (begin(I2C_SDA_PIN, I2C_SCL_PIN, 100000)) {
+ setTimeOut(50);
ESP_LOGI("I2CBus", "Bus initialized");
} else {
ESP_LOGE("I2CBus", "Failed to initialize the i2c software bus!");
diff --git a/src/Logic/FlightController.cpp b/src/Logic/FlightController.cpp
index 41eb672..f81c533 100644
--- a/src/Logic/FlightController.cpp
+++ b/src/Logic/FlightController.cpp
@@ -1,63 +1,74 @@
#include "FlightController.hpp"
-FlightController::FlightController(Sensors *sensors, PIDController *pid1, PIDController *pid2,
- PIDController *pid3, bool unlimitedBattery) {
+FlightController::FlightController(
+ Sensors *sensors, SavedPidRegulator *heightController, SavedPidRegulator *yawController,
+ BrushedMotor *rotor1, BrushedMotor *rotor2,
+ PIDController *tailRotorController, bool unlimitedBattery) {
assert(sensors != nullptr);
- assert(pid1 != nullptr);
- assert(pid2 != nullptr);
- assert(pid3 != nullptr);
+ assert(heightController != nullptr);
+ assert(yawController != nullptr);
+ assert(tailRotorController != nullptr);
+ assert(rotor1 != nullptr);
+ assert(rotor2 != nullptr);
_sensors = sensors;
_unlimitedBattery = unlimitedBattery;
- _pid1 = pid1;
- _pid2 = pid2;
- _pid3 = pid3;
+ _heightController = heightController;
+ _yawController = yawController;
+ _tailRotorController = tailRotorController;
+ _rotor1 = rotor1;
+ _rotor2 = rotor2;
_status = DeviceStatus::Idle;
- _updateBatteryCharge();
+ _checkBatteryCharge();
+ stopAllRotors();
ESP_LOGI(_tag, "Flight controller initialized");
}
FlightController::~FlightController() {
delete _sensors;
- delete _pid1;
- delete _pid2;
- delete _pid3;
+ delete _heightController;
+ delete _yawController;
+ delete _tailRotorController;
+ delete _rotor1;
+ delete _rotor2;
}
void FlightController::tick() {
bool hasUpdates = _sensors->tick();
if (hasUpdates) {
- _updateBatteryCharge();
+ _checkBatteryCharge();
}
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;
+ 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) {
+ _tailRotorController->tick();
+ }
+ break;
}
}
ReportedDeviceState FlightController::currentDeviceState() const {
- return { .batteryCharge = _sensors->batteryCharge(),
- .flightHeight = _sensors->flightHeight(),
- .status = _status,
- .mpuState = _sensors->mpuData() };
+ return {.batteryCharge = _sensors->batteryCharge(),
+ .flightHeight = _sensors->flightHeight(),
+ .status = _status,
+ .mpuState = _sensors->mpuData()};
}
void FlightController::startTakeoff() {
/* Start preparation for takeoff */
- _updateBatteryCharge();
+ _checkBatteryCharge();
if (_status != DeviceStatus::Idle)
return;
@@ -77,7 +88,7 @@ void FlightController::startBoarding() {
}
}
-void FlightController::_updateBatteryCharge() {
+void FlightController::_checkBatteryCharge() {
if (_unlimitedBattery)
return;
if (_sensors->batteryCharge() <= _batteryCriticalCharge) {
@@ -92,16 +103,18 @@ void FlightController::_updateBatteryCharge() {
void FlightController::_takeoff() {
// TODO: implement takeoff
_status = DeviceStatus::IsFlying;
- if (_onTakeoffStarted) {
- _onTakeoffStarted();
- }
+ _tailRotorController->enable();
+ _rotor1->enable();
+ _rotor2->enable();
}
void FlightController::_boarding() {
if (_sensors->flightHeight() <= _defaultStopMotorHeight) {
// TODO: stop motors and setpoints
_status = DeviceStatus::Idle;
-
+ _rotor1->disable();
+ _rotor2->disable();
+ _tailRotorController->disable();
} else {
// TODO: implement boarding
}
@@ -134,63 +147,61 @@ void FlightController::newPitchStickPosition(int16_t newYawStickPostition) {
// TODO: implement stick input
}
-void FlightController::newRotor1Duty(uint32_t rotor1Duty) {
+void FlightController::setRotor1Duty(uint32_t rotor1Duty) {
if (_status == DeviceStatus::Idle) {
- _pid1->setRotorDuty(rotor1Duty);
+ _rotor1->setDuty(rotor1Duty);
}
}
-void FlightController::newRotor2Duty(uint32_t rotor2Duty) {
+void FlightController::setRotor2Duty(uint32_t rotor2Duty) {
if (_status == DeviceStatus::Idle) {
- _pid2->setRotorDuty(rotor2Duty);
+ _rotor2->setDuty(rotor2Duty);
}
}
-void FlightController::newRotor3Duty(uint32_t rotor3Duty) {
+void FlightController::setRotor3Duty(uint32_t rotor3Duty) {
if (_status == DeviceStatus::Idle) {
- _pid3->setRotorDuty(rotor3Duty);
+ _tailRotorController->setRotorDuty(rotor3Duty);
}
}
void FlightController::stopAllRotors() {
- _pid1->setRotorDuty(0);
- _pid2->setRotorDuty(0);
- _pid3->setRotorDuty(0);
+ setRotor1Duty(0);
+ setRotor2Duty(0);
+ setRotor3Duty(0);
}
-void FlightController::setPid1Params(float p, float i, float d) {
+void FlightController::setHeightControllerParams(float p, float i, float d) {
if (_status == DeviceStatus::Idle) {
- _pid1->Kp = p;
- _pid1->Ki = i;
- _pid1->Kd = d;
- _pid1->save();
+ _heightController->Kp = p;
+ _heightController->Ki = i;
+ _heightController->Kd = d;
+ _heightController->save();
}
}
-void FlightController::setPid2Params(float p, float i, float d) {
+void FlightController::setYawControllerParams(float p, float i, float d) {
if (_status == DeviceStatus::Idle) {
- _pid2->Kp = p;
- _pid2->Ki = i;
- _pid2->Kd = d;
- _pid1->save();
+ _yawController->Kp = p;
+ _yawController->Ki = i;
+ _yawController->Kd = d;
+ _heightController->save();
}
}
-void FlightController::setPid3Params(float p, float i, float d) {
+void FlightController::setPitchControllerParams(float p, float i, float d) {
if (_status == DeviceStatus::Idle) {
- _pid3->Kp = p;
- _pid3->Ki = i;
- _pid3->Kd = d;
- _pid1->save();
+ _tailRotorController->Kp = p;
+ _tailRotorController->Ki = i;
+ _tailRotorController->Kd = d;
+ _heightController->save();
}
}
-std::unique_ptr FlightController::pidSettings() const {
- std::unique_ptr settings = std::make_unique();
- settings->pid1 = { .p = _pid1->Kp, .i = _pid1->Ki, .d = _pid1->Kd };
- settings->pid2 = { .p = _pid2->Kp, .i = _pid2->Ki, .d = _pid2->Kd };
- settings->pid3 = { .p = _pid3->Kp, .i = _pid3->Ki, .d = _pid3->Kd };
- ESP_LOGI(_tag, "PID №1 settings: (%f, %f, %f)", settings->pid1.p, settings->pid1.i,
- settings->pid1.d);
- return std::move(settings);
+PidSettings FlightController::pidSettings() const {
+ PidSettings settings;
+ settings.flightController = {.p = _heightController->Kp, .i = _heightController->Ki, .d = _heightController->Kd};
+ settings.yawController = {.p = _yawController->Kp, .i = _yawController->Ki, .d = _yawController->Kd};
+ settings.pitchController = {.p = _tailRotorController->Kp, .i = _tailRotorController->Ki, .d = _tailRotorController->Kd};
+ return settings;
}
\ No newline at end of file
diff --git a/src/Logic/FlightController.hpp b/src/Logic/FlightController.hpp
index 3d915db..b3b5550 100644
--- a/src/Logic/FlightController.hpp
+++ b/src/Logic/FlightController.hpp
@@ -1,7 +1,7 @@
// This is a personal academic project. Dear PVS-Studio, please check it.
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
-#include "PID.hpp"
+#include "PIDController.hpp"
#include "Sensors/Sensors.hpp"
#include
@@ -27,51 +27,67 @@ struct FlightParams {
float pitch; // [degrees]
};
-struct PidParam {
- float p;
- float i;
- float d;
-};
-
struct PidSettings {
- PidParam pid1;
- PidParam pid2;
- PidParam pid3;
+ PidParams flightController;
+ PidParams yawController;
+ PidParams pitchController;
};
class FlightController {
- public:
- FlightController(Sensors *sensors, PIDController *pid1, PIDController *pid2,
- PIDController *pid3, bool unlimitedBattery = false);
+public:
+ FlightController(Sensors *sensors, SavedPidRegulator *heightController, SavedPidRegulator *yawController,
+ BrushedMotor *_rotor1, BrushedMotor *_rotor2,
+ PIDController *tailRotor, bool unlimitedBattery);
+
~FlightController();
- ReportedDeviceState currentDeviceState() const;
+
+ [[nodiscard]] 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 setRotor1Duty(uint32_t rotor1Duty);
+
+ void setRotor2Duty(uint32_t rotor2Duty);
+
+ void setRotor3Duty(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);
- std::unique_ptr pidSettings() const;
- private:
- void _updateBatteryCharge();
+
+ void setHeightControllerParams(float p, float i, float d);
+
+ void setYawControllerParams(float p, float i, float d);
+
+ void setPitchControllerParams(float p, float i, float d);
+
+ [[nodiscard]] PidSettings pidSettings() const;
+
+private:
+ void _checkBatteryCharge();
+
[[deprecated]] void _takeoff();
+
[[deprecated]] void _boarding();
Sensors *_sensors = nullptr;
- PIDController *_pid1 = nullptr;
- PIDController *_pid2 = nullptr;
- PIDController *_pid3 = nullptr;
+ SavedPidRegulator *_heightController = nullptr;
+ SavedPidRegulator *_yawController = nullptr;
+ PIDController *_tailRotorController = nullptr;
+ BrushedMotor *_rotor1 = nullptr;
+ BrushedMotor *_rotor2 = nullptr;
DeviceStatus _status;
- OnMeasuringFinishedCb _onTakeoffStarted = nullptr;
static constexpr const char *_tag = "FlightController";
static constexpr uint8_t _batteryCriticalCharge = 15; // [%]
diff --git a/src/Logic/FlightDispatcher.cpp b/src/Logic/FlightDispatcher.cpp
index fdf0b1c..1d97111 100644
--- a/src/Logic/FlightDispatcher.cpp
+++ b/src/Logic/FlightDispatcher.cpp
@@ -22,8 +22,7 @@ FlightDispatcher::FlightDispatcher(BluetoothDispatcher *bluetoothDispatcher,
ESP_LOGI(_tag, "Bluetooth initialized successfully.");
} else {
ESP_LOGE(_tag, "Failed to initialize Bluetooth dispatcher!");
- while (loop_on_fail)
- ;
+ while (loop_on_fail);
}
_telemetryTimer = millis();
}
@@ -66,44 +65,59 @@ void FlightDispatcher::_onNewMessageReceived(char *package) {
}
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;
+ 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->setRotor1Duty(value.toInt32());
+ break;
+ case SH("r2"):
+ _flightController->setRotor2Duty(value.toInt32());
+ break;
+ case SH("r3"):
+ _flightController->setRotor3Duty(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->setHeightControllerParams(
+ 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->setYawControllerParams(
+ 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->setPitchControllerParams(
+ pair[SH("p")].toFloat(), pair[SH("i")].toFloat(), pair[SH("d")].toFloat());
+ }
+ break;
+ case SH("pidSettingOpened"):
+ _pidSettingsOpened();
+ break;
+ default:
+ break;
}
}
}
@@ -111,13 +125,20 @@ void FlightDispatcher::_onNewMessageReceived(char *package) {
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;
+ case DeviceStatus::IsImuCalibration:
+ _flightController->startImuCalibration();
+ break;
+ case DeviceStatus::IsFlying:
+ _flightController->startTakeoff();
+ break;
+ case DeviceStatus::IsPreparingForTakeoff:
+ _flightController->startTakeoff();
+ break;
+ case DeviceStatus::IsBoarding:
+ _flightController->startBoarding();
+ break;
+ default:
+ break;
}
}
@@ -127,21 +148,21 @@ void FlightDispatcher::_pidSettingsOpened() {
pkg.beginObj();
pkg.beginObj("p1");
- pkg["p"] = settings->pid1.p;
- pkg["i"] = settings->pid1.i;
- pkg["d"] = settings->pid1.d;
+ pkg["p"] = settings.flightController.p;
+ pkg["i"] = settings.flightController.i;
+ pkg["d"] = settings.flightController.d;
pkg.endObj();
pkg.beginObj("p2");
- pkg["p"] = settings->pid2.p;
- pkg["i"] = settings->pid2.i;
- pkg["d"] = settings->pid2.d;
+ pkg["p"] = settings.yawController.p;
+ pkg["i"] = settings.yawController.i;
+ pkg["d"] = settings.yawController.d;
pkg.endObj();
pkg.beginObj("p3");
- pkg["p"] = settings->pid3.p;
- pkg["i"] = settings->pid3.i;
- pkg["d"] = settings->pid3.d;
+ pkg["p"] = settings.pitchController.p;
+ pkg["i"] = settings.pitchController.i;
+ pkg["d"] = settings.pitchController.d;
pkg.endObj();
pkg.endObj();
diff --git a/src/Logic/PID.hpp b/src/Logic/PID.hpp
deleted file mode 100644
index 9b62542..0000000
--- a/src/Logic/PID.hpp
+++ /dev/null
@@ -1,59 +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 "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/Logic/PIDController.hpp b/src/Logic/PIDController.hpp
new file mode 100644
index 0000000..74c736c
--- /dev/null
+++ b/src/Logic/PIDController.hpp
@@ -0,0 +1,46 @@
+// 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 "Motor/BrushedMotor.hpp"
+#include "SavedPidRegulator.hpp"
+#include "esp_log.h"
+
+
+class PIDController : public SavedPidRegulator {
+public:
+ PIDController(float p, float i, float d, BrushedMotor *rotor,
+ const char *name = "PID", uint16_t dt = 10)
+ : SavedPidRegulator(p, i, d, name, dt) {
+ assert(rotor != nullptr);
+ _rotor = rotor;
+ this->_dt = dt;
+ setLimits(0, _rotor->maxDuty());
+ _pid_timer = millis();
+ ESP_LOGI(_tag, "PID %s initialized with params (%f, %f, %f)", name, Kp, Ki, Kd);
+ }
+
+ void enable() {
+ _rotor->enable();
+ }
+
+ void disable() {
+ _rotor->disable();
+ }
+
+ void tick() {
+ if (millis() - _pid_timer >= _dt) {
+ _rotor->setDuty((uint32_t) getResultTimer());
+ _pid_timer = millis();
+ }
+ }
+
+ void setRotorDuty(uint32_t duty) {
+ /* Used only for test purposes */
+ _rotor->setDuty(duty);
+ }
+
+private:
+ uint16_t _dt;
+ BrushedMotor *_rotor = nullptr;
+ uint32_t _pid_timer = 0;
+};
\ No newline at end of file
diff --git a/src/Logic/SavedPidRegulator.hpp b/src/Logic/SavedPidRegulator.hpp
new file mode 100644
index 0000000..d175975
--- /dev/null
+++ b/src/Logic/SavedPidRegulator.hpp
@@ -0,0 +1,71 @@
+#include "GyverPID.h"
+#include "Preferences.h"
+#include "esp_log.h"
+#include
+
+struct PidParams {
+ float p;
+ float i;
+ float d;
+};
+
+class SavedPidRegulator : public GyverPID {
+public:
+ SavedPidRegulator(float p, float i, float d, const char *name = "saved_pid", uint16_t dt = 10)
+ : GyverPID(p, i, d, (int16_t) dt) {
+ _name = name;
+ _preferences.begin(name);
+ _pDefault = p;
+ _iDefault = i;
+ _dDefault = d;
+ _loadPreferences();
+ ESP_LOGI(_tag, "PID %s initialized with params (%f, %f, %f)", _name.c_str(), Kp, Ki, Kd);
+ }
+
+ void setParams(PidParams params) {
+ this->Kp = params.p;
+ this->Ki = params.i;
+ this->Kd = params.d;
+ }
+
+ std::shared_ptr getParams() {
+ std::shared_ptr params = std::make_unique();
+ params->p = Kp;
+ params->i = Ki;
+ params->d = Kd;
+ return params;
+ }
+
+ void resetToDefault() {
+ this->Kp = _pDefault;
+ this->Ki = _iDefault;
+ this->Kd = _dDefault;
+ save();
+ }
+
+ void save() {
+ _preferences.putFloat("p", Kp);
+ _preferences.putFloat("i", Ki);
+ _preferences.putFloat("d", Kd);
+ ESP_LOGI(_tag, "PID %s saved with params (%f, %f, %f)", _name.c_str(), Kp, Ki, Kd);
+ }
+
+protected:
+ static constexpr const char *_tag = "PIDController";
+
+private:
+ float _pDefault = 0.f;
+ float _iDefault = 0.f;
+ float _dDefault = 0.f;
+
+ String _name = "PID";
+ Preferences _preferences;
+
+ void _loadPreferences() {
+ 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.hpp b/src/Motor/BrushedMotor.hpp
index 20e9a7f..65ddd22 100644
--- a/src/Motor/BrushedMotor.hpp
+++ b/src/Motor/BrushedMotor.hpp
@@ -8,19 +8,27 @@
class BrushedMotor {
/* Driver for native MCPWM controller. */
// TODO: define default values
- public:
+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:
+protected:
/* MCPWM group */
int _mcpwmGroupId = 0;
uint32_t _mcpwmResolutionHz;
diff --git a/src/RF/BluetoothDispatcher.cpp b/src/RF/BluetoothDispatcher.cpp
index 98ebb37..6aaab97 100644
--- a/src/RF/BluetoothDispatcher.cpp
+++ b/src/RF/BluetoothDispatcher.cpp
@@ -4,6 +4,7 @@
#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) {
@@ -26,12 +27,11 @@ bool BluetoothDispatcher::initialize(volatile bool loop_on_fail, int readTimeout
deviceConnectedCallback = [this](BTAddress device) {
_onDeviceConnected(device);
};
- _controller->setTimeout(2);
+ _controller->setTimeout(50);
bool success = _controller->begin(_device_name, false);
if (!success) {
ESP_LOGI(_tag, "Failed to initialize Bluetooth controller!");
- while (loop_on_fail)
- ;
+ while (loop_on_fail);
return false;
} else {
ESP_LOGI(_tag, "Bluetooth host initialized");
@@ -43,12 +43,12 @@ void BluetoothDispatcher::onNewPackageReceived(NewPackageCallback newPackageRece
_newPackageReceivedCb = newPackageReceivedCb;
}
-void BluetoothDispatcher::tick(const char *message_delimeter) {
+void BluetoothDispatcher::tick(const char message_delimeter) {
/* Call the callback, if new package received */
- while (_controller->available()) { _buffer += (char)_controller->read(); }
- if (_buffer.endsWith(message_delimeter)) {
+ while (_controller->available()) { _buffer += (char) _controller->read(); }
+ if (_buffer.lastIndexOf(message_delimeter) != -1) {
char buffer[_buffer_size];
- auto messageEnd = _buffer.lastIndexOf('}');
+ auto messageEnd = _buffer.indexOf(message_delimeter) - 1;
if (messageEnd == -1) {
_buffer.clear();
} else {
@@ -71,7 +71,7 @@ BluetoothDispatcher::~BluetoothDispatcher() {
}
void BluetoothDispatcher::_onConfirmRequest(uint16_t pin) {
- ESP_LOGI(_tag, "The Bluetooth PIN is: %06lu", (long unsigned int)pin);
+ ESP_LOGI(_tag, "The Bluetooth PIN is: %06lu", (long unsigned int) pin);
_controller->confirmReply(true);
}
@@ -97,7 +97,7 @@ void BluetoothDispatcher::onNewDeviceConnected(DeviceConnectedCb deviceConnected
}
void BluetoothDispatcher::sendPackage(const char *package, size_t size) {
- _controller->write((uint8_t *)package, size);
+ _controller->write((uint8_t *) package, size);
}
diff --git a/src/RF/BluetoothDispatcher.hpp b/src/RF/BluetoothDispatcher.hpp
index e33641e..c922113 100644
--- a/src/RF/BluetoothDispatcher.hpp
+++ b/src/RF/BluetoothDispatcher.hpp
@@ -29,7 +29,7 @@ class BluetoothDispatcher {
public:
BluetoothDispatcher(BluetoothSerial *controller, const char *device_name = "Helicopter");
bool initialize(volatile bool loop_on_fail = true, int readTimeoutMS = 2);
- void tick(const char *message_delimeter = "\n");
+ void tick(const char message_delimeter = '\n');
void onNewPackageReceived(NewPackageCallback newPackageReceivedCb);
void onNewDeviceConnected(DeviceConnectedCb deviceConnectedCb);
void sendPackage(const char *package, size_t size);
diff --git a/src/main.cpp b/src/main.cpp
index 3bbeba1..b493106 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -11,14 +11,18 @@ 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(BATTERY_DATA_PIN, BATTERY_DATA_SWITCH_PIN)),
- new PIDController(1.f, 1.f, 1.f, new BrushedMotor(1, 2, 3, 4, 1), 1),
- new PIDController(1.f, 1.f, 1.f, new BrushedMotor(1, 2, 3, 4, 2), 2),
- new PIDController(1.f, 1.f, 1.f, new BrushedMotor(1, 2, 3, 4, 3), 3), true)));
+ 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(BATTERY_DATA_PIN, BATTERY_DATA_SWITCH_PIN)),
+ new SavedPidRegulator(3.5f, 0.0015f, 0.01f, "HeightController"), // height
+ new SavedPidRegulator(2.f, 12.f, 0.f, "YawController"), // yaw
+ new BrushedMotor(1, 2, 3, 4, 1),
+ new BrushedMotor(1, 2, 3, 4, 1),
+ new PIDController(0.6f, 3.5f, 0.03f, new BrushedMotor(1, 2, 3, 4, 3), "RollController"), // pitch
+ true)));
}
void loop() {