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

PID controllers hierarchy revision

Browse Source
This commit is contained in:
Goga Coder
2024-03-08 23:10:55 +07:00
parent 271bd845bf
commit 4a85827150
16 changed files with 389 additions and 234 deletions
Download Patch File Download Diff File Expand all files Collapse all files

151
src/Logic/FlightController.cpp
View File

@@ -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<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);
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;
}

70
src/Logic/FlightController.hpp
View File

@@ -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 <memory>
@@ -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> 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; // [%]

133
src/Logic/FlightDispatcher.cpp
View File

@@ -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();

59
src/Logic/PID.hpp
View File

@@ -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);
}
}
};

46
src/Logic/PIDController.hpp Normal file
View File

@@ -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;
};

71
src/Logic/SavedPidRegulator.hpp Normal file
View File

@@ -0,0 +1,71 @@
#include "GyverPID.h"
#include "Preferences.h"
#include "esp_log.h"
#include <memory>
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<PidParams> getParams() {
std::shared_ptr params = std::make_unique<PidParams>();
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);
}
}
};