stable v0.4

- 修正 disconnected 後不斷重刷的問題
- 修正 相同 sysid 再連接會產生抓錯的問題
- 增加 非 autopilot 裝置被 subscribe telemetry 問題

mavone/globals.cpp

@@ -22,7 +22,11 @@ gVehicleCommand
 
 
 gHandlerMask    
+    這個變數是讓 主迴圈 得知 副迴圈 的連線狀態
+    gHandlerMask[sysid]["is_connected"]
 
+    這個變數是讓 主迴圈 通知 副迴圈 切斷Loop
+    gHandlerMask[sysid]["cutoff"]
 
 
 */

mavone/mavone.cpp

@@ -81,7 +81,6 @@ 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 
@@ -91,19 +90,21 @@ int main(int argc, char** argv)
 
         // Avoid Duplicate System ID 
         bool duplicated;
-        for(int i=0; i<comingSystems.size(); i++){
+        // 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;
+                    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
@@ -120,12 +121,10 @@ int main(int argc, char** argv)
                 });
 
                 // only get ONE system each time
-                break;
+                break; // this will break the i for loop
             }
         }
 
-        // Need some extra time fpr complete init
-        // sleep_for(milliseconds(500));
     });
 
     // MySQL connection is here
@@ -261,12 +260,17 @@ int main(int argc, char** argv)
             
         }
 
-
-
-        // 刪除被輪尋到的 handlerInfo
-        // systemHandlerInfos.erase(std::remove_if(systemHandlerInfos.begin(), systemHandlerInfos.end(), [&](const auto& info) {
-        //     return info.handlerState == systemHandlerState::Disconnected;
-        // }), systemHandlerInfos.end());
+        // 刪除 Disconnected 的 handlerInfo
+        for (auto it = systemHandlerInfos.begin(); it != systemHandlerInfos.end(); /* no increment here */) {
+            if (it->handlerState == systemHandlerState::Disconnected) {
+                if (it->systemThread.joinable()) {
+                    it->systemThread.join();
+                }
+                it = systemHandlerInfos.erase(it);
+            } else {
+                ++it;
+            }
+        }
 
     }
 
@@ -285,9 +289,6 @@ int main(int argc, char** argv)
         handlerInfo.systemThread.join();
     }
 
-    // delete mavsdk;
-    // ~mavsdk;
-
     return 0;
 }
 

mavone/mavone.h

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

mavone/systemHandler.cpp

@@ -48,7 +48,10 @@ void systemHandler(System& system) {
     //
     int _counter;
 
+    // std::cout << "Has AutoPilot : " << system.has_autopilot() << std::endl; // debug
+
     // Put all the subscriber init in this section
+    if (system.has_autopilot()) {
         telemetry.subscribe_position([&system, &telemetryInfo](Telemetry::Position position) {
             // 轉換數字到字串用的暫存變數
             std::ostringstream num_str_ss;
@@ -131,14 +134,14 @@ void systemHandler(System& system) {
                 telemetryInfo["drone_arm"] = "0";
             }
         });
-
+    }
     // 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;
+            std::cout << "(systemHandler.cpp:systemHandler) System " << sysid << " waiting too long without any coming info." << std::endl;
             break;
         }        
     }
@@ -161,9 +164,11 @@ void systemHandler(System& system) {
 
     }
 
-    // Destroy
-    gHandlerMask[sysid]["is_connected"] = 0;
+
 
     // Thread Terminate
     std::cout << "(systemHandler.cpp:systemHandler) Thread Out " << sysid << std::endl; //debug
+
+    // Destroy
+    gHandlerMask[sysid]["is_connected"] = 0;
 }