Project structure redefined
This commit is contained in:
194
src/Logic/FlightController.cpp
Normal file
194
src/Logic/FlightController.cpp
Normal file
@@ -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;
|
||||
}
|
||||
74
src/Logic/FlightController.hpp
Normal file
74
src/Logic/FlightController.hpp
Normal file
@@ -0,0 +1,74 @@
|
||||
#include "PID.hpp"
|
||||
#include <memory>
|
||||
#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
|
||||
};
|
||||
180
src/Logic/FlightDispatcher.cpp
Normal file
180
src/Logic/FlightDispatcher.cpp
Normal file
@@ -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())));
|
||||
}
|
||||
35
src/Logic/FlightDispatcher.hpp
Normal file
35
src/Logic/FlightDispatcher.hpp
Normal file
@@ -0,0 +1,35 @@
|
||||
#include "FlightController.hpp"
|
||||
#include "GSON.h"
|
||||
#include "RF/BluetoothDispatcher.hpp"
|
||||
#include <map>
|
||||
|
||||
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<size_t, std::function<void(gson::Entry)>> _routes;
|
||||
};
|
||||
56
src/Logic/PID.hpp
Normal file
56
src/Logic/PID.hpp
Normal file
@@ -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);
|
||||
}
|
||||
}
|
||||
};
|
||||
Reference in New Issue
Block a user