From 60c8a49b7e4c982cf9b9b75c85502dc6961474fd Mon Sep 17 00:00:00 2001 From: chiyu1468 Date: Fri, 29 Dec 2023 17:45:42 +0800 Subject: [PATCH] stable v0.2 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 沒有添加新功能，僅調整程式文件結構以利後續開發。 mavone.cpp => 處理主要迴圈與mysql連接 systemHandler.cpp => 處理 mavsdk_server 迴圈 globals.cpp => 宣告全域變數 --- mavone/CMakeLists.txt | 5 ++ mavone/globals.cpp | 6 +- mavone/mavone.cpp | 172 +++------------------------------------ mavone/systemHandler.cpp | 152 ++++++++++++++++++++++++++++++++++ mavone/systemHandler.h | 7 ++ 5 files changed, 180 insertions(+), 162 deletions(-) create mode 100644 mavone/systemHandler.cpp create mode 100644 mavone/systemHandler.h diff --git a/mavone/CMakeLists.txt b/mavone/CMakeLists.txt index dda87ec..9eea34f 100644 --- a/mavone/CMakeLists.txt +++ b/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) diff --git a/mavone/globals.cpp b/mavone/globals.cpp index e5f04dc..6d84fd6 100644 --- a/mavone/globals.cpp +++ b/mavone/globals.cpp @@ -1,6 +1,6 @@ #include "globals.h" -extern std::map> gTelemetryInfo; -extern std::map> gVehicleCommand; -extern std::mutex gTeleMtx; \ No newline at end of file +std::map> gTelemetryInfo; +std::map> gVehicleCommand; +std::mutex gTeleMtx; \ No newline at end of file diff --git a/mavone/mavone.cpp b/mavone/mavone.cpp index d57bae8..00c0155 100644 --- a/mavone/mavone.cpp +++ b/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(system.get_system_id()); - std::cout << "System " << sysid << " get in Thread. (debug)" << std::endl; // debug - - // Store telemetry information - std::map 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; diff --git a/mavone/systemHandler.cpp b/mavone/systemHandler.cpp new file mode 100644 index 0000000..8275b5a --- /dev/null +++ b/mavone/systemHandler.cpp @@ -0,0 +1,152 @@ + + + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#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(system.get_system_id()); + std::cout << "System " << sysid << " get in Thread. (debug)" << std::endl; // debug + + // Store telemetry information + std::map 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 +} diff --git a/mavone/systemHandler.h b/mavone/systemHandler.h new file mode 100644 index 0000000..4be05df --- /dev/null +++ b/mavone/systemHandler.h @@ -0,0 +1,7 @@ +#ifndef SYSTEMHANDLER_H +#define SYSTEMHANDLER_H + + +void systemHandler(System& system); + +#endif