stable v0.2

沒有添加新功能，僅調整程式文件結構以利後續開發。

mavone.cpp => 處理主要迴圈與mysql連接
systemHandler.cpp => 處理 mavsdk_server 迴圈
globals.cpp => 宣告全域變數

mavone/CMakeLists.txt

@@ -8,6 +8,10 @@ project(mavone)
 include_directories(/usr/include/mysql-cppconn-8/jdbc) # for mysqlcppconn
 link_directories(/usr/lib/x86_64-linux-gnu) # for mysqlcppconn
 
+add_library(mavoneSys_lib 
+    systemHandler.cpp 
+    globals.cpp
+)
 
 add_executable(mavone
     mavone.cpp
@@ -20,6 +24,7 @@ target_link_libraries(mavone
     MAVSDK::mavsdk
     pthread
     mysqlcppconn # for mysqlcppconn
+    mavoneSys_lib # for the lib i created
 )
 
 if(NOT MSVC)

mavone/globals.cpp

@@ -1,6 +1,6 @@
 
 #include "globals.h"
 
-extern std::map<int, std::map<std::string, std::string>> gTelemetryInfo;
+std::map<int, std::map<std::string, std::string>> gTelemetryInfo;
-extern std::map<int, std::map<std::string, double>> gVehicleCommand;
+std::map<int, std::map<std::string, double>> gVehicleCommand;
-extern std::mutex gTeleMtx;
+std::mutex gTeleMtx;

mavone/mavone.cpp

@@ -1,12 +1,11 @@
 
 #include "mavone.h"
+#include "systemHandler.h"
 
 // 我無法解決跨執行緒的 promise 變數生命週期問題
 // 用最智障的全域變數 (砍掉 promise 結構簡單好多)
 #include "globals.h"
 
-
-
 int initializeParameters(const std::string& filePath, MavInitParameter& initSetting) {
     std::ifstream configFile(filePath);
     if (!configFile) {
@@ -28,8 +27,6 @@ int initializeParameters(const std::string& filePath, MavInitParameter& initSett
             // Check if the key matches any field in MavInitParameter
             if (key == "MAVSDKListeningPort") {
                 initSetting.connectPort = value;
-            } else if (key == "connectPort2") {
-                initSetting.connectPort2 = value;
             } else if (key == "mysqlHost") {
                 initSetting.mysqlHost = value;
             } else if (key == "mysqlPort") {
@@ -47,146 +44,21 @@ int initializeParameters(const std::string& filePath, MavInitParameter& initSett
         parameters.push_back(line); // Add the parameter to the vector
     }
     configFile.close();
-    std::cout << "Parameters read successfully!" << std::endl;
+    std::cout << "(mavone.cpp:initializeParameters) Parameters read successfully!" << std::endl;
     // Print all the parameters
     for (const auto& parameter : parameters) {
-        std::cout << "Line: " << parameter << std::endl;
+        std::cout << "(mavone.cpp:initializeParameters) Line: " << parameter << std::endl;
     }
     return 0;
 }
 
-/*
- * Important multi-threaded functions, 
- * handling system initialization, receiving messages and sending messages.
- */
-void systemHandler(System& system) {
-    // Initialization
-    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
-
-    // Store telemetry information
-    std::map<std::string,std::string> telemetryInfo; 
-
-
-    // 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";
-        }
-    });
-
-    // Wait Until telemetryInfo get something.
-    while(telemetryInfo.empty()){
-        sleep_for(seconds(1));
-    }
-    
-    // Loop
-    while(system.is_connected() && !reset) {
-        
-        // Send Telemetry Data 
-        gTeleMtx.lock();
-        gTelemetryInfo[sysid] = telemetryInfo;
-        gTeleMtx.unlock();
-
-        // Deal Command 
-
-        // for purpose
-        sleep_for(milliseconds(100));
-    }
-
-    // Destroy
-    gVehicleCommand[sysid]["is_connected"] = 0;
-
-    // Thread Terminate
-    std::cout << "Thread Out " << sysid << std::endl; //debug
-}
-
 
 int main(int argc, char** argv)
 {
     reset = false;
 
     if (argc != 2) {
-        std::cout << "Config File Needed! ex. config.txt" << std::endl;
+        std::cout << "(mavone.cpp:main) Config File Needed! ex. config.txt " << std::endl;
         return 1;
     }
 
@@ -195,14 +67,14 @@ int main(int argc, char** argv)
     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;
+        std::cerr << "(mavone.cpp:main) Failed to open config file: " << argv[1] << std::endl;
     }
 
     // Start MAVSDK Server and connect to it.
     Mavsdk mavsdk;
     ConnectionResult connection_result = mavsdk.add_any_connection(initSetting.connectPort);
     if (connection_result != ConnectionResult::Success) {
-        std::cerr << "Connection failed: " << connection_result << '\n';
+        std::cerr << "(mavone.cpp:main) Connection failed: " << connection_result << '\n';
         return 1;
     }
 
@@ -222,7 +94,7 @@ int main(int argc, char** argv)
                 if(comingSystems.size() != 0){
                     sys = comingSystems.back();
                 } else {
-                    std::cout << "Duplicate System ID Collision : " << sysid << std::endl;
+                    std::cout << "(mavone.cpp:main) Duplicate System ID Collision : " << sysid << std::endl;
                     return;
                 }
             }
