MAVSDK_Dev/mavone/mavone.cpp

#include "mavone.h"
#include "systemHandler.h"

// 我無法解決跨執行緒的 promise 變數生命週期問題
// 用最智障的全域變數 (砍掉 promise 結構簡單好多)
#include "globals.h"

int initializeParameters(const std::string& filePath, MavInitParameter& initSetting) {
    std::ifstream configFile(filePath);
    if (!configFile) {
        return 1;
    }
    
    std::vector<std::string> parameters; // Use a vector to store the parameters
    std::string line;
    while (std::getline(configFile, line)) {
        if (line == "EOC") {
            break; // Stop reading when "EOC" is encountered
        }
        // Split the line into key and value
        std::size_t delimiterPos = line.find('=');
        if (delimiterPos != std::string::npos) {
            std::string key = line.substr(0, delimiterPos);
            std::string value = line.substr(delimiterPos + 1);

            // Check if the key matches any field in MavInitParameter
            if (key == "MAVSDKListeningPort") {
                initSetting.connectPort = value;
            } else if (key == "mysqlHost") {
                initSetting.mysqlHost = value;
            } else if (key == "mysqlPort") {
                initSetting.mysqlPort = value;
            } else if (key == "mysqlUser") {
                initSetting.mysqlUser = value;
            } else if (key == "mysqlPW") {
                initSetting.mysqlPW = value;    
            } else if (key == "mysqlDatabase") {
                initSetting.mysqlDatabase = value;                                                            
            } else if (key == "C") {
                initSetting.C = std::stof(value);
            }
        }
        parameters.push_back(line); // Add the parameter to the vector
    }
    configFile.close();
    std::cout << "(mavone.cpp:initializeParameters) Parameters read successfully!" << std::endl;
    // Print all the parameters
    for (const auto& parameter : parameters) {
        std::cout << "(mavone.cpp:initializeParameters) Line: " << parameter << std::endl;
    }
    return 0;
}


