firmware/src/Logic/FlightDispatcher.cpp

// 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) {
    _sendTelemetry();
}

void FlightDispatcher::_onNewMessageReceived(char *package) {
    using sutil::SH;
    ESP_LOGI(_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->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;
            }
        }
    }
}

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.flightController.p;
    pkg["i"] = settings.flightController.i;
    pkg["d"] = settings.flightController.d;
    pkg.endObj();

    pkg.beginObj("p2");
    pkg["p"] = settings.yawController.p;
    pkg["i"] = settings.yawController.i;
    pkg["d"] = settings.yawController.d;
    pkg.endObj();

    pkg.beginObj("p3");
    pkg["p"] = settings.pitchController.p;
    pkg["i"] = settings.pitchController.i;
    pkg["d"] = settings.pitchController.d;
    pkg.endObj();

    pkg.endObj();
    pkg.end();
    pkg.s = String(MessageType::PidSettings) + ";" + pkg.s;
    _bluetoothDispatcher->sendPackage(pkg.s.c_str());
    ESP_LOGI(_tag, "PID settings sended %s", pkg.s.c_str());
}

void FlightDispatcher::_sendTelemetry() {
    /* Notify mobile client about device state */
    auto state = _flightController->currentDeviceState();
    gson::string package;
    package.beginObj();
    package["charge"] = 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.s = String(MessageType::UpdatePackage) + ";" + package.s;
    _bluetoothDispatcher->sendPackage(package.s.c_str());
}