Project structure redefined
This commit is contained in:
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())));
|
||||
}
|
||||
Reference in New Issue
Block a user