stable v0.1

mavone 是 專案主要程式
unit function dev 是 一些開發時的測試程式

虛擬機系統可以隨時連線與斷連了
把 mysql table schema 備份在 NodeRed_one.sql

mavone/NodeRed_one.sql

@@ -0,0 +1,61 @@
+-- MySQL dump 10.13  Distrib 8.0.34, for Win64 (x86_64)
+--
+-- Host: 140.120.108.238    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 utf8 */;
+/*!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` 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=43 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 2023-12-15  8:39:43

mavone/mavone.cpp

@@ -44,7 +44,8 @@ struct MavInitParameter {
 enum class systemHandlerState {
     Init,                       // 剛剛交給 systemHandler 去處理
     Prologue,                   // 建立了 mysql row 並取得 serialNo
-    Ready                       // 收到第一筆並放到 telemetryInfo 後
+    Ready,                      // 收到第一筆並放到 telemetryInfo 後
+    Disconnected                // mavsdk 判斷該系統斷線了
 };
 
 struct systemHandlerInfo {
@@ -61,6 +62,7 @@ struct systemHandlerInfo {
 // 我無法解決跨執行緒的 promise 變數生命週期問題
 // 用最智障的全域變數 (砍掉 promise 結構簡單好多)
 std::map<int,std::map<std::string,std::string>> gTelemetryInfo;
+std::map<int,std::map<std::string,double>> gVehicleCommand;
 std::mutex gTeleMtx;
 
 
@@ -86,7 +88,7 @@ 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") {
                 initSetting.connectPort2 = value;
@@ -123,7 +125,8 @@ 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
+    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; 
@@ -134,7 +137,7 @@ void systemHandler(System& system) {
         // 轉換數字到字串用的暫存變數
         std::ostringstream num_str_ss;
 
-        // std::cout << "System ID: " << static_cast<int>(system.get_system_id()) << " Altitude: " << position.relative_altitude_m << " m\n"; // debug
+        // 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();
@@ -223,7 +226,7 @@ void systemHandler(System& system) {
         
         // Send Telemetry Data 
         gTeleMtx.lock();
-        gTelemetryInfo[static_cast<int>(system.get_system_id())] = telemetryInfo;
+        gTelemetryInfo[sysid] = telemetryInfo;
         gTeleMtx.unlock();
 
         // Deal Command 
@@ -233,10 +236,10 @@ void systemHandler(System& system) {
     }
 
     // Destroy
-
+    gVehicleCommand[sysid]["is_connected"] = 0;
 
     // Thread Terminate
-    std::cout << "Thread Out " << static_cast<int>(system.get_system_id()) << std::endl; //debug
+    std::cout << "Thread Out " << sysid << std::endl; //debug
 }
 
 
@@ -273,6 +276,7 @@ int main(int argc, char** argv)
         // Get the last subscribed system 
         auto comingSystems = mavsdk.systems();
         std::shared_ptr<System> sys = comingSystems.back();
+        int sysid = static_cast<int>(sys->get_system_id());
         // Avoid Duplicate System ID (Component ID is careless, maybe determind by UUID will be better?)
         for(int i = 0;i < systemHandlerInfos.size();i++) {
             if(sys->get_system_id() == systemHandlerInfos[i].systemID){
@@ -280,23 +284,24 @@ int main(int argc, char** argv)
                 if(comingSystems.size() != 0){
                     sys = comingSystems.back();
                 } else {
-                    std::cout << "Duplicate System ID Collision : " << static_cast<int>(sys->get_system_id()) << std::endl;
+                    std::cout << "Duplicate System ID Collision : " << sysid << std::endl;
                     return;
                 }
             }
         }
         // make std::shared_ptr<MAVSDK::System> back to MAVSDK::System
         System& new_system = *sys;
-        // std::cout << "system detect : " << static_cast<int>(new_system.get_system_id()) << "(Debug)"<< std::endl; // debug
+        // std::cout << "system detect : " << sysid << "(Debug)"<< std::endl; // debug
 
         // Let handler progrem deal with System  
         std::thread systemHandleThread([&new_system]() {systemHandler(new_system);});
+        gVehicleCommand[sysid]["is_connected"] = 1;
 
         // Need some extra time fpr complete init
         sleep_for(milliseconds(500));
 
         systemHandlerInfos.push_back(systemHandlerInfo{
-            static_cast<int>(new_system.get_system_id()),
+            sysid,
             std::move(systemHandleThread),
             systemHandlerState::Init,
             -1
@@ -342,12 +347,12 @@ int main(int argc, char** argv)
                     // 用系統時間生成的隨機數 標記目前欄位
                     std::srand(std::time(0));
                     randomValue = std::rand() % 10000;
-                    sqlString = "INSERT INTO NodeRed_one (system_id, connect_state) VALUES (" + std::to_string(handlerInfo.systemID) + ", " + std::to_string(randomValue) + ");";
+                    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 connect_state = " +std::to_string(randomValue) + ";";
+                    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()) {
@@ -357,6 +362,10 @@ int main(int argc, char** argv)
 
                     // 切換到下一個狀態
                     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);
+
                     break;
 
                 case systemHandlerState::Prologue :
@@ -368,6 +377,15 @@ int main(int argc, char** argv)
                         std::cout << "handlerInfo.mysqlSN not aquired. Something goes wrong. Shutdown all service. " << std::endl;
                     }
 
+                    // 確認系統是否仍連線 
+                    if(gVehicleCommand[handlerInfo.systemID]["is_connected"] == 0){
+                        handlerInfo.handlerState = systemHandlerState::Disconnected;
+                        // 更新 mavsys_connect_status 欄位
+                        sqlString = "UPDATE NodeRed_one SET mavsys_connect_status = 'Disconnected' WHERE SerialNO = " + std::to_string(handlerInfo.mysqlSN) + ";";
+                        stmt->execute(sqlString);
+                        break;
+                    }
+                    
                     // MAVSDK 還沒收到第一筆 telemetry 就持續在這個狀態
                     aTelemetryInfo = gTelemetryInfo[handlerInfo.systemID];
                     if(aTelemetryInfo.size() == 0){
@@ -376,10 +394,22 @@ int main(int argc, char** argv)
 
                     // 切換到下一個狀態
                     handlerInfo.handlerState = systemHandlerState::Ready;
+                    // 更新 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(gVehicleCommand[handlerInfo.systemID]["is_connected"] == 0){
+                        handlerInfo.handlerState = systemHandlerState::Disconnected;
+                        // 更新 mavsys_connect_status 欄位
+                        sqlString = "UPDATE NodeRed_one SET mavsys_connect_status = 'Disconnected' WHERE SerialNO = " + std::to_string(handlerInfo.mysqlSN) + ";";
+                        stmt->execute(sqlString);
+                        break;
+                    }
+
                     // 把 MAVSDK 從無人機接到的 telemetry 放到 mysql
                     aTelemetryInfo = gTelemetryInfo[handlerInfo.systemID];
                     if(aTelemetryInfo.size() != 0) {
@@ -399,6 +429,13 @@ int main(int argc, char** argv)
                     // TODO: 把 mysql 的指令放給 MAVSDK 準備傳到無人機
 
 
+                    // reset = true; //debug
+                    break;
+
+                case systemHandlerState::Disconnected :
+                    std::cout << "SystemHandler:" << std::to_string(handlerInfo.systemID) << " at Disconnected " << std::endl; //debug
+                    
+                    
                     // reset = true; //debug
                     break;
             }