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;
}

// 這裡會對 mysql 上的指令 做第一次轉換，再放給 systemHandler 做第二次轉換
std::map<std::string, int> commandTypeMap = {
    {"aGuided", 101},
    {"aLand", 102},
    {"aArm", 103},
    {"aSetHeight", 104},
    {"aTakeoff", 105},
    {"aGoto", 106},
};

std::map<int, std::array<double, 2>> decodeMysqlCommand(std::string commandType, std::string Para1, std::string Para2) {
    // std::cout << commandType << ", " << Para1 << ", " << Para2 <<std::endl;  // for Dev
    std::map<std::string, std::function<void()>> commandMapFunc;
    std::map<int, std::array<double, 2>> result;

    switch (commandTypeMap[commandType]) {
        case 101: // aGuided
            result[1] = {4,0}; // means change mode to 4
            break;
        case 102: // aLand
            result[1] = {9,0}; // means change mode to 9
            break;
        case 103: // aArm
            result[11] = {0,0}; // means action arm
            break;
        case 104: // aSetHeight
            result[13] = {std::stof(Para1),0}; // means change height to Para1
            break;
        case 105: // aTakeoff
            result[12] = {0,0}; // means action takeoff
            break;
        case 106: // aGoto
            result[21] = {std::stof(Para1),std::stof(Para2)}; // means goto 
            break;
    }

    return result;
}

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;
    // Set this MAVLINK service has system id 197, component id 254, NOT send heartbeat
    mavsdk::Mavsdk::Configuration mavserver_configuration(197,254,false);
    mavsdk.set_configuration(mavserver_configuration);
    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;
        int sysid;

        // Avoid Duplicate System ID 
        bool duplicated;
        // for(int i=0; i<comingSystems.size(); i++){
        for(int i=comingSystems.size()-1; i>=0; i--){
            sys = comingSystems[i];
            if (!sys->is_connected()) {continue;}
            sysid = static_cast<int>(sys->get_system_id());
            duplicated = false;

            for(int j=0; j<systemHandlerInfos.size(); j++){
                if(sysid == systemHandlerInfos[j].systemID){
                    duplicated = true;
                    break; // this will break the j for loop
                }
            }

            if(duplicated == false){
                // Get New System                
                System& new_system = *sys;  // make std::shared_ptr<MAVSDK::System> back to MAVSDK::System
                std::cout << "(mavone.cpp:subscribe_on_new_system) system detect sysid : " << sysid << "(Debug)"<< std::endl; // debug

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

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

                // only get ONE system each time
                break; // this will break the i for loop
            }
        }

    });

    // 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:while) 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:while) SystemHandler:" << std::to_string(handlerInfo.systemID) << " at Prologue " << std::endl; //debug

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

                    // 確認系統是否仍連線 
                    if (gHandlerMask[handlerInfo.systemID]["is_connected"] == 0) {
                        handlerInfo.handlerState = systemHandlerState::Disconnected;
                        break;
                    }
                    
                    // MAVSDK 還沒收到第一筆 telemetry 就持續在這個狀態
                    aTelemetryInfo = gTelemetryInfo[handlerInfo.systemID];
                    if (aTelemetryInfo.size() == 0) {
                        break;
                    }

                    // 切換到下一個狀態
                    handlerInfo.handlerState = systemHandlerState::Ready;
                    std::cout << "(mavone.cpp:while) SystemHandler:" << std::to_string(handlerInfo.systemID) << " into Ready " << std::endl; //debug
                    // 更新 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 (gHandlerMask[handlerInfo.systemID]["is_connected"] == 0) {
                        handlerInfo.handlerState = systemHandlerState::Disconnected;
                        break;
                    }

                    aTelemetryInfo = gTelemetryInfo[handlerInfo.systemID];

                    // TODO: 把 mysql 的指令放給 MAVSDK 準備傳到無人機
                    sqlString = "SELECT command_type,command_para1,command_para2 FROM NodeRed_one WHERE SerialNo = " \
                                + std::to_string(handlerInfo.mysqlSN) + " AND command_type IS NOT NULL;";
                    res = stmt->executeQuery(sqlString);
                    if (res->next()) {
                        // 有讀到資料的話 就直接更新 gVehicleCommand
                        gComMtx.lock();
                        gVehicleCommand[handlerInfo.systemID] = decodeMysqlCommand(res->getString(1), res->getString(2), res->getString(3));
                        gComMtx.unlock();
                        // 本來想要把清空 mysql 的 command 欄位跟下面更新 telemetry 放一起  但是這樣會把程式複雜度弄上去 所以算了
                        sqlString = "UPDATE NodeRed_one SET command_type = NULL, command_para1 = NULL, command_para2 = NULL WHERE SerialNo = " \
                                    + std::to_string(handlerInfo.mysqlSN) + ";";
                        stmt->execute(sqlString);
                    }
                    
                    // 把 MAVSDK 從無人機接到的 telemetry 放到 mysql
                    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] = {};
                    }

                    
                    


                    // reset = true; //debug
                    break;

                case systemHandlerState::Disconnected :
                    std::cout << "(mavone.cpp:while) SystemHandler:" << std::to_string(handlerInfo.systemID) << " at Disconnected. " << std::endl; //debug
                    
                    // 更新 mavsys_connect_status 欄位
                    sqlString = "UPDATE NodeRed_one SET mavsys_connect_status = 'Disconnected' WHERE SerialNO = " + std::to_string(handlerInfo.mysqlSN) + ";";
                    stmt->execute(sqlString);

                    // 變更狀態
                    handlerInfo.handlerState = systemHandlerState::Terminate;

                    // reset = true; //debug
                    break;
            }
            
        }

        // 刪除 Disconnected 的 handlerInfo
        for (auto it = systemHandlerInfos.begin(); it != systemHandlerInfos.end(); /* no increment here */) {
            if (it->handlerState == systemHandlerState::Terminate) {
                // 這個 if 如果不加 則副程序在結束以前就會被主程序放生 執行時會出現 SIGABRT 錯誤
                if (it->systemThread.joinable()) {
                    it->systemThread.join();
                }
                it = systemHandlerInfos.erase(it);
            } else {
                ++it;
            }
        }

    }

    // When Progrem Terminate
    reset = true;

    std::cout << "(mavone.cpp:main) Main Loop End !!!!!!" << std::endl;

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

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

    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";
    // });