stable v0.3

修正 system id 重複獲取問題
修正 handlerState 卡在 Prologue 問題

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()
+
+
+
+
+
+

mavone/globals.cpp

@@ -1,6 +1,35 @@
 
 #include "globals.h"
+#include <array>
+
+/*
+用簡單粗暴的全域變數讓主迴圈跟副迴圈傳遞資料
+另外加個 互斥鎖(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<int, std::map<std::string, std::string>> gTelemetryInfo;
-std::map<int, std::map<std::string, double>> gVehicleCommand;
+std::map<int, std::map<std::string, std::array<double, 2>>> gVehicleCommand;
+std::map<int, std::map<std::string, int>> gHandlerMask;
 std::mutex gTeleMtx;

mavone/globals.h

@@ -7,7 +7,8 @@
 #include <mutex>
 
 extern std::map<int, std::map<std::string, std::string>> gTelemetryInfo;
-extern std::map<int, std::map<std::string, double>> gVehicleCommand;
+extern std::map<int, std::map<std::string, std::array<double, 2>>> gVehicleCommand;
+extern std::map<int, std::map<std::string, int>> gHandlerMask;
 extern std::mutex gTeleMtx;
 
 #endif

mavone/mavone.cpp

@@ -81,34 +81,36 @@ int main(int argc, char** argv)
     // 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();
+        std::shared_ptr<System> sys;
-        int sysid = static_cast<int>(sys->get_system_id());
+        int sysid;
-        // Avoid Duplicate System ID (Component ID is careless, maybe determind by UUID will be better?)
+
-        for(int i = 0;i < systemHandlerInfos.size();i++) {
+        // Avoid Duplicate System ID 
-            if(sys->get_system_id() == systemHandlerInfos[i].systemID){
+        bool duplicated;
-                comingSystems.pop_back();
+        for(int i=0; i<comingSystems.size(); i++){
-                if(comingSystems.size() != 0){
+            sys = comingSystems[i];
-                    sys = comingSystems.back();
+            sysid = static_cast<int>(sys->get_system_id());
-                } else {
+            duplicated = false;
-                    std::cout << "(mavone.cpp:main) Duplicate System ID Collision : " << sysid << std::endl;
+            for(int j=0; j<systemHandlerInfos.size(); j++){
-                    return;
+                if(sysid == systemHandlerInfos[j].systemID){
+                    duplicated = true;
+                    break;
                 }
             }
-        }
+
-        // make std::shared_ptr<MAVSDK::System> back to MAVSDK::System
+            if(duplicated == false){
-        System& new_system = *sys;
+
-        // std::cout << "system detect : " << sysid << "(Debug)"<< std::endl; // debug
+                // 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);});
-        gVehicleCommand[sysid]["is_connected"] = 1;
+                // gHandlerMask[sysid]["is_connected"] = 1;
-
-        // Need some extra time fpr complete init
-        sleep_for(milliseconds(500));
 
                 systemHandlerInfos.push_back(systemHandlerInfo{
                     sysid,
@@ -117,6 +119,13 @@ int main(int argc, char** argv)
                     -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<std::string,std::string> aTelemetryInfo;
 
-    while(!reset){
+    while (!reset) {
 
         sleep_for(seconds(1)); // debug
 
         // 每個 System 輪詢過去
-        for( auto& handlerInfo : systemHandlerInfos){
+        for ( auto& handlerInfo : systemHandlerInfos) {
-            switch(handlerInfo.handlerState){
+            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
+    std::cout << "(mavone.cpp:main) Main Loop End !!!!!!" << std::endl;
-    for( auto& handlerInfo : systemHandlerInfos){
-        handlerInfo.systemThread.join();
-    }
 
     // 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;
 }
 

mavone/systemHandler.cpp

@@ -35,11 +35,18 @@ void systemHandler(System& system) {
     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
+    std::cout << "(systemHandler.cpp:systemHandler) System " << sysid << " get in Thread. (debug)" << std::endl; // debug
 
     // Store telemetry information
     std::map<std::string,std::string> 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
 }