stable v0.42

mavone.cpp
    - 修正 status 管理 bug : 還未改變資料庫狀態 就被程式的管理列表刪除
    - 新增 指定本服務於 MAVLink 上的 sysid=197, compid=254 (應該是不會特別去動態調整所以硬寫在裡面)

mavone/CMakeLists.txt

@@ -5,15 +5,26 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON)
 
 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
 )
 
-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
     pthread
+    mysqlcppconn # for mysqlcppconn
+    mavoneSys_lib # for the lib i created
 )
 
 if(NOT MSVC)
@@ -21,3 +32,9 @@ if(NOT MSVC)
 else()
     add_compile_options(mavone PRIVATE -WX -W2)
 endif()
+
+
+
+
+
+

mavone/NodeRed_one.sql

@@ -0,0 +1,62 @@
+-- MySQL dump 10.13  Distrib 8.0.36, for Linux (x86_64)
+--
+-- Host: 172.17.0.2    Database: Mav_one
+-- ------------------------------------------------------
+-- Server version	8.0.29
+
+/*!40101 SET @OLD_CHARACTER_SET_CLIENT=@@CHARACTER_SET_CLIENT */;
+/*!40101 SET @OLD_CHARACTER_SET_RESULTS=@@CHARACTER_SET_RESULTS */;
+/*!40101 SET @OLD_COLLATION_CONNECTION=@@COLLATION_CONNECTION */;
+/*!50503 SET NAMES utf8mb4 */;
+/*!40103 SET @OLD_TIME_ZONE=@@TIME_ZONE */;
+/*!40103 SET TIME_ZONE='+00:00' */;
+/*!40014 SET @OLD_UNIQUE_CHECKS=@@UNIQUE_CHECKS, UNIQUE_CHECKS=0 */;
+/*!40014 SET @OLD_FOREIGN_KEY_CHECKS=@@FOREIGN_KEY_CHECKS, FOREIGN_KEY_CHECKS=0 */;
+/*!40101 SET @OLD_SQL_MODE=@@SQL_MODE, SQL_MODE='NO_AUTO_VALUE_ON_ZERO' */;
+/*!40111 SET @OLD_SQL_NOTES=@@SQL_NOTES, SQL_NOTES=0 */;
+
+--
+-- Table structure for table `NodeRed_one`
+--
+
+DROP TABLE IF EXISTS `NodeRed_one`;
+/*!40101 SET @saved_cs_client     = @@character_set_client */;
+/*!50503 SET character_set_client = utf8mb4 */;
+CREATE TABLE `NodeRed_one` (
+  `SerialNo` int NOT NULL AUTO_INCREMENT,
+  `create_time` datetime DEFAULT CURRENT_TIMESTAMP,
+  `update_time` datetime DEFAULT CURRENT_TIMESTAMP ON UPDATE CURRENT_TIMESTAMP,
+  `mavsys_connect_status` varchar(40) DEFAULT NULL,
+  `system_id` int NOT NULL,
+  `vehicle_type` varchar(10) DEFAULT NULL,
+  `vehicle_mode` varchar(10) DEFAULT NULL,
+  `vehicle_lat` double DEFAULT NULL,
+  `vehicle_lon` double DEFAULT NULL,
+  `vehicle_head` float DEFAULT NULL,
+  `vehicle_bat_volt` float DEFAULT NULL,
+  `vehicle_bat_perc` float DEFAULT NULL,
+  `vehicle_speed` float DEFAULT NULL,
+  `drone_alt` float DEFAULT NULL,
+  `drone_air` tinyint DEFAULT NULL,
+  `drone_arm` tinyint DEFAULT NULL,
+  `drone_pitch` float DEFAULT NULL,
+  `drone_roll` float DEFAULT NULL,
+  `drone_yaw` float DEFAULT NULL,
+  `StatusText` varchar(45) DEFAULT NULL,
+  `command_type` varchar(10) DEFAULT NULL,
+  `command_para1` double DEFAULT NULL,
+  `command_para2` double DEFAULT NULL,
+  PRIMARY KEY (`SerialNo`)
+) ENGINE=InnoDB AUTO_INCREMENT=118 DEFAULT CHARSET=utf8mb4 COLLATE=utf8mb4_0900_ai_ci;
+/*!40101 SET character_set_client = @saved_cs_client */;
+/*!40103 SET TIME_ZONE=@OLD_TIME_ZONE */;
+
+/*!40101 SET SQL_MODE=@OLD_SQL_MODE */;
+/*!40014 SET FOREIGN_KEY_CHECKS=@OLD_FOREIGN_KEY_CHECKS */;
+/*!40014 SET UNIQUE_CHECKS=@OLD_UNIQUE_CHECKS */;
+/*!40101 SET CHARACTER_SET_CLIENT=@OLD_CHARACTER_SET_CLIENT */;
+/*!40101 SET CHARACTER_SET_RESULTS=@OLD_CHARACTER_SET_RESULTS */;
+/*!40101 SET COLLATION_CONNECTION=@OLD_COLLATION_CONNECTION */;
+/*!40111 SET SQL_NOTES=@OLD_SQL_NOTES */;
+
+-- Dump completed on 2024-02-02 15:33:46

