From efe5bc4e3c24cc118436ed9e401533dda02750a0 Mon Sep 17 00:00:00 2001 From: chiyu1468 Date: Fri, 15 Dec 2023 09:37:55 +0800 Subject: [PATCH] stable v0.1 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit mavone 是 專案主要程式 unit function dev 是 一些開發時的測試程式 虛擬機系統可以隨時連線與斷連了 把 mysql table schema 備份在 NodeRed_one.sql --- mavone/NodeRed_one.sql | 61 +++++++++++++++++++ mavone/mavone.cpp | 61 +++++++++++++++---- .../mavlinkpassth.cpp | 0 .../mavsdkTest.py | 0 .../takeoff and land.cpp | 0 5 files changed, 110 insertions(+), 12 deletions(-) create mode 100644 mavone/NodeRed_one.sql rename mavlinkpassth.cpp => unit function dev/mavlinkpassth.cpp (100%) rename mavsdkTest.py => unit function dev/mavsdkTest.py (100%) rename takeoff and land.cpp => unit function dev/takeoff and land.cpp (100%) diff --git a/mavone/NodeRed_one.sql b/mavone/NodeRed_one.sql new file mode 100644 index 0000000..c6cc9ec --- /dev/null +++ b/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 diff --git a/mavone/mavone.cpp b/mavone/mavone.cpp index f89437b..6b5d03c 100644 --- a/mavone/mavone.cpp +++ b/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> gTelemetryInfo; +std::map> 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(system.get_system_id()) << " get in Thread. (debug)" << std::endl; // debug + int sysid = static_cast(system.get_system_id()); + std::cout << "System " << sysid << " get in Thread. (debug)" << std::endl; // debug // Store telemetry information std::map telemetryInfo; @@ -134,7 +137,7 @@ void systemHandler(System& system) { // 轉換數字到字串用的暫存變數 std::ostringstream num_str_ss; - // std::cout << "System ID: " << static_cast(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(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(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 sys = comingSystems.back(); + int sysid = static_cast(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(sys->get_system_id()) << std::endl; + std::cout << "Duplicate System ID Collision : " << sysid << std::endl; return; } } } // make std::shared_ptr back to MAVSDK::System System& new_system = *sys; - // std::cout << "system detect : " << static_cast(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(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 : @@ -367,6 +376,15 @@ int main(int argc, char** argv) reset = true; 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]; @@ -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; } diff --git a/mavlinkpassth.cpp b/unit function dev/mavlinkpassth.cpp similarity index 100% rename from mavlinkpassth.cpp rename to unit function dev/mavlinkpassth.cpp diff --git a/mavsdkTest.py b/unit function dev/mavsdkTest.py similarity index 100% rename from mavsdkTest.py rename to unit function dev/mavsdkTest.py diff --git a/takeoff and land.cpp b/unit function dev/takeoff and land.cpp similarity index 100% rename from takeoff and land.cpp rename to unit function dev/takeoff and land.cpp