GlobalVariable Pass Telemetry

這版捨棄 future promise 傳遞 改用全域變數傳遞資料

mavone/mavone.cpp

@@ -15,6 +15,7 @@
 #include <memory>
 #include <thread>
 #include <vector>
+#include <map>
 
 using namespace mavsdk;
 using std::chrono::seconds;
@@ -30,11 +31,16 @@ struct MavInitParameter {
 struct systemHandlerInfo {
     int systemID;
     std::thread systemThread;
-    
+
 
 };
 
 
+// 我無法解決跨執行緒的 promise 變數生命週期問題
+// 用最智障的全域變數 (砍掉 promise 結構簡單好多)
+std::map<int,std::map<std::string,float>> gTelemetryInfo;
+
+
 int initializeParameters(const std::string& filePath, MavInitParameter& initSetting) {
     std::ifstream configFile(filePath);
     if (!configFile) {
@@ -79,38 +85,86 @@ 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) {
     // Initialization
     auto telemetry = Telemetry{system};
     auto action = Action{system};
-    std::cout << "System " << static_cast<int>(system.get_system_id()) << " get in Thread.(debug)" << std::endl; // debug
-    telemetry.subscribe_position([&system](Telemetry::Position position) {
+    std::cout << "System " << static_cast<int>(system.get_system_id()) << " get in Thread. (debug)" << std::endl; // debug
+
+    // Store telemetry information
+    std::map<std::string,float> telemetryInfo; 
+
+    // Put all the subscriber init in this section
+    telemetry.subscribe_position([&system, &telemetryInfo](Telemetry::Position position) {
         std::cout << "System ID: " << static_cast<int>(system.get_system_id()) << " Altitude: " << position.relative_altitude_m << " m\n"; // debug
-        // Feed To mysql
+
+        // Store telemetry information
+        telemetryInfo["altitude"] = position.relative_altitude_m;
+        gTelemetryInfo[static_cast<int>(system.get_system_id())] = telemetryInfo;
     });
 
+    // Loop
 
-    sleep_for(seconds(5)); // debug
 
+    // Destroy
+
+
+    sleep_for(seconds(5));
+    // Thread Terminate
     std::cout << "Thread Out " << static_cast<int>(system.get_system_id()) << std::endl;
 }
 
 
+// class systemHandler {
+
+//     private:
+//         Telemetry telemetry;
+//         Action action;
+
+
+//     public:
+//         std::map<std::string,float> telemetryInfo;
+//         std::map<std::string,float> actionCommand;
+
+//         // Constructor
+//         systemHandler(System& system) {
+//             // Initialization
+//             telemetry = Telemetry{system};
+//             action = Action{system};
+//             std::cout << "System " << static_cast<int>(system.get_system_id()) << " captured. (debug)" << std::endl; // debug
+
+
+//             telemetry.subscribe_position([&system, &telemetryInfo](Telemetry::Position position) {
+//                 std::cout << "System ID: " << static_cast<int>(system.get_system_id()) << " Altitude: " << position.relative_altitude_m << " m\n"; // debug
+
+//                 // Store telemetry information
+//                 telemetryInfo["altitude"] = position.relative_altitude_m;
+//             });
+
+//         }
+
+
+//         // Loop 
+//         void operator()() {
+            
+//             std::cout << "Thread is running" << 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;
     }
@@ -147,18 +201,15 @@ int main(int argc, char** argv)
         System& new_system = *sys;
         std::cout << "system detect : " << static_cast<int>(new_system.get_system_id()) << "(Debug)"<< std::endl; // debug
 
-        // 
-        // std::map<string, float> telemetryInfo;
-        auto telemetryInfo_pro = std::promise<std::map<string, float>>{};
-        auto telemetryInfo_fut = telemetryInfo_pro.get_future();
-
         // Let handler progrem deal with System  
         std::thread systemHandleThread([&new_system]() {systemHandler(new_system);});
 
         systemHandlerInfos.push_back(systemHandlerInfo{
             static_cast<int>(new_system.get_system_id()),
-            std::move(systemHandleThread)
+            std::move(systemHandleThread),
+
         });
+
     });
 
     // MySQL connection is here
@@ -170,9 +221,14 @@ int main(int argc, char** argv)
     //   -- 從 mysql 抓到指令 丟到 systemHandlerInfo
 
 
+    sleep_for(seconds(3));
 
+    std::cout << "mark A" << gTelemetryInfo[1]["altitude"] << std::endl;
 
-
+    // make all thread join
+    for( auto& info : systemHandlerInfos){
+        info.systemThread.join();
+    }
 
 // ==============================================================================
 
@@ -359,7 +415,7 @@ int main(int argc, char** argv)
     // }
 
     // Let it hover for a bit before landing again.
-    sleep_for(seconds(15));
+    sleep_for(seconds(5));
 
     // std::cout << "Landing...\n";
     // const Action::Result land_result = action.land();