diff --git a/CMakeLists.txt b/mavone/CMakeLists.txt similarity index 100% rename from CMakeLists.txt rename to mavone/CMakeLists.txt diff --git a/config.txt b/mavone/config.txt similarity index 100% rename from config.txt rename to mavone/config.txt diff --git a/mavone.cpp b/mavone/mavone.cpp similarity index 81% rename from mavone.cpp rename to mavone/mavone.cpp index 1bacffb..c5b6dac 100644 --- a/mavone.cpp +++ b/mavone/mavone.cpp @@ -15,6 +15,7 @@ #include #include #include +#include using namespace mavsdk; using std::chrono::seconds; @@ -30,11 +31,16 @@ struct MavInitParameter { struct systemHandlerInfo { int systemID; std::thread systemThread; - + std::future> telemetryInfo_fut; }; + + + + + int initializeParameters(const std::string& filePath, MavInitParameter& initSetting) { std::ifstream configFile(filePath); if (!configFile) { @@ -79,42 +85,78 @@ int initializeParameters(const std::string& filePath, MavInitParameter& initSett * Important multi-threaded functions, * handling system initialization, receiving messages and sending messages. */ - - - -void systemHandler(System& system){ - +void systemHandler(System& system, std::promise>& telemetryInfo_pro) { // Initialization auto telemetry = Telemetry{system}; auto action = Action{system}; std::cout << "System " << static_cast(system.get_system_id()) << " get in Thread.(debug)" << std::endl; // debug - telemetry.subscribe_position([&system](Telemetry::Position position) { + + // Store telemetry information + std::map telemetryInfo; + + // dev del S + std::cout << "mark tA" << std::endl; + std::future> telemetryInfo_future = telemetryInfo_pro.get_future(); // dev del + + std::cout << "mark tB" << std::endl; + if (!telemetryInfo_future.valid()) { + std::cout << "telemetryInfo_pro is not valid" << std::endl; + } + + telemetryInfo["test"] = 3.14; + telemetryInfo_pro.set_value(telemetryInfo); + std::cout << "mark tC" << std::endl; + if (!telemetryInfo_future.valid()) { + std::cout << "telemetryInfo_pro is not valid" << std::endl; + } + + + + // dev del E + + telemetry.subscribe_position([&system, &telemetryInfo, &telemetryInfo_pro](Telemetry::Position position) { std::cout << "System ID: " << static_cast(system.get_system_id()) << " Altitude: " << position.relative_altitude_m << " m\n"; // debug - // Feed To mysql - }); + // Store telemetry information + telemetryInfo["altitude"] = position.relative_altitude_m; + // Add more telemetry information as needed + + // Set the telemetry information in the promise + std::cout << "mark tsA" << std::endl; + telemetryInfo_pro.set_value(telemetryInfo); + std::cout << "mark tsB" << std::endl; + + + sleep_for(seconds(1)); // debug + }); sleep_for(seconds(5)); // debug std::cout << "Thread Out " << static_cast(system.get_system_id()) << std::endl; -} +} + int main(int argc, char** argv) { - if (argc != 2) { - std::cout << "Config File Needed! ex. config.txt" << std::endl; - return 1; - } + // 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(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); @@ -149,15 +191,32 @@ int main(int argc, char** argv) // // std::map telemetryInfo; - auto telemetryInfo_pro = std::promise>{}; - auto telemetryInfo_fut = telemetryInfo_pro.get_future(); + auto telemetryInfo_pro = std::promise>{}; + // auto telemetryInfo_fut = telemetryInfo_pro.get_future(); + + + // dev del S + + auto telemetryInfo_pro1 = std::promise>{}; + auto telemetryInfo_fut1 = telemetryInfo_pro.get_future(); + + std::thread worker([&telemetryInfo_pro1]() { + // 執行任務並將結果回傳至 promise + std::map result; + result["test"]=3.14; + telemetryInfo_pro1.set_value(result); + }); + worker.join(); + // dev del E + // Let handler progrem deal with System - std::thread systemHandleThread([&new_system]() {systemHandler(new_system);}); + std::thread systemHandleThread([&new_system, &telemetryInfo_pro]() {systemHandler(new_system, telemetryInfo_pro);}); systemHandlerInfos.push_back(systemHandlerInfo{ static_cast(new_system.get_system_id()), - std::move(systemHandleThread) + std::move(systemHandleThread), + std::move(telemetryInfo_fut1) }); }); @@ -169,9 +228,13 @@ int main(int argc, char** argv) // -- 從 systemHandlerInfos 中 輪巡所有 system 檢查並處理是否有需要丟到 mysql 的資料 // -- 從 mysql 抓到指令 丟到 systemHandlerInfo - - - + // for (const auto& info : systemHandlerInfos) { + // // Access info.systemID, info.systemHandleThread, info.telemetryInfo_fut + // // Perform operations on each object in systemHandlerInfos + // info.telemetryInfo_fut.wait(); + // auto x = info.telemetryInfo_fut.get(); + // std::cout << "mark D :" << info.systemID << "; " << x["altitude"] << std::endl; + // } // ==============================================================================