mavone/config.txt

@@ -1,8 +1,10 @@
-A=Hello World
+MAVSDKListeningPort=udp://:14550
-B=42
+mysqlHost=140.120.108.238
+mysqlPort=1880
+mysqlUser=root
+mysqlPW=wp21qpck57
+mysqlDatabase=Mav_one
 C=3.14
-connectPort=udp://:14550
 EOC
 
 EOC 就是結尾了 所以這邊可以打註解
-目前只有 connectPort 有功能

mavone/globals.cpp

@@ -0,0 +1,39 @@
+
+#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    
+    這個變數是讓 主迴圈 得知 副迴圈 的連線狀態
+    gHandlerMask[sysid]["is_connected"]
+
+    這個變數是讓 主迴圈 通知 副迴圈 切斷Loop
+    gHandlerMask[sysid]["cutoff"]
+
+
+*/
+
+
+
+std::map<int, std::map<std::string, std::string>> gTelemetryInfo;
+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

@@ -0,0 +1,14 @@
+// globals.h
+#ifndef GLOBALS_H
+#define GLOBALS_H
+
+#include <map>
+#include <string>
+#include <mutex>
+
+extern std::map<int, std::map<std::string, std::string>> gTelemetryInfo;
+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

@@ -1,45 +1,10 @@
-//
-// Simple example to demonstrate how takeoff and land using MAVSDK.
-//
-
-#include <chrono>
-#include <cstdint>
-#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 <iostream>
-#include <fstream>
-#include <sstream>
-#include <future>
-#include <memory>
-#include <thread>
-#include <vector>
-#include <map>
-
-using namespace mavsdk;
-using std::chrono::seconds;
-using std::this_thread::sleep_for;
-
-struct MavInitParameter {
-    std::string connectPort;
-    std::string connectPort2;
-    int B;
-    float C;
-};
-
-struct systemHandlerInfo {
-    int systemID;
-    std::thread systemThread;
-
-
-};
 
+#include "mavone.h"
+#include "systemHandler.h"
 
 // 我無法解決跨執行緒的 promise 變數生命週期問題
 // 用最智障的全域變數 (砍掉 promise 結構簡單好多)
-std::map<int,std::map<std::string,float>> gTelemetryInfo;
+#include "globals.h"
-
 
 int initializeParameters(const std::string& filePath, MavInitParameter& initSetting) {
     std::ifstream configFile(filePath);
@@ -60,12 +25,18 @@ int initializeParameters(const std::string& filePath, MavInitParameter& initSett
             std::string value = line.substr(delimiterPos + 1);
 
             // Check if the key matches any field in MavInitParameter
-            if (key == "connectPort") {
+            if (key == "MAVSDKListeningPort") {
                 initSetting.connectPort = value;
-            } else if (key == "connectPort2") {
+            } else if (key == "mysqlHost") {
-                initSetting.connectPort2 = value;
+                initSetting.mysqlHost = value;
-            } else if (key == "B") {
+            } else if (key == "mysqlPort") {
-                initSetting.B = std::stoi(value);
+                initSetting.mysqlPort = value;
+            } else if (key == "mysqlUser") {
+                initSetting.mysqlUser = value;
+            } else if (key == "mysqlPW") {
+                initSetting.mysqlPW = value;    
+            } else if (key == "mysqlDatabase") {
+                initSetting.mysqlDatabase = value;                                                            
             } else if (key == "C") {
                 initSetting.C = std::stof(value);
             }
@@ -73,107 +44,40 @@ 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};
-    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
-
-        // Store telemetry information
-        telemetryInfo["altitude"] = position.relative_altitude_m;
-        gTelemetryInfo[static_cast<int>(system.get_system_id())] = telemetryInfo;
-    });
-
-    // Loop
-
-
-    // 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)
 {
+    reset = false;
 
-    // if (argc != 2) {
+    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;
+        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);
+    // 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;
+    // Set this MAVLINK service has system id 197, component id 254, NOT send heartbeat
+    mavsdk::Mavsdk::Configuration mavserver_configuration(197,254,false);
+    mavsdk.set_configuration(mavserver_configuration);
     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;
     }
 
