81 lines
2.6 KiB
C++
81 lines
2.6 KiB
C++
#include "FlightDispatcher.hpp"
|
|
|
|
FlightDispatcher::FlightDispatcher(BluetoothDispatcher *bluetoothDispatcher,
|
|
FlightController *flightController, 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) {
|
|
ESP_LOGI(_tag, "Received new package: %s", package);
|
|
gson::Doc pkg;
|
|
pkg.parse(package, _jsonMaxDepth);
|
|
// TODO: process package
|
|
}
|
|
|
|
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())));
|
|
}
|