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

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