@@ -184,256 +88,211 @@ int main(int argc, char** argv)
     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;
-        // Avoid Duplicate System ID (Component ID is careless, maybe determind by UUID will be better?)
+        int sysid;
-        for(int i = 0;i < systemHandlerInfos.size();i++) {
+
-            if(sys->get_system_id() == systemHandlerInfos[i].systemID){
+        // Avoid Duplicate System ID 
-                comingSystems.pop_back();
+        bool duplicated;
-                if(comingSystems.size() != 0){
+        // for(int i=0; i<comingSystems.size(); i++){
-                    sys = comingSystems.back();
+        for(int i=comingSystems.size()-1; i>=0; i--){
-                } else {
+            sys = comingSystems[i];
-                    std::cout << "Duplicate System ID Collision : " << static_cast<int>(sys->get_system_id()) << std::endl;
+            if (!sys->is_connected()) {continue;}
-                    return;
+            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; // this will break the j for loop
                 }
             }
-        }
+
-        // make std::shared_ptr<MAVSDK::System> back to MAVSDK::System
+            if(duplicated == false){
-        System& new_system = *sys;
+                // Get New System                
-        std::cout << "system detect : " << static_cast<int>(new_system.get_system_id()) << "(Debug)"<< std::endl; // debug
+                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);});
+                // gHandlerMask[sysid]["is_connected"] = 1;
 
                 systemHandlerInfos.push_back(systemHandlerInfo{
-            static_cast<int>(new_system.get_system_id()),
+                    sysid,
                     std::move(systemHandleThread),
-
+                    systemHandlerState::Init,
+                    -1
                 });
 
+                // only get ONE system each time
+                break; // this will break the i for loop
+            }
+        }
+
     });
 
     // MySQL connection is here
+    sql::mysql::MySQL_Driver *driver;
+    sql::Connection *con;
+    sql::Statement *stmt;
+    sql::ResultSet *res;
+    try {
+        driver = sql::mysql::get_mysql_driver_instance();
+        con = driver->connect("tcp://" + initSetting.mysqlHost + ":" + initSetting.mysqlPort, initSetting.mysqlUser, initSetting.mysqlPW);
+        con->setSchema(initSetting.mysqlDatabase);
+        stmt = con->createStatement();
         
+    } catch (sql::SQLException &e) {
+        std::cout << "(mavone.cpp:main) MySQL Connection Error: " << e.what() << std::endl;
+        return 1;
+    }
 
     // Main Loop is here
     //   -- 將這個服務目前的狀態 更新到 mysql 中
     //   -- 從 systemHandlerInfos 中 輪巡所有 system 檢查並處理是否有需要丟到 mysql 的資料
     //   -- 從 mysql 抓到指令 丟到 systemHandlerInfo
 
+    int randomValue;
+    std::string sqlString;
+    std::map<std::string,std::string> aTelemetryInfo;
 
