From ce0fb7454aa830adb03fc23aaf938cbe7596ca27 Mon Sep 17 00:00:00 2001 From: chiyu1468 Date: Thu, 14 Dec 2023 00:49:06 +0800 Subject: [PATCH] The first connection success MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 第一版把 MAVSDK 的 Telemetry 接上 MySQL 資料庫 額外加了不少的 include 同時也在 CMakeLists 加了 mysqlcppconn 相依庫 (這是 mysql 的官方庫) 但是其中的設定一定會因為環境不同需要修正的 --- mavone/CMakeLists.txt | 5 + mavone/config.txt | 10 +- mavone/mavone.cpp | 511 +++++++++++++++++++++--------------------- 3 files changed, 268 insertions(+), 258 deletions(-) diff --git a/mavone/CMakeLists.txt b/mavone/CMakeLists.txt index 4ef0102..1331779 100644 --- a/mavone/CMakeLists.txt +++ b/mavone/CMakeLists.txt @@ -5,6 +5,10 @@ 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_executable(mavone mavone.cpp ) @@ -14,6 +18,7 @@ find_package(MAVSDK REQUIRED) target_link_libraries(mavone MAVSDK::mavsdk pthread + mysqlcppconn # for mysqlcppconn ) if(NOT MSVC) diff --git a/mavone/config.txt b/mavone/config.txt index c582ce3..c2daddb 100644 --- a/mavone/config.txt +++ b/mavone/config.txt @@ -1,8 +1,10 @@ -A=Hello World -B=42 C=3.14 connectPort=udp://:14550 +mysqlHost=140.120.108.238 +mysqlPort=1880 +mysqlUser=root +mysqlPW=wp21qpck57 +mysqlDatabase=Mav_one EOC -EOC 就是結尾了 所以這邊可以打註解 -目前只有 connectPort 有功能 \ No newline at end of file +EOC 就是結尾了 所以這邊可以打註解 \ No newline at end of file diff --git a/mavone/mavone.cpp b/mavone/mavone.cpp index 4235379..f89437b 100644 --- a/mavone/mavone.cpp +++ b/mavone/mavone.cpp @@ -2,43 +2,69 @@ // Simple example to demonstrate how takeoff and land using MAVSDK. // -#include -#include #include #include #include #include +#include #include #include #include +#include #include -#include #include +#include #include +#include #include +#include + +#include +#include +#include +#include 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; - int B; + 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 後 }; struct systemHandlerInfo { int systemID; std::thread systemThread; - + systemHandlerState handlerState; + int mysqlSN; }; + + // 我無法解決跨執行緒的 promise 變數生命週期問題 // 用最智障的全域變數 (砍掉 promise 結構簡單好多) -std::map> gTelemetryInfo; +std::map> gTelemetryInfo; +std::mutex gTeleMtx; + + +bool reset; int initializeParameters(const std::string& filePath, MavInitParameter& initSetting) { @@ -64,8 +90,16 @@ int initializeParameters(const std::string& filePath, MavInitParameter& initSett initSetting.connectPort = value; } else if (key == "connectPort2") { initSetting.connectPort2 = value; - } else if (key == "B") { - initSetting.B = std::stoi(value); + } else if (key == "mysqlHost") { + initSetting.mysqlHost = value; + } else if (key == "mysqlPort") { + 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); } @@ -92,79 +126,133 @@ void systemHandler(System& system) { std::cout << "System " << static_cast(system.get_system_id()) << " get in Thread. (debug)" << std::endl; // debug // Store telemetry information - std::map telemetryInfo; + std::map telemetryInfo; + // Put all the subscriber init in this section telemetry.subscribe_position([&system, &telemetryInfo](Telemetry::Position position) { - std::cout << "System ID: " << static_cast(system.get_system_id()) << " Altitude: " << position.relative_altitude_m << " m\n"; // debug + // 轉換數字到字串用的暫存變數 + std::ostringstream num_str_ss; + // std::cout << "System ID: " << static_cast(system.get_system_id()) << " Altitude: " << position.relative_altitude_m << " m\n"; // debug // Store telemetry information - telemetryInfo["altitude"] = position.relative_altitude_m; - gTelemetryInfo[static_cast(system.get_system_id())] = telemetryInfo; - }); + num_str_ss << std::fixed << std::setprecision(4) << position.relative_altitude_m; + telemetryInfo["drone_alt"] = num_str_ss.str(); + num_str_ss.str(""); - // Loop + 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(""); + }); - // Destroy + 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(""); + }); - sleep_for(seconds(5)); - // Thread Terminate - std::cout << "Thread Out " << static_cast(system.get_system_id()) << 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(""); -// class systemHandler { + num_str_ss << std::fixed << std::setprecision(4) << eulerAngle.pitch_deg; + telemetryInfo["drone_pitch"] = num_str_ss.str(); + num_str_ss.str(""); -// private: -// Telemetry telemetry; -// Action action; + 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; -// public: -// std::map telemetryInfo; -// std::map actionCommand; + // 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(""); + }); -// // Constructor -// systemHandler(System& system) { -// // Initialization -// telemetry = Telemetry{system}; -// action = Action{system}; -// std::cout << "System " << static_cast(system.get_system_id()) << " captured. (debug)" << std::endl; // debug + 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_position([&system, &telemetryInfo](Telemetry::Position position) { -// std::cout << "System ID: " << static_cast(system.get_system_id()) << " Altitude: " << position.relative_altitude_m << " m\n"; // debug + telemetry.subscribe_in_air([&system, &telemetryInfo](bool in_air) { + if(in_air){ + telemetryInfo["drone_air"] = "1"; + }else{ + telemetryInfo["drone_air"] = "0"; + } + }); -// // Store telemetry information -// telemetryInfo["altitude"] = position.relative_altitude_m; -// }); + 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[static_cast(system.get_system_id())] = telemetryInfo; + gTeleMtx.unlock(); + // Deal Command -// // Loop -// void operator()() { - -// std::cout << "Thread is running" << std::endl; -// } -// } + // for purpose + sleep_for(milliseconds(100)); + } + // Destroy + + + // Thread Terminate + std::cout << "Thread Out " << static_cast(system.get_system_id()) << std::endl; //debug +} int main(int argc, char** argv) { + reset = false; - // if (argc != 2) { - // std::cout << "Config File Needed! ex. config.txt" << std::endl; - // return 1; - // } + if (argc != 2) { + std::cout << "Config File Needed! ex. config.txt" << std::endl; + return 1; + } // Program Initial Read Setting from config file struct MavInitParameter initSetting; - // auto i = initializeParameters(argv[1], initSetting); - auto i = initializeParameters("config.txt", initSetting); + 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; } @@ -199,241 +287,156 @@ int main(int argc, char** argv) } // 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 : " << static_cast(new_system.get_system_id()) << "(Debug)"<< std::endl; // debug // Let handler progrem deal with System std::thread systemHandleThread([&new_system]() {systemHandler(new_system);}); + // Need some extra time fpr complete init + sleep_for(milliseconds(500)); + systemHandlerInfos.push_back(systemHandlerInfo{ static_cast(new_system.get_system_id()), std::move(systemHandleThread), - + systemHandlerState::Init, + -1 }); }); // 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 << "MySQL Connection Error: " << e.what() << std::endl; + return 1; + } // Main Loop is here // -- 將這個服務目前的狀態 更新到 mysql 中 // -- 從 systemHandlerInfos 中 輪巡所有 system 檢查並處理是否有需要丟到 mysql 的資料 // -- 從 mysql 抓到指令 丟到 systemHandlerInfo - - sleep_for(seconds(3)); - - std::cout << "mark A" << gTelemetryInfo[1]["altitude"] << std::endl; - - // make all thread join - for( auto& info : systemHandlerInfos){ - info.systemThread.join(); + int randomValue; + std::string sqlString; + std::map aTelemetryInfo; + + while(!reset){ + + sleep_for(seconds(1)); // debug + + // 每個 System 輪詢過去 + for( auto& handlerInfo : systemHandlerInfos){ + switch(handlerInfo.handlerState){ + case systemHandlerState::Init : + std::cout << "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, connect_state) 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) + ";"; + // 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; + break; + + case systemHandlerState::Prologue : + std::cout << "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; + } + + // MAVSDK 還沒收到第一筆 telemetry 就持續在這個狀態 + aTelemetryInfo = gTelemetryInfo[handlerInfo.systemID]; + if(aTelemetryInfo.size() == 0){ + break; + } + + // 切換到下一個狀態 + handlerInfo.handlerState = systemHandlerState::Ready; + + case systemHandlerState::Ready : + std::cout << "SystemHandler:" << std::to_string(handlerInfo.systemID) << " at Ready " << std::endl; //debug + + // 把 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] = {}; + } + + + // TODO: 把 mysql 的指令放給 MAVSDK 準備傳到無人機 + + + // reset = true; //debug + break; + } + + } + } -// ============================================================================== - - // mavsdk.subscribe_on_new_system([&mavsdk]() { - // auto system = mavsdk.systems().back(); - - - // if (system->has_autopilot()) { - // std::cout << "Discovered autopilot\n"; - // } - // }); - + // // dev del S - - - - // if (connection_result != ConnectionResult::Success) { - // std::cerr << "Connection failed: " << connection_result << '\n'; - // return 1; - // } - - // auto system = get_system(mavsdk); - // if (!system) { - // return 1; - // } - - - - // for (auto system : mavsdk.systems()) { - // std::cout << "(mavsdk.systems) get id : " << static_cast(system->get_system_id()) << std::endl; - // for (auto systemHandler : systemHandlers) { - // std::cout << systemHandler.systemID << " Mark B" << std::endl; - // if (systemHandler.systemID == static_cast(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 - // // while (telemetry.health_all_ok() != true) { - // // std::cout << "System ID: " << systemHandler.systemID << " Vehicle is getting ready to arm\n"; - // // sleep_for(seconds(1)); - // // } - - // // // Arm vehicle - // // std::cout << "System ID: " << systemHandler.systemID << " Arming...\n"; - // // const Action::Result arm_result = action.arm(); - - // // if (arm_result != Action::Result::Success) { - // // std::cerr << "System ID: " << systemHandler.systemID << " Arming failed: " << arm_result << '\n'; - // // return 1; - // // } - // // telemetrys.push_back(telemetry); - // sleep_for(seconds(5)); - // } else { - // std::cout << " Mark D " << std::endl; + // 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)); // } - // } - - - - - // 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(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(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)); - // } + // // dev del E - // // 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 + // When Progrem Terminate + reset = true; - // // 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"; + // make all thread join + for( auto& handlerInfo : systemHandlerInfos){ + handlerInfo.systemThread.join(); + } - // // 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"; + // MySQL Indicator + delete res; + delete stmt; + delete con; return 0; }