MAVSDK_Dev/mavone/mavone.cpp

//
// Simple example to demonstrate how takeoff and land using MAVSDK.
//

#include <mavsdk/mavsdk.h>
#include <mavsdk/plugins/action/action.h>
#include <mavsdk/plugins/telemetry/telemetry.h>
#include <mavsdk/plugins/mavlink_passthrough/mavlink_passthrough.h>
#include <cstdint>
#include <iostream>
#include <fstream>
#include <sstream>
#include <iomanip>
#include <future>
#include <thread>
#include <chrono>
#include <vector>
#include <mutex>
#include <map>
#include <memory>

#include <mysql_driver.h>
#include <mysql_connection.h>
#include <cppconn/resultset.h>
#include <cppconn/statement.h>

using namespace mavsdk;
using std::chrono::seconds;
using std::chrono::milliseconds;
using std::this_thread::sleep_for;

struct MavInitParameter {
    std::string connectPort;
    std::string connectPort2;

    float C;
    std::string mysqlHost;
    std::string mysqlPort;
    std::string mysqlUser;
    std::string mysqlPW;
    std::string mysqlDatabase;
};

enum class systemHandlerState {
    Init,                       // 剛剛交給 systemHandler 去處理
    Prologue,                   // 建立了 mysql row 並取得 serialNo
    Ready,                      // 收到第一筆並放到 telemetryInfo 後
    Disconnected                // mavsdk 判斷該系統斷線了
};

struct systemHandlerInfo {
    int systemID;
    std::thread systemThread;
    systemHandlerState handlerState;
    int mysqlSN;

};




// 我無法解決跨執行緒的 promise 變數生命週期問題
// 用最智障的全域變數 (砍掉 promise 結構簡單好多)
std::map<int,std::map<std::string,std::string>> gTelemetryInfo;
std::map<int,std::map<std::string,double>> gVehicleCommand;
std::mutex gTeleMtx;


bool reset;


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 == "connectPort2") {
                initSetting.connectPort2 = 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 << "Parameters read successfully!" << std::endl;
    // Print all the parameters
    for (const auto& parameter : parameters) {
        std::cout << "Line: " << parameter << std::endl;
    }
    return 0;
}

/*
 * Important multi-threaded functions, 
 * handling system initialization, receiving messages and sending messages.
 */