-    sleep_for(seconds(3));
+    while (!reset) {
 
-    std::cout << "mark A" << gTelemetryInfo[1]["altitude"] << std::endl;
+        sleep_for(seconds(1)); // debug
 
-    // make all thread join
+        // 每個 System 輪詢過去
-    for( auto& info : systemHandlerInfos){
+        for ( auto& handlerInfo : systemHandlerInfos) {
-        info.systemThread.join();
+            switch (handlerInfo.handlerState) {
+                case systemHandlerState::Init :
+                    std::cout << "(mavone.cpp:while) SystemHandler:" << std::to_string(handlerInfo.systemID) << " at Init " << std::endl; //debug
+
+                    // 用系統時間生成的隨機數 標記目前欄位
+                    std::srand(std::time(0));
+                    randomValue = std::rand() % 10000;
+                    sqlString = "INSERT INTO NodeRed_one (system_id, mavsys_connect_status) VALUES (" + std::to_string(handlerInfo.systemID) + ", " + std::to_string(randomValue) + ");";
+                    // std::cout << "MySQL Insert Query: " << sqlString << std::endl; //debug
+                    stmt->execute(sqlString);
+
+                    // 取得對應的欄位序號 SerialNO
+                    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()) {
+                        // std::cout << "Query Reselt: " << res->getString(1) << std::endl;  //debug
+                        handlerInfo.mysqlSN = std::stof(res->getString(1));
                     }
 
-// ==============================================================================
+                    // 切換到下一個狀態
+                    handlerInfo.handlerState = systemHandlerState::Prologue;
+                    // 更新 mavsys_connect_status 欄位
+                    sqlString = "UPDATE NodeRed_one SET mavsys_connect_status = 'Prologue' WHERE SerialNO = " + std::to_string(handlerInfo.mysqlSN) + ";";
+                    stmt->execute(sqlString);
 
-    // mavsdk.subscribe_on_new_system([&mavsdk]() {
+                    break;
-    //     auto system = mavsdk.systems().back();
+
+                case systemHandlerState::Prologue :
+                    std::cout << "(mavone.cpp:while) SystemHandler:" << std::to_string(handlerInfo.systemID) << " at Prologue " << std::endl; //debug
+
+                    // 確認之前的 handlerInfo.mysqlSN 有正常被取得  若沒有整個程式即將關閉
+                    if (handlerInfo.mysqlSN == -1) {
+                        reset = true;
+                        std::cout << "(mavone.cpp:while) handlerInfo.mysqlSN not aquired. Fatal Error. Shutdown all service. " << std::endl;
+                    }
+
+                    // 確認系統是否仍連線 
+                    if (gHandlerMask[handlerInfo.systemID]["is_connected"] == 0) {
+                        handlerInfo.handlerState = systemHandlerState::Disconnected;
+                        break;
+                    }
+                    
+                    // MAVSDK 還沒收到第一筆 telemetry 就持續在這個狀態
+                    aTelemetryInfo = gTelemetryInfo[handlerInfo.systemID];
+                    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);
+
+                case systemHandlerState::Ready :
+                    // std::cout << "SystemHandler:" << std::to_string(handlerInfo.systemID) << " at Ready " << std::endl; //debug
+
+                    // 確認系統是否仍連線 
+                    if (gHandlerMask[handlerInfo.systemID]["is_connected"] == 0) {
+                        handlerInfo.handlerState = systemHandlerState::Disconnected;
+                        break;
+                    }
+
+                    // 把 MAVSDK 從無人機接到的 telemetry 放到 mysql
+                    aTelemetryInfo = gTelemetryInfo[handlerInfo.systemID];
+                    if(aTelemetryInfo.size() != 0) {
+                        sqlString = "UPDATE NodeRed_one SET ";
+                        for(const auto& pair : aTelemetryInfo) {
+                            // std::cout << "Key: " << pair.first << ", Value: " << pair.second << std::endl; //debug
+                            sqlString += pair.first + " = '" + pair.second + "',";
+                        }
+                        sqlString.pop_back();
+                        sqlString += " WHERE SerialNO = " + std::to_string(handlerInfo.mysqlSN) + ";";
+                        // std::cout << "MySQL UPDATE Query: " << sqlString << std::endl; //debug
+                        stmt->execute(sqlString);
+                        gTelemetryInfo[handlerInfo.systemID] = {};
+                    }
 
                     
-    //     if (system->has_autopilot()) {
+                    // TODO: 把 mysql 的指令放給 MAVSDK 準備傳到無人機
-    //         std::cout << "Discovered autopilot\n";
-    //     }
-    // });
 
 
+                    // reset = true; //debug
+                    break;
 
+                case systemHandlerState::Disconnected :
+                    std::cout << "(mavone.cpp:while) SystemHandler:" << std::to_string(handlerInfo.systemID) << " at Disconnected. " << std::endl; //debug
                     
+                    // 更新 mavsys_connect_status 欄位
+                    sqlString = "UPDATE NodeRed_one SET mavsys_connect_status = 'Disconnected' WHERE SerialNO = " + std::to_string(handlerInfo.mysqlSN) + ";";
+                    stmt->execute(sqlString);
 
-    // if (connection_result != ConnectionResult::Success) {
+                    // 變更狀態
-    //     std::cerr << "Connection failed: " << connection_result << '\n';
+                    handlerInfo.handlerState = systemHandlerState::Terminate;
-    //     return 1;
-    // }
 
-    // auto system = get_system(mavsdk);
+                    // reset = true; //debug
-    // if (!system) {
+                    break;
-    //     return 1;
+            }
-    // }
             
+        }
 
