chiyu1468
/
MAVSDK_Dev
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

stable v0.1

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

虛擬機系統可以隨時連線與斷連了
把 mysql table schema 備份在 NodeRed_one.sql
master
chiyu1468 2 years ago
parent ce0fb7454a
commit efe5bc4e3c
5 changed files with 110 additions and 12 deletions
Show Stats Download Patch File Download Diff File

61
mavone/NodeRed_one.sql
Unescape Escape View File

@ -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

61
mavone/mavone.cpp
Unescape Escape View File

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

0
mavlinkpassth.cpp → unit function dev/mavlinkpassth.cpp
Unescape Escape View File

0
mavsdkTest.py → unit function dev/mavsdkTest.py
Unescape Escape View File

0
takeoff and land.cpp → unit function dev/takeoff and land.cpp
Unescape Escape View File

Write Preview
Loading…
Cancel
Save