void systemHandler(System& system) {
    // Initialization
    auto telemetry = Telemetry{system};
    auto action = Action{system};
    int sysid = static_cast<int>(system.get_system_id());
    std::cout << "System " << sysid << " get in Thread. (debug)" << std::endl; // debug

    // Store telemetry information
    std::map<std::string,std::string> telemetryInfo; 


    // Put all the subscriber init in this section
    telemetry.subscribe_position([&system, &telemetryInfo](Telemetry::Position position) {
        // 轉換數字到字串用的暫存變數
        std::ostringstream num_str_ss;

        // std::cout << "System ID: " << sysid << " Altitude: " << position.relative_altitude_m << " m\n"; // debug
        // Store telemetry information
        num_str_ss << std::fixed << std::setprecision(4) << position.relative_altitude_m;
        telemetryInfo["drone_alt"] = num_str_ss.str();
        num_str_ss.str("");

        num_str_ss << std::fixed << std::setprecision(8) << position.latitude_deg;
        telemetryInfo["vehicle_lat"] = num_str_ss.str();
        num_str_ss.str("");

        num_str_ss << std::fixed << std::setprecision(8) << position.longitude_deg;
        telemetryInfo["vehicle_lon"] = num_str_ss.str();
        num_str_ss.str("");
    });

    telemetry.subscribe_raw_gps([&system, &telemetryInfo](Telemetry::RawGps rawGps) {
        // 轉換數字到字串用的暫存變數
        std::ostringstream num_str_ss;

        // Store telemetry information
        num_str_ss << std::fixed << std::setprecision(4) << rawGps.velocity_m_s;
        telemetryInfo["vehicle_speed"] = num_str_ss.str();
        num_str_ss.str("");
    });

    telemetry.subscribe_attitude_euler([&system, &telemetryInfo](Telemetry::EulerAngle eulerAngle) {
        // 轉換數字到字串用的暫存變數
        std::ostringstream num_str_ss;

        // Store telemetry information
        num_str_ss << std::fixed << std::setprecision(4) << eulerAngle.roll_deg;
        telemetryInfo["drone_roll"] = num_str_ss.str();
        num_str_ss.str("");

        num_str_ss << std::fixed << std::setprecision(4) << eulerAngle.pitch_deg;
        telemetryInfo["drone_pitch"] = num_str_ss.str();
        num_str_ss.str("");

        num_str_ss << std::fixed << std::setprecision(4) << eulerAngle.yaw_deg;
        telemetryInfo["drone_yaw"] = num_str_ss.str();
        num_str_ss.str("");        
    });

    telemetry.subscribe_heading([&system, &telemetryInfo](Telemetry::Heading heading) {
        // 轉換數字到字串用的暫存變數
        std::ostringstream num_str_ss;

        // Store telemetry information
        num_str_ss << std::fixed << std::setprecision(4) << heading.heading_deg;
        telemetryInfo["vehicle_head"] = num_str_ss.str();
        num_str_ss.str("");
    });

    telemetry.subscribe_battery([&system, &telemetryInfo](Telemetry::Battery battery) {
        // 轉換數字到字串用的暫存變數
        std::ostringstream num_str_ss;

        // Store telemetry information
        num_str_ss << std::fixed << std::setprecision(4) << battery.remaining_percent;
        telemetryInfo["vehicle_bat"] = num_str_ss.str();
        num_str_ss.str("");
    });

    telemetry.subscribe_in_air([&system, &telemetryInfo](bool in_air) {
        if(in_air){
            telemetryInfo["drone_air"] = "1";
        }else{
            telemetryInfo["drone_air"] = "0";
        }
    });

    telemetry.subscribe_armed([&system, &telemetryInfo](bool is_armed) {
        if(is_armed){
            telemetryInfo["drone_arm"] = "1";
        }else{
            telemetryInfo["drone_arm"] = "0";
        }
    });

    // Wait Until telemetryInfo get something.
    while(telemetryInfo.empty()){
        sleep_for(seconds(1));
    }
    
    // Loop
    while(system.is_connected() && !reset) {
        
        // Send Telemetry Data 
        gTeleMtx.lock();
        gTelemetryInfo[sysid] = telemetryInfo;
        gTeleMtx.unlock();

        // Deal Command 

        // for purpose
        sleep_for(milliseconds(100));
    }

    // Destroy
    gVehicleCommand[sysid]["is_connected"] = 0;

    // Thread Terminate
    std::cout << "Thread Out " << sysid << std::endl; //debug
}


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

    if (argc != 2) {
        std::cout << "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 << "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 << "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 << "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 << "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 << "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 << "SystemHandler:" << std::to_string(handlerInfo.systemID) << " at Prologue " << std::endl; //debug

                    // 確認之前的 handlerInfo.mysqlSN 有正常被取得  若沒有整個程式即將關閉
                    if(handlerInfo.mysqlSN == -1) {
                        reset = true;
                        std::cout << "handlerInfo.mysqlSN not aquired. Something goes wrong. 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;
            }
            
        }
        
    }

    // // dev del S

    //     sleep_for(seconds(3));
    //     std::cout << "connect system number: " << gTelemetryInfo.size() << std::endl;
    //     for(int t=0;t<10;t++){
    //         for(int i = 0; i < gTelemetryInfo.size()-1 ; i++){
    //             auto& info = gTelemetryInfo[i];
    //             std::cout << "Sysid : " << i+1 << " ,Alt : " << info["drone_alt"] << " ,Lon : " << info["vehicle_lon"] << " ,Lat : " << info["vehicle_lat"] << std::endl;
    //         }
    //         sleep_for(seconds(1));
    //     }

    // // dev del E





    // When Progrem Terminate
    reset = true;

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

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