+        // 刪除 Disconnected 的 handlerInfo
+        for (auto it = systemHandlerInfos.begin(); it != systemHandlerInfos.end(); /* no increment here */) {
+            if (it->handlerState == systemHandlerState::Terminate) {
+                // 這個 if 如果不加 則副程序在結束以前就會被主程序放生 執行時會出現 SIGABRT 錯誤
+                if (it->systemThread.joinable()) {
+                    it->systemThread.join();
+                }
+                it = systemHandlerInfos.erase(it);
+            } else {
+                ++it;
+            }
+        }
 
-    // for (auto system : mavsdk.systems()) {
+    }
-    //     std::cout << "(mavsdk.systems) get id : " << static_cast<int>(system->get_system_id()) << std::endl;
-    //     for (auto systemHandler : systemHandlers) {
-    //         std::cout << systemHandler.systemID << " Mark B" << std::endl;
-    //         if (systemHandler.systemID == static_cast<int>(system->get_system_id())) {
-    //             std::cout << "Mark C" << std::endl;
-    //             // telemetry = Telemetry{system};
-    //             const auto set_rate_result = telemetry.set_rate_position(1);
-    //             if (set_rate_result != Telemetry::Result::Success) {
-    //                 std::cerr << "Setting rate failed: " << set_rate_result << '\n';
-    //                 return 1;
-    //             }
-    //             telemetry.subscribe_position([systemHandler](Telemetry::Position position) {
-    //                 std::cout << "System ID: " << systemHandler.systemID << " Altitude: " << position.relative_altitude_m << " m\n";
-    //             });
 
-    //             // // Check until vehicle is ready to arm
+    // When Progrem Terminate
-    //             // while (telemetry.health_all_ok() != true) {
+    reset = true;
-    //             //     std::cout << "System ID: " << systemHandler.systemID << " Vehicle is getting ready to arm\n";
-    //             //     sleep_for(seconds(1));
-    //             // }
 
-    //             // // Arm vehicle
+    std::cout << "(mavone.cpp:main) Main Loop End !!!!!!" << std::endl;
-    //             // std::cout << "System ID: " << systemHandler.systemID << " Arming...\n";
-    //             // const Action::Result arm_result = action.arm();
 
-    //             // if (arm_result != Action::Result::Success) {
+    // terminate MySQL Indicator
-    //             //     std::cerr << "System ID: " << systemHandler.systemID << " Arming failed: " << arm_result << '\n';
+    delete res;
-    //             //     return 1;
+    delete stmt;
-    //             // }
+    delete con;
-    //             // telemetrys.push_back(telemetry);
-    //             sleep_for(seconds(5));
-    //         } else {
-    //             std::cout << " Mark D " << std::endl;
-    //         }
-    //     }
-    // }
 