@@ -259,7 +131,7 @@ int main(int argc, char** argv)
         stmt = con->createStatement();
         
     } catch (sql::SQLException &e) {
-        std::cout << "MySQL Connection Error: " << e.what() << std::endl;
+        std::cout << "(mavone.cpp:main) MySQL Connection Error: " << e.what() << std::endl;
         return 1;
     }
 
@@ -280,7 +152,7 @@ int main(int argc, char** argv)
         for( auto& handlerInfo : systemHandlerInfos){
             switch(handlerInfo.handlerState){
                 case systemHandlerState::Init :
-                    std::cout << "SystemHandler:" << std::to_string(handlerInfo.systemID) << " at Init " << std::endl; //debug
+                    std::cout << "(mavone.cpp:main) SystemHandler:" << std::to_string(handlerInfo.systemID) << " at Init " << std::endl; //debug
 
                     // 用系統時間生成的隨機數 標記目前欄位
                     std::srand(std::time(0));
@@ -307,12 +179,12 @@ int main(int argc, char** argv)
                     break;
 
                 case systemHandlerState::Prologue :
-                    std::cout << "SystemHandler:" << std::to_string(handlerInfo.systemID) << " at Prologue " << std::endl; //debug
+                    std::cout << "(mavone.cpp:main) SystemHandler:" << std::to_string(handlerInfo.systemID) << " at Prologue " << std::endl; //debug
 
                     // 確認之前的 handlerInfo.mysqlSN 有正常被取得  若沒有整個程式即將關閉
                     if(handlerInfo.mysqlSN == -1) {
                         reset = true;
-                        std::cout << "handlerInfo.mysqlSN not aquired. Something goes wrong. Shutdown all service. " << std::endl;
+                        std::cout << "(mavone.cpp:main) handlerInfo.mysqlSN not aquired. Fatal Error. Shutdown all service. " << std::endl;
                     }
 
                     // 確認系統是否仍連線 
@@ -363,7 +235,7 @@ int main(int argc, char** argv)
                         gTelemetryInfo[handlerInfo.systemID] = {};
                     }
 
-
+                    
                     // TODO: 把 mysql 的指令放給 MAVSDK 準備傳到無人機
 
 
@@ -382,24 +254,6 @@ int main(int argc, char** argv)
         
     }
 
-    // // dev del S
-
-    //     sleep_for(seconds(3));
-    //     std::cout << "connect system number: " << gTelemetryInfo.size() << std::endl;
-    //     for(int t=0;t<10;t++){
-    //         for(int i = 0; i < gTelemetryInfo.size()-1 ; i++){
-    //             auto& info = gTelemetryInfo[i];
-    //             std::cout << "Sysid : " << i+1 << " ,Alt : " << info["drone_alt"] << " ,Lon : " << info["vehicle_lon"] << " ,Lat : " << info["vehicle_lat"] << std::endl;
-    //         }
-    //         sleep_for(seconds(1));
-    //     }
-
-    // // dev del E
-
-
-
-
-
     // When Progrem Terminate
     reset = true;
 
@@ -408,7 +262,7 @@ int main(int argc, char** argv)
         handlerInfo.systemThread.join();
     }
 
-    // MySQL Indicator
+    // terminate MySQL Indicator
     delete res;
     delete stmt;
     delete con;

mavone/systemHandler.cpp

@@ -0,0 +1,152 @@
+
+
+
+#include <mavsdk/mavsdk.h>
+#include <mavsdk/plugins/action/action.h>
+#include <mavsdk/plugins/telemetry/telemetry.h>
+#include <mavsdk/plugins/mavlink_passthrough/mavlink_passthrough.h>
+#include <cstdint>
+#include <iostream>
+#include <fstream>
+#include <sstream>
+#include <iomanip>
+#include <future>
+#include <thread>
+#include <chrono>
+#include <vector>
+#include <mutex>
+#include <map>
+#include <memory>
+
+
+#include "globals.h"
+
+using namespace mavsdk;
+using std::chrono::seconds;
+using std::chrono::milliseconds;
+using std::this_thread::sleep_for;
+
+/*
+ * Important multi-threaded functions, 
+ * handling system initialization, receiving messages and sending messages.
+ */
+void systemHandler(System& system) {
+    // Initialization
+    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
+
+    // Store telemetry information
+    std::map<std::string,std::string> telemetryInfo; 
+
+
+    // 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";
+        }
+    });
+
+    // Wait Until telemetryInfo get something.
+    while(telemetryInfo.empty()){
+        sleep_for(seconds(1));
+    }
+    
+    // Loop
+    while(system.is_connected()) {
+        
+        // Send Telemetry Data 
+        gTeleMtx.lock();
+        gTelemetryInfo[sysid] = telemetryInfo;
+        gTeleMtx.unlock();
+
+        // Deal Command 
+
+        // for purpose
+        sleep_for(milliseconds(100));
+    }
+
+    // Destroy
+    gVehicleCommand[sysid]["is_connected"] = 0;
+
+    // Thread Terminate
+    std::cout << "Thread Out " << sysid << std::endl; //debug
+}

mavone/systemHandler.h

@@ -0,0 +1,7 @@
+#ifndef SYSTEMHANDLER_H
+#define SYSTEMHANDLER_H
+
+
+void systemHandler(System& system);
+
+#endif