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

The first connection success

Browse Source 
第一版把 MAVSDK 的 Telemetry 接上 MySQL 資料庫
額外加了不少的 include
同時也在 CMakeLists 加了 mysqlcppconn 相依庫 (這是 mysql 的官方庫)
但是其中的設定一定會因為環境不同需要修正的
This commit is contained in:
chiyu1468
2023-12-14 00:49:06 +08:00
parent 872eacb529
commit ce0fb7454a
3 changed files with 275 additions and 265 deletions
Download Patch File Download Diff File Expand all files Collapse all files
mavone/CMakeLists.txt
+5
View File
@@ -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)
mavone/config.txt
+6 -4
View File
@@ -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 有功能
EOC 就是結尾了 所以這邊可以打註解
mavone/mavone.cpp
+264 -261
View File
@@ -2,43 +2,69 @@
// Simple example to demonstrate how takeoff and land using MAVSDK.
//
#include <chrono>
#include <cstdint>
#include <mavsdk/mavsdk.h>
#include <mavsdk/plugins/action/action.h>
#include <mavsdk/plugins/telemetry/telemetry.h>
#include <mavsdk/plugins/mavlink_passthrough/mavlink_passthrough.h>
#include <cstdint>
#include <iostream>
#include <fstream>
#include <sstream>
#include <iomanip>
#include <future>
#include <memory>
#include <thread>
#include <chrono>
#include <vector>
#include <mutex>
#include <map>
#include <memory>
#include <mysql_driver.h>
#include <mysql_connection.h>
#include <cppconn/resultset.h>
#include <cppconn/statement.h>
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<int,std::map<std::string,float>> gTelemetryInfo;
std::map<int,std::map<std::string,std::string>> 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<int>(system.get_system_id()) << " get in Thread. (debug)" << std::endl; // debug
// Store telemetry information
std::map<std::string,float> telemetryInfo;
std::map<std::string,std::string> telemetryInfo;
// Put all the subscriber init in this section
telemetry.subscribe_position([&system, &telemetryInfo](Telemetry::Position position) {
std::cout << "System ID: " << static_cast<int>(system.get_system_id()) << " Altitude: " << position.relative_altitude_m << " m\n"; // debug
// 轉換數字到字串用的暫存變數
std::ostringstream num_str_ss;
// std::cout << "System ID: " << static_cast<int>(system.get_system_id()) << " Altitude: " << position.relative_altitude_m << " m\n"; // debug
// Store telemetry information
telemetryInfo["altitude"] = position.relative_altitude_m;
gTelemetryInfo[static_cast<int>(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("");
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("");
});
// Loop
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[static_cast<int>(system.get_system_id())] = telemetryInfo;
gTeleMtx.unlock();
// Deal Command
// for purpose
sleep_for(milliseconds(100));
}
// Destroy
sleep_for(seconds(5));
// Thread Terminate
std::cout << "Thread Out " << static_cast<int>(system.get_system_id()) << std::endl;
std::cout << "Thread Out " << static_cast<int>(system.get_system_id()) << std::endl; //debug
}
// class systemHandler {
// private:
// Telemetry telemetry;
// Action action;
// public:
// std::map<std::string,float> telemetryInfo;
// std::map<std::string,float> actionCommand;
// // Constructor
// systemHandler(System& system) {
// // Initialization
// telemetry = Telemetry{system};
// action = Action{system};
// std::cout << "System " << static_cast<int>(system.get_system_id()) << " captured. (debug)" << std::endl; // debug
// telemetry.subscribe_position([&system, &telemetryInfo](Telemetry::Position position) {
// std::cout << "System ID: " << static_cast<int>(system.get_system_id()) << " Altitude: " << position.relative_altitude_m << " m\n"; // debug
// // Store telemetry information
// telemetryInfo["altitude"] = position.relative_altitude_m;
// });
// }
// // Loop
// void operator()() {
// std::cout << "Thread is running" << std::endl;
// }
// }
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<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 : " << static_cast<int>(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<int>(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
int randomValue;
std::string sqlString;
std::map<std::string,std::string> aTelemetryInfo;
sleep_for(seconds(3));
while(!reset){
std::cout << "mark A" << gTelemetryInfo[1]["altitude"] << std::endl;
sleep_for(seconds(1)); // debug
// make all thread join
for( auto& info : systemHandlerInfos){
info.systemThread.join();
// 每個 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;
}
}
}
// ==============================================================================
// // dev del S
// mavsdk.subscribe_on_new_system([&mavsdk]() {
// auto system = mavsdk.systems().back();
// if (system->has_autopilot()) {
// std::cout << "Discovered autopilot\n";
// }
// });
// 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<int>(system->get_system_id()) << std::endl;
// for (auto systemHandler : systemHandlers) {
// std::cout << systemHandler.systemID << " Mark B" << std::endl;
// if (systemHandler.systemID == static_cast<int>(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;
// }
// }
// }
// 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<int>(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<int>(system->get_system_id())
// << ", connected: " << (system->is_connected() ? "yes" : "no") << '\n';
// sleep_for(seconds(1));
// }
// 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));
// }
// // 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
// // dev del E
// // 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;
// }
// When Progrem Terminate
reset = true;
// // 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;
}