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

Better routing

Browse Source
This commit is contained in:
Goga Coder
2024-01-18 19:30:25 +07:00
parent 5dd404d44b
commit 2aa9648dd4
8 changed files with 247 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files

101
Logic/FlightDispatcher.cpp
View File

@@ -54,10 +54,105 @@ void FlightDispatcher::_onNewDeviceConnected(BTAddress device) {
}
void FlightDispatcher::_onNewMessageReceived(char *package) {
ESP_LOGI(_tag, "Received new package: %s", package);
using sutil::SH;
ESP_LOGD(_tag, "Received new package: %s", package);
gson::Doc pkg;
pkg.parse(package, _jsonMaxDepth);
// TODO: process package
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: _flightController->startTakeoff(); 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() {