int main(int argc, char** argv)
{
    reset = false;

    if (argc != 2) {
        std::cout << "(mavone.cpp:main) Config File Needed! ex. config.txt " << std::endl;
        return 1;
    }

    // Program Initial Read Setting from config file
    struct MavInitParameter initSetting;
    auto i = initializeParameters(argv[1], initSetting);
    // auto i = initializeParameters("config.txt", initSetting);
    if (i == 1){
        std::cerr << "(mavone.cpp:main) Failed to open config file: " << argv[1] << std::endl;
    }

    // Start MAVSDK Server and connect to it.
    Mavsdk mavsdk;
    ConnectionResult connection_result = mavsdk.add_any_connection(initSetting.connectPort);
    if (connection_result != ConnectionResult::Success) {
        std::cerr << "(mavone.cpp:main) Connection failed: " << connection_result << '\n';
        return 1;
    }

    // For store Discover systems in a list
    std::vector<systemHandlerInfo> systemHandlerInfos;

    // Subscribe to new system discovery
    mavsdk.subscribe_on_new_system([&mavsdk, &systemHandlerInfos]() {
        // Get the last subscribed system 
        auto comingSystems = mavsdk.systems();
        std::shared_ptr<System> sys = comingSystems.back();
        int sysid = static_cast<int>(sys->get_system_id());
        // Avoid Duplicate System ID (Component ID is careless, maybe determind by UUID will be better?)
        for(int i = 0;i < systemHandlerInfos.size();i++) {
            if(sys->get_system_id() == systemHandlerInfos[i].systemID){
                comingSystems.pop_back();
                if(comingSystems.size() != 0){
                    sys = comingSystems.back();
                } else {
                    std::cout << "(mavone.cpp:main) Duplicate System ID Collision : " << sysid << std::endl;
                    return;
                }
            }
        }
        // make std::shared_ptr<MAVSDK::System> back to MAVSDK::System
        System& new_system = *sys;
        // std::cout << "system detect : " << sysid << "(Debug)"<< std::endl; // debug

        // Let handler progrem deal with System  
        std::thread systemHandleThread([&new_system]() {systemHandler(new_system);});
        gVehicleCommand[sysid]["is_connected"] = 1;

        // Need some extra time fpr complete init
        sleep_for(milliseconds(500));

        systemHandlerInfos.push_back(systemHandlerInfo{
            sysid,
            std::move(systemHandleThread),
            systemHandlerState::Init,
            -1
        });

    });

    // MySQL connection is here
    sql::mysql::MySQL_Driver *driver;
    sql::Connection *con;
    sql::Statement *stmt;
    sql::ResultSet *res;
    try {
        driver = sql::mysql::get_mysql_driver_instance();
        con = driver->connect("tcp://" + initSetting.mysqlHost + ":" + initSetting.mysqlPort, initSetting.mysqlUser, initSetting.mysqlPW);
        con->setSchema(initSetting.mysqlDatabase);
        stmt = con->createStatement();
        
    } catch (sql::SQLException &e) {
        std::cout << "(mavone.cpp:main) MySQL Connection Error: " << e.what() << std::endl;
        return 1;
    }

    // Main Loop is here
    //   -- 將這個服務目前的狀態 更新到 mysql 中
    //   -- 從 systemHandlerInfos 中 輪巡所有 system 檢查並處理是否有需要丟到 mysql 的資料
    //   -- 從 mysql 抓到指令 丟到 systemHandlerInfo

    int randomValue;
    std::string sqlString;
    std::map<std::string,std::string> aTelemetryInfo;

    while(!reset){

        sleep_for(seconds(1)); // debug

        // 每個 System 輪詢過去
        for( auto& handlerInfo : systemHandlerInfos){
            switch(handlerInfo.handlerState){
                case systemHandlerState::Init :
                    std::cout << "(mavone.cpp:main) SystemHandler:" << std::to_string(handlerInfo.systemID) << " at Init " << std::endl; //debug

                    // 用系統時間生成的隨機數 標記目前欄位
                    std::srand(std::time(0));
                    randomValue = std::rand() % 10000;
                    sqlString = "INSERT INTO NodeRed_one (system_id, mavsys_connect_status) VALUES (" + std::to_string(handlerInfo.systemID) + ", " + std::to_string(randomValue) + ");";
                    // std::cout << "MySQL Insert Query: " << sqlString << std::endl; //debug
                    stmt->execute(sqlString);

                    // 取得對應的欄位序號 SerialNO
                    sqlString =  "SELECT SerialNO FROM NodeRed_one WHERE system_id = " + std::to_string(handlerInfo.systemID) + " AND mavsys_connect_status = " +std::to_string(randomValue) + ";";
                    // std::cout << "MySQL SELECT Query: " << sqlString << std::endl; //debug
                    res = stmt->executeQuery(sqlString);
                    if(res->next()) {
                        // std::cout << "Query Reselt: " << res->getString(1) << std::endl;  //debug
                        handlerInfo.mysqlSN = std::stof(res->getString(1));
                    }

                    // 切換到下一個狀態
                    handlerInfo.handlerState = systemHandlerState::Prologue;
                    // 更新 mavsys_connect_status 欄位
                    sqlString = "UPDATE NodeRed_one SET mavsys_connect_status = 'Prologue' WHERE SerialNO = " + std::to_string(handlerInfo.mysqlSN) + ";";
                    stmt->execute(sqlString);

                    break;

                case systemHandlerState::Prologue :
                    std::cout << "(mavone.cpp:main) SystemHandler:" << std::to_string(handlerInfo.systemID) << " at Prologue " << std::endl; //debug

                    // 確認之前的 handlerInfo.mysqlSN 有正常被取得  若沒有整個程式即將關閉
                    if(handlerInfo.mysqlSN == -1) {
                        reset = true;
                        std::cout << "(mavone.cpp:main) handlerInfo.mysqlSN not aquired. Fatal Error. Shutdown all service. " << std::endl;
                    }

                    // 確認系統是否仍連線 
                    if(gVehicleCommand[handlerInfo.systemID]["is_connected"] == 0){
                        handlerInfo.handlerState = systemHandlerState::Disconnected;
                        // 更新 mavsys_connect_status 欄位
                        sqlString = "UPDATE NodeRed_one SET mavsys_connect_status = 'Disconnected' WHERE SerialNO = " + std::to_string(handlerInfo.mysqlSN) + ";";
                        stmt->execute(sqlString);
                        break;
                    }
                    
                    // MAVSDK 還沒收到第一筆 telemetry 就持續在這個狀態
                    aTelemetryInfo = gTelemetryInfo[handlerInfo.systemID];
                    if(aTelemetryInfo.size() == 0){
                        break;
                    }

                    // 切換到下一個狀態
                    handlerInfo.handlerState = systemHandlerState::Ready;
                    // 更新 mavsys_connect_status 欄位
                    sqlString = "UPDATE NodeRed_one SET mavsys_connect_status = 'Ready' WHERE SerialNO = " + std::to_string(handlerInfo.mysqlSN) + ";";
                    stmt->execute(sqlString);

                case systemHandlerState::Ready :
                    // std::cout << "SystemHandler:" << std::to_string(handlerInfo.systemID) << " at Ready " << std::endl; //debug

                    // 確認系統是否仍連線 
                    if(gVehicleCommand[handlerInfo.systemID]["is_connected"] == 0){
                        handlerInfo.handlerState = systemHandlerState::Disconnected;
                        // 更新 mavsys_connect_status 欄位
                        sqlString = "UPDATE NodeRed_one SET mavsys_connect_status = 'Disconnected' WHERE SerialNO = " + std::to_string(handlerInfo.mysqlSN) + ";";
                        stmt->execute(sqlString);
                        break;
                    }

                    // 把 MAVSDK 從無人機接到的 telemetry 放到 mysql
                    aTelemetryInfo = gTelemetryInfo[handlerInfo.systemID];
                    if(aTelemetryInfo.size() != 0) {
                        sqlString = "UPDATE NodeRed_one SET ";
                        for(const auto& pair : aTelemetryInfo) {
                            // std::cout << "Key: " << pair.first << ", Value: " << pair.second << std::endl; //debug
                            sqlString += pair.first + " = '" + pair.second + "',";
                        }
                        sqlString.pop_back();
                        sqlString += " WHERE SerialNO = " + std::to_string(handlerInfo.mysqlSN) + ";";
                        // std::cout << "MySQL UPDATE Query: " << sqlString << std::endl; //debug
                        stmt->execute(sqlString);
                        gTelemetryInfo[handlerInfo.systemID] = {};
                    }

                    
                    // TODO: 把 mysql 的指令放給 MAVSDK 準備傳到無人機


                    // reset = true; //debug
                    break;

                case systemHandlerState::Disconnected :
                    std::cout << "SystemHandler:" << std::to_string(handlerInfo.systemID) << " at Disconnected " << std::endl; //debug
                    
                    
                    reset = true; //debug
                    break;
            }
            
        }
        
    }

    // When Progrem Terminate
    reset = true;

    // make all thread join
    for( auto& handlerInfo : systemHandlerInfos){
        handlerInfo.systemThread.join();
    }

    // terminate MySQL Indicator
    delete res;
    delete stmt;
    delete con;

    return 0;
}






















// ===================================================
// 1.

    // std::cout << "Mark A" << std::endl;
    // sleep_for(seconds(3));

    // System& s0 = *mavsdk.systems()[0];
    // System& s1 = *mavsdk.systems()[1];
    // std::cout << static_cast<int>(s0.get_system_id()) << std::endl;


    // auto t0 = Telemetry{s0};
    // auto t1 = Telemetry{s1};
    // t0.subscribe_position([](Telemetry::Position position) {
    //     std::cout << "System ID: " << "0" << " Altitude: " << position.relative_altitude_m << " m\n";
    // });

    // t1.subscribe_position([](Telemetry::Position position) {
    //     std::cout << "System ID: " << "1" << " Altitude: " << position.relative_altitude_m << " m\n";
    // });