firmware/Logic/FlightDispatcher.cpp

#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) {
    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: _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() {
    /* 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())));
}