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,97 +48,100 @@ void systemHandler(System& system) {
     //
     int _counter;
 
-    // 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";
-        }
-    });
+    // 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;
+
+            // 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. 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;
 }