-
+    // make all thread join
-
+    for( auto& handlerInfo : systemHandlerInfos){
-
+        handlerInfo.systemThread.join();
-    // std::cout << typeid().name() << std::endl;
+    }
-
-
-    // return 1;
-
-    // Instantiate plugins.
-    // auto telemetry = Telemetry{systemsList[0]};
-    // auto telemetry1 = Telemetry{systemsList[1]};
-
-    // // Instantiate plugins.
-    // auto telemetry = Telemetry{system};
-    // auto action = Action{system};
-
-    // // Instantiate mavlink_passthrough plugin.
-    // auto mavlink_passthrough = MavlinkPassthrough{system};
-
-    // auto x = static_cast<int>(system->get_system_id());
-    // std::cout << "System ID : " << x << '\n';
-
-    // System::AutopilotVersion y = system->get_autopilot_version_data();
-    // std::cout << "MAV_PROTOCOL_CAPABILITY : " << y.capabilities << '\n';
-    // std::cout << "Flight Stack Version : " << y.flight_sw_version << '\n';
-
-    // // Check vehicle mode
-    // const Telemetry::FlightMode mode_result = telemetry.flight_mode();
-    // std::cerr << "Vehicle Mode: " << mode_result << '\n';
-    // // return 1; // debug
-
-    // // Set the flight mode to "AUTO". use SET_MODE (#11) in MAVLink Messages.
-    // mavlink_message_t message;
-
-
-    // // mavlink_msg_set_mode_pack(mavlink_passthrough.get_our_sysid(),
-    // //                           mavlink_passthrough.get_our_compid(),
-    // //                           &message,
-    // //                           system->get_system_id(),
-    // //                           MAV_COMP_ID_AUTOPILOT1,
-    // //                           4);
-
-    // mavlink_command_long_t command;
-    // command.target_system = system->get_system_id();
-    // command.target_component = MAV_COMP_ID_AUTOPILOT1;
-    // command.command = MAV_CMD_DO_SET_MODE;
-    // command.param1 = MAV_MODE_AUTO_ARMED;
-    // mavlink_msg_command_long_encode(mavlink_passthrough.get_our_sysid(),
-    //                                 mavlink_passthrough.get_our_compid(),
-    //                                 &message,
-    //                                 &command);
-
-    // mavlink_passthrough.send_message(message);
-
-
-    // return 1; // debug
-
-    // We want to listen to the altitude of the drone at 1 Hz.
-    // const auto set_rate_result = telemetry0.set_rate_position(1);
-    // if (set_rate_result != Telemetry::Result::Success) {
-    //     std::cerr << "Setting rate failed: " << set_rate_result << '\n';
-    //     return 1;
-    // }
-    // const auto set_rate_result = telemetry1.set_rate_position(1);
-    // if (set_rate_result != Telemetry::Result::Success) {
-    //     std::cerr << "Setting rate failed: " << set_rate_result << '\n';
-    //     return 1;
-    // }
-
-    
-
-    // Set up callback to monitor altitude while the vehicle is in flight
-    // auto x = systemsList[0]->get_system_id();
-    // telemetry.subscribe_position([x](Telemetry::Position position) {
-    //     std::cout << "System ID: " << x << "Altitude: " << position.relative_altitude_m << " m\n";
-    // });
-
-    // std::cout << "Mark C" << std::endl;
-
-    // for(int i = 0; i < 5; ++i) {
-    //     sleep_for(seconds(5));
-
-    //     std::cout << "Number of discovered systems: " << systemsList.size() << std::endl;
-
-    //     for (auto system : mavsdk.systems()) {
-    //         std::cout << "Found system with system ID: " << static_cast<int>(system->get_system_id())
-    //           << ", connected: " << (system->is_connected() ? "yes" : "no") << '\n';
-    //     }
-
-    //     std::cout << "link check " << i << " time. \n";
-    // }
-
-    // // Check until vehicle is ready to arm
-    // while (telemetry.health_all_ok() != true) {
-    //     std::cout << "Vehicle is getting ready to arm\n";
-    //     sleep_for(seconds(1));
-    // }
-
-    // // Arm vehicle
-    // std::cout << "Arming...\n";
-    // const Action::Result arm_result = action.arm();
-
-    // if (arm_result != Action::Result::Success) {
-    //     std::cerr << "Arming failed: " << arm_result << '\n';
-    //     return 1;
-    // }
-
-    // return 1; // debug
-
-
-
-    // // Take off
-    // std::cout << "Taking off...\n";
-    // const Action::Result takeoff_result = action.takeoff();
-    // if (takeoff_result != Action::Result::Success) {
-    //     std::cerr << "Takeoff failed: " << takeoff_result << '\n';
-    //     return 1;
-    // }
-
-    // Let it hover for a bit before landing again.
-    sleep_for(seconds(5));
-
-    // std::cout << "Landing...\n";
-    // const Action::Result land_result = action.land();
-    // if (land_result != Action::Result::Success) {
-    //     std::cerr << "Land failed: " << land_result << '\n';
-    //     return 1;
-    // }
-
-    // // Check if vehicle is still in air
-    // while (telemetry.in_air()) {
-    //     std::cout << "Vehicle is landing...\n";
-    //     sleep_for(seconds(1));
-    // }
-    // std::cout << "Landed!\n";
-
-    // // We are relying on auto-disarming but let's keep watching the telemetry for a bit longer.
-    // sleep_for(seconds(3));
-    // std::cout << "Finished...\n";
 
     return 0;
 }

mavone/mavone.h

@@ -0,0 +1,64 @@
+#ifndef MAVONE_H
+#define MAVONE_H
+
+
+#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 <mysql_driver.h>
+#include <mysql_connection.h>
+#include <cppconn/resultset.h>
+#include <cppconn/statement.h>
+
+
+using namespace mavsdk;
+using std::chrono::seconds;
+using std::chrono::milliseconds;
+using std::this_thread::sleep_for;
+
+struct MavInitParameter {
+    std::string connectPort;
+    std::string connectPort2;
+
+    float C;
+    std::string mysqlHost;
+    std::string mysqlPort;
+    std::string mysqlUser;
+    std::string mysqlPW;
+    std::string mysqlDatabase;
+};
+
+enum class systemHandlerState {
+    Init,                       // 剛剛交給 systemHandler 去處理
+    Prologue,                   // 建立了 mysql row 並取得 serialNo
+    Ready,                      // 收到第一筆並放到 telemetryInfo 後
+    Disconnected,               // mavsdk 判斷該系統斷線了
+    Terminate,                  // 準備要從管理列表中刪除
+    // Void
+};
+
+struct systemHandlerInfo {
+    int systemID;
+    std::thread systemThread;
+    systemHandlerState handlerState;
+    int mysqlSN;
+
+};
+
+bool reset;
+
+#endif

mavone/systemHandler.cpp

@@ -0,0 +1,195 @@
+
+
+
+#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 << "(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;
+
+    // std::cout << "Has AutoPilot : " << system.has_autopilot() << std::endl; // debug
+
+    // Put all the subscriber init in this section
+    if (system.has_autopilot()) {
+
+        auto set_rate_result = telemetry.set_rate_position(1.0);
+        if (set_rate_result != mavsdk::Telemetry::Result::Success) {
+            std::cout << "(systemHandler.cpp:systemHandler) Setting position rate failed:" << set_rate_result << std::endl;
+        }
+        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("");
+        });
+
+        // set_rate_result = telemetry.set_rate_attitude_euler(1.0);  // only in v2.0
+        // if (set_rate_result != mavsdk::Telemetry::Result::Success) {
+        //     std::cout << "(systemHandler.cpp:systemHandler) Setting euler rate failed:" << set_rate_result << std::endl;
+        // }
+        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("");
+        });
+
+        set_rate_result = telemetry.set_rate_battery(1.0);
+        if (set_rate_result != mavsdk::Telemetry::Result::Success) {
+            std::cout << "(systemHandler.cpp:systemHandler) Setting battery rate failed:" << set_rate_result << std::endl;
+        }
+        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_perc"] = num_str_ss.str();
+            num_str_ss.str("");
+            num_str_ss << std::fixed << std::setprecision(4) << battery.voltage_v;
+            telemetryInfo["vehicle_bat_volt"] = num_str_ss.str();
+            num_str_ss.str("");
+        });
+
+        set_rate_result = telemetry.set_rate_in_air(1.0);
+        if (set_rate_result != mavsdk::Telemetry::Result::Success) {
+            std::cout << "(systemHandler.cpp:systemHandler) Setting in_air rate failed:" << set_rate_result << std::endl;
+        }
+        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 << "(systemHandler.cpp:systemHandler) System " << sysid << " waiting too long without any coming info." << std::endl;
+            break;
+        }        
+    }
+    
+    // Loop
+    while(system.is_connected() & !reset) {
+        
+        // Send Telemetry Data 
+        gTeleMtx.lock();
+        gTelemetryInfo[sysid] = telemetryInfo;
+        gTeleMtx.unlock();
+
+        // Handler Command 
+        reset = (gHandlerMask[sysid]["cutoff"] != 0);
+
+        // Vehicle Command
+
+        // for purpose
+        sleep_for(milliseconds(100));
+
+    }
+
+
+
+    // Thread Terminate
+    std::cout << "(systemHandler.cpp:systemHandler) Thread Out " << sysid << std::endl; //debug
+
+    // Destroy
+    gHandlerMask[sysid]["is_connected"] = 0;
+}

mavone/systemHandler.h

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