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

stable v0.2

Browse Source 
沒有添加新功能，僅調整程式文件結構以利後續開發。

mavone.cpp => 處理主要迴圈與mysql連接
systemHandler.cpp => 處理 mavsdk_server 迴圈
globals.cpp => 宣告全域變數
This commit is contained in:
chiyu1468
2023-12-29 17:55:28 +08:00
parent 57588d5026
commit 60c8a49b7e
5 changed files with 180 additions and 162 deletions
Download Patch File Download Diff File Expand all files Collapse all files
mavone/CMakeLists.txt
+5
View File
@@ -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)
mavone/globals.cpp
+3 -3
View File
@@ -1,6 +1,6 @@
#include "globals.h"
extern std::map<int, std::map<std::string, std::string>> gTelemetryInfo;
extern std::map<int, std::map<std::string, double>> gVehicleCommand;
extern std::mutex gTeleMtx;
std::map<int, std::map<std::string, std::string>> gTelemetryInfo;
std::map<int, std::map<std::string, double>> gVehicleCommand;
std::mutex gTeleMtx;
mavone/mavone.cpp
+13 -159
View File
@@ -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<int>(system.get_system_id());
std::cout << "System " << sysid << " get in Thread. (debug)" << std::endl; // debug
// Store telemetry information
std::map<std::string,std::string> 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;
mavone/systemHandler.cpp
+152
View File
@@ -0,0 +1,152 @@
#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 <thread>
#include <chrono>
#include <vector>
#include <mutex>
#include <map>
#include <memory>
#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<int>(system.get_system_id());
std::cout << "System " << sysid << " get in Thread. (debug)" << std::endl; // debug
// Store telemetry information
std::map<std::string,std::string> 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
}
mavone/systemHandler.h
+7
View File
@@ -0,0 +1,7 @@
#ifndef SYSTEMHANDLER_H
#define SYSTEMHANDLER_H
void systemHandler(System& system);
#endif