From feb441d2f4127b503a7fcc6e7395d5dd0bf649ca Mon Sep 17 00:00:00 2001 From: chiyu1468 Date: Fri, 12 Jan 2024 14:20:34 +0800 Subject: [PATCH] stable v0.3 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 修正 system id 重複獲取問題 修正 handlerState 卡在 Prologue 問題 --- mavone/CMakeLists.txt | 10 +++- mavone/globals.cpp | 31 ++++++++++- mavone/globals.h | 3 +- mavone/mavone.cpp | 111 +++++++++++++++++++++++---------------- mavone/systemHandler.cpp | 29 +++++++--- 5 files changed, 129 insertions(+), 55 deletions(-) diff --git a/mavone/CMakeLists.txt b/mavone/CMakeLists.txt index 9eea34f..f491f8d 100644 --- a/mavone/CMakeLists.txt +++ b/mavone/CMakeLists.txt @@ -15,10 +15,10 @@ add_library(mavoneSys_lib add_executable(mavone mavone.cpp - globals.cpp ) -find_package(MAVSDK REQUIRED) +# find MAVSDK package and accept only when MAVSDK version > 1.4.10 +find_package(MAVSDK 1.4.10 REQUIRED) target_link_libraries(mavone MAVSDK::mavsdk @@ -32,3 +32,9 @@ if(NOT MSVC) else() add_compile_options(mavone PRIVATE -WX -W2) endif() + + + + + + diff --git a/mavone/globals.cpp b/mavone/globals.cpp index 6d84fd6..3f7d8c2 100644 --- a/mavone/globals.cpp +++ b/mavone/globals.cpp @@ -1,6 +1,35 @@ #include "globals.h" +#include + +/* +用簡單粗暴的全域變數讓主迴圈跟副迴圈傳遞資料 +另外加個 互斥鎖(Mutex) 避免存取衝突 + +gTelemetryInfo + 這個變數是存放 副迴圈 到 主迴圈的資訊 (主要是要讓無人機的 telemetry 藉由主迴圈再寫進資料庫) + + 資料形式為 systemid[INT] : { 資訊名稱[String] : 資訊內容[String] } + + 例如 + gTelemetryInfo = { + 1 : { "vehicle_lat" : "24.16584990", "vehicle_lon" : "120.99977417", "vehicle_head" : "167.01" }, + 2 : { "vehicle_bat" : "0.97", "drone_air" : "1" } + } + 則代表裡面存有 system id 1 的 經緯度與航向 而 system id 2 有電量與滯空狀態 + +gVehicleCommand + + +gHandlerMask + + + +*/ + + std::map> gTelemetryInfo; -std::map> gVehicleCommand; +std::map>> gVehicleCommand; +std::map> gHandlerMask; std::mutex gTeleMtx; \ No newline at end of file diff --git a/mavone/globals.h b/mavone/globals.h index 23a56f8..b980104 100644 --- a/mavone/globals.h +++ b/mavone/globals.h @@ -7,7 +7,8 @@ #include extern std::map> gTelemetryInfo; -extern std::map> gVehicleCommand; +extern std::map>> gVehicleCommand; +extern std::map> gHandlerMask; extern std::mutex gTeleMtx; #endif \ No newline at end of file diff --git a/mavone/mavone.cpp b/mavone/mavone.cpp index 00c0155..25d31c0 100644 --- a/mavone/mavone.cpp +++ b/mavone/mavone.cpp @@ -81,42 +81,51 @@ int main(int argc, char** argv) // For store Discover systems in a list std::vector 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 sys = comingSystems.back(); - int sysid = static_cast(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; + std::shared_ptr sys; + int sysid; + + // Avoid Duplicate System ID + bool duplicated; + for(int i=0; i(sys->get_system_id()); + duplicated = false; + for(int j=0; j 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; + if(duplicated == false){ - // Need some extra time fpr complete init - sleep_for(milliseconds(500)); + // Get New System + System& new_system = *sys; // make std::shared_ptr back to MAVSDK::System + std::cout << "(mavone.cpp:subscribe_on_new_system) system detect sysid : " << sysid << "(Debug)"<< std::endl; // debug - systemHandlerInfos.push_back(systemHandlerInfo{ - sysid, - std::move(systemHandleThread), - systemHandlerState::Init, - -1 - }); + // 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; + } + } + + // Need some extra time fpr complete init + // sleep_for(milliseconds(500)); }); // MySQL connection is here @@ -144,15 +153,15 @@ int main(int argc, char** argv) std::string sqlString; std::map aTelemetryInfo; - while(!reset){ + while (!reset) { sleep_for(seconds(1)); // debug // 每個 System 輪詢過去 - for( auto& handlerInfo : systemHandlerInfos){ - switch(handlerInfo.handlerState){ + 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::cout << "(mavone.cpp:while) SystemHandler:" << std::to_string(handlerInfo.systemID) << " at Init " << std::endl; //debug // 用系統時間生成的隨機數 標記目前欄位 std::srand(std::time(0)); @@ -165,7 +174,7 @@ int main(int argc, char** argv) 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()) { + if (res->next()) { // std::cout << "Query Reselt: " << res->getString(1) << std::endl; //debug handlerInfo.mysqlSN = std::stof(res->getString(1)); } @@ -179,16 +188,16 @@ int main(int argc, char** argv) break; case systemHandlerState::Prologue : - std::cout << "(mavone.cpp:main) SystemHandler:" << std::to_string(handlerInfo.systemID) << " at Prologue " << std::endl; //debug + std::cout << "(mavone.cpp:while) SystemHandler:" << std::to_string(handlerInfo.systemID) << " at Prologue " << std::endl; //debug // 確認之前的 handlerInfo.mysqlSN 有正常被取得 若沒有整個程式即將關閉 - if(handlerInfo.mysqlSN == -1) { + if (handlerInfo.mysqlSN == -1) { reset = true; - std::cout << "(mavone.cpp:main) handlerInfo.mysqlSN not aquired. Fatal Error. Shutdown all service. " << std::endl; + std::cout << "(mavone.cpp:while) handlerInfo.mysqlSN not aquired. Fatal Error. Shutdown all service. " << std::endl; } // 確認系統是否仍連線 - if(gVehicleCommand[handlerInfo.systemID]["is_connected"] == 0){ + if (gHandlerMask[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) + ";"; @@ -198,12 +207,13 @@ int main(int argc, char** argv) // MAVSDK 還沒收到第一筆 telemetry 就持續在這個狀態 aTelemetryInfo = gTelemetryInfo[handlerInfo.systemID]; - if(aTelemetryInfo.size() == 0){ + 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); @@ -212,7 +222,7 @@ int main(int argc, char** argv) // std::cout << "SystemHandler:" << std::to_string(handlerInfo.systemID) << " at Ready " << std::endl; //debug // 確認系統是否仍連線 - if(gVehicleCommand[handlerInfo.systemID]["is_connected"] == 0){ + if (gHandlerMask[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) + ";"; @@ -243,30 +253,41 @@ int main(int argc, char** argv) break; case systemHandlerState::Disconnected : - std::cout << "SystemHandler:" << std::to_string(handlerInfo.systemID) << " at Disconnected " << std::endl; //debug - + std::cout << "(mavone.cpp:while) SystemHandler:" << std::to_string(handlerInfo.systemID) << " at Disconnected. " << std::endl; //debug - reset = true; //debug + // reset = true; //debug break; } } - + + + + // 刪除被輪尋到的 handlerInfo + // systemHandlerInfos.erase(std::remove_if(systemHandlerInfos.begin(), systemHandlerInfos.end(), [&](const auto& info) { + // return info.handlerState == systemHandlerState::Disconnected; + // }), systemHandlerInfos.end()); + } // When Progrem Terminate reset = true; - // make all thread join - for( auto& handlerInfo : systemHandlerInfos){ - handlerInfo.systemThread.join(); - } + 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(); + } + + // delete mavsdk; + // ~mavsdk; + return 0; } diff --git a/mavone/systemHandler.cpp b/mavone/systemHandler.cpp index 8275b5a..4f976ac 100644 --- a/mavone/systemHandler.cpp +++ b/mavone/systemHandler.cpp @@ -35,11 +35,18 @@ void systemHandler(System& system) { auto telemetry = Telemetry{system}; auto action = Action{system}; int sysid = static_cast(system.get_system_id()); - std::cout << "System " << sysid << " get in Thread. (debug)" << std::endl; // debug + std::cout << "(systemHandler.cpp:systemHandler) System " << sysid << " get in Thread. (debug)" << std::endl; // debug // Store telemetry information std::map telemetryInfo; + // 這些變數監控 handler 的行為 + gHandlerMask[sysid]["cutoff"] = 0; + gHandlerMask[sysid]["is_connected"] = 1; + bool reset = false; + + // + int _counter; // Put all the subscriber init in this section telemetry.subscribe_position([&system, &telemetryInfo](Telemetry::Position position) { @@ -125,28 +132,38 @@ void systemHandler(System& system) { } }); - // Wait Until telemetryInfo get something. + // Wait Until telemetryInfo get something. and if nothing get from System for 10 sec will cut off this connection. + _counter = 0; while(telemetryInfo.empty()){ sleep_for(seconds(1)); + if(_counter++ > 10) { + reset = true; + std::cout << "mark B " << sysid << " retry : " << _counter << std::endl; + break; + } } // Loop - while(system.is_connected()) { + while(system.is_connected() & !reset) { // Send Telemetry Data gTeleMtx.lock(); gTelemetryInfo[sysid] = telemetryInfo; gTeleMtx.unlock(); - // Deal Command + // Handler Command + reset = (gHandlerMask[sysid]["cutoff"] != 0); + + // Vehicle Command // for purpose sleep_for(milliseconds(100)); + } // Destroy - gVehicleCommand[sysid]["is_connected"] = 0; + gHandlerMask[sysid]["is_connected"] = 0; // Thread Terminate - std::cout << "Thread Out " << sysid << std::endl; //debug + std::cout << "(systemHandler.cpp:systemHandler) Thread Out " << sysid << std::endl; //debug }