Helicopter/firmware
2
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Project structure redefinition

Browse Source
This commit is contained in:
Goga Coder
2024-03-01 22:58:59 +07:00
parent 9d0f5d06ea
commit 69c736b1eb
18 changed files with 126 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
src/Logic/FlightController.cpp
View File

@@ -1,6 +1,3 @@
// This is a personal academic project. Dear PVS-Studio, please check it.
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
#include "FlightController.hpp"
FlightController::FlightController(Sensors *sensors, PIDController *pid1, PIDController *pid2,
@@ -113,6 +110,9 @@ void FlightController::_boarding() {
void FlightController::startImuCalibration() {
if (_status != DeviceStatus::Idle)
return;
ESP_LOGI(_tag, "Started IMU calibration");
_status = DeviceStatus::IsImuCalibration;
_sensors->startMpuCalibration();
_sensors->onMpuCalibrationFinished([this]() {
this->_status = DeviceStatus::Idle;
});
@@ -185,10 +185,12 @@ void FlightController::setPid3Params(float p, float i, float d) {
}
}
PidSettings *FlightController::pidSettings() const {
PidSettings pid1 = { .p = _pid1->Kp, .i = _pid1->Ki, .d = _pid1->Kd };
PidSettings pid2 = { .p = _pid2->Kp, .i = _pid2->Ki, .d = _pid2->Kd };
PidSettings pid3 = { .p = _pid3->Kp, .i = _pid3->Ki, .d = _pid3->Kd };
static PidSettings settings[] = { pid1, pid2, pid3 };
return settings;
std::unique_ptr<PidSettings> FlightController::pidSettings() const {
std::unique_ptr<PidSettings> settings = std::make_unique<PidSettings>();
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);
}

15
src/Logic/FlightController.hpp
View File

@@ -1,6 +1,9 @@
// This is a personal academic project. Dear PVS-Studio, please check it.
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
#include "PID.hpp"
#include <memory>
#include "Sensors/Sensors.hpp"
#include <memory>
enum DeviceStatus {
Idle = 0,
@@ -24,12 +27,18 @@ struct FlightParams {
float pitch; // [degrees]
};
struct PidSettings {
struct PidParam {
float p;
float i;
float d;
};
struct PidSettings {
PidParam pid1;
PidParam pid2;
PidParam pid3;
};
class FlightController {
public:
FlightController(Sensors *sensors, PIDController *pid1, PIDController *pid2,
@@ -51,7 +60,7 @@ class FlightController {
void setPid1Params(float p, float i, float d);
void setPid2Params(float p, float i, float d);
void setPid3Params(float p, float i, float d);
PidSettings *pidSettings() const;
std::unique_ptr<PidSettings> pidSettings() const;
private:
void _updateBatteryCharge();
[[deprecated]] void _takeoff();

40
src/Logic/FlightDispatcher.cpp
View File

@@ -43,22 +43,12 @@ void FlightDispatcher::tick() {
}
void FlightDispatcher::_onNewDeviceConnected(BTAddress device) {
auto currentState = _flightController->currentDeviceState();
gson::string package;
package.beginObj();
package["batteryCharge"] = currentState.batteryCharge;
package["flightHeight"] = currentState.flightHeight;
package["status"] = currentState.status;
package.endObj();
package.end();
package += "\r\n";
auto pkg = package.s.c_str();
_bluetoothDispatcher->sendPackage(pkg, strlen(pkg));
_sendTelemetry();
}
void FlightDispatcher::_onNewMessageReceived(char *package) {
using sutil::SH;
ESP_LOGD(_tag, "Received new package: %s", package);
ESP_LOGI(_tag, "Received new package: %s", package);
gson::Doc pkg;
if (!pkg.parse(package, _jsonMaxDepth)) {
ESP_LOGE(_tag, "Parcing error occured with new package (error %s, place %d): %s",
@@ -137,27 +127,28 @@ void FlightDispatcher::_pidSettingsOpened() {
pkg.beginObj();
pkg.beginObj("p1");
pkg["p"] = settings[0].p;
pkg["i"] = settings[0].i;
pkg["d"] = settings[0].d;
pkg["p"] = settings->pid1.p;
pkg["i"] = settings->pid1.i;
pkg["d"] = settings->pid1.d;
pkg.endObj();
pkg.beginObj("p2");
pkg["p"] = settings[1].p;
pkg["i"] = settings[1].i;
pkg["d"] = settings[1].d;
pkg["p"] = settings->pid2.p;
pkg["i"] = settings->pid2.i;
pkg["d"] = settings->pid2.d;
pkg.endObj();
pkg.beginObj("p3");
pkg["p"] = settings[2].p;
pkg["i"] = settings[2].i;
pkg["d"] = settings[2].d;
pkg["p"] = settings->pid3.p;
pkg["i"] = settings->pid3.i;
pkg["d"] = settings->pid3.d;
pkg.endObj();
pkg.endObj();
pkg.end();
pkg += "\r\n";
pkg.s = String(MessageType::PidSettings) + ";" + pkg.s + _message_delimeter;
_bluetoothDispatcher->sendPackage(pkg.s.c_str(), strlen(pkg.s.c_str()));
ESP_LOGI(_tag, "PID settings sended %s", pkg.s.c_str());
}
void FlightDispatcher::_sendTelemetry() {
@@ -165,7 +156,7 @@ void FlightDispatcher::_sendTelemetry() {
auto state = _flightController->currentDeviceState();
gson::string package;
package.beginObj();
package["batteryCharge"] = state.batteryCharge;
package["charge"] = state.batteryCharge;
package["flightHeight"] = state.flightHeight;
package["status"] = state.status;
package["y"] = state.mpuState.yaw;
@@ -174,7 +165,6 @@ void FlightDispatcher::_sendTelemetry() {
package["zIn"] = state.mpuState.zInertial;
package.endObj();
package.end();
package += "\r\n";
package.s = String(MessageType::UpdatePackage) + ";" + package.s + _message_delimeter;
_bluetoothDispatcher->sendPackage(package.s.c_str(), strlen((package.s.c_str())));
}

9
src/Logic/FlightDispatcher.hpp
View File

@@ -1,8 +1,14 @@
// This is a personal academic project. Dear PVS-Studio, please check it.
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
#include "FlightController.hpp"
#include "GSON.h"
#include "RF/BluetoothDispatcher.hpp"
#include <map>
// Message type annotation for mobile app
enum MessageType { UpdatePackage = 0, PidSettings };
class FlightDispatcher {
/* Deserialize state and update it in FlightController. */
public:
@@ -26,10 +32,9 @@ class FlightDispatcher {
static constexpr const char *_tag = "FlightDispatcher";
static constexpr const int _telemetryTimeIntervalMS = 200;
static constexpr const uint8_t _jsonMaxDepth = 5;
static constexpr const char *_message_delimeter = "\n";
uint32_t _telemetryTimer = millis();
BluetoothDispatcher *_bluetoothDispatcher;
FlightController *_flightController;
std::map<size_t, std::function<void(gson::Entry)>> _routes;
};

3
src/Logic/PID.hpp
View File

@@ -1,3 +1,6 @@
// This is a personal academic project. Dear PVS-Studio, please check it.
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
#include "GyverPID.h"
#include "Motor/BrushedMotor.hpp"
#include "Preferences.h"