|
|
|
|
@ -15,6 +15,7 @@
|
|
|
|
|
#include <memory>
|
|
|
|
|
#include <thread>
|
|
|
|
|
#include <vector>
|
|
|
|
|
#include <map>
|
|
|
|
|
|
|
|
|
|
using namespace mavsdk;
|
|
|
|
|
using std::chrono::seconds;
|
|
|
|
|
@ -30,11 +31,16 @@ struct MavInitParameter {
|
|
|
|
|
struct systemHandlerInfo {
|
|
|
|
|
int systemID;
|
|
|
|
|
std::thread systemThread;
|
|
|
|
|
|
|
|
|
|
std::future<std::map<std::string, float>> telemetryInfo_fut;
|
|
|
|
|
|
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
int initializeParameters(const std::string& filePath, MavInitParameter& initSetting) {
|
|
|
|
|
std::ifstream configFile(filePath);
|
|
|
|
|
if (!configFile) {
|
|
|
|
|
@ -79,42 +85,78 @@ int initializeParameters(const std::string& filePath, MavInitParameter& initSett
|
|
|
|
|
* Important multi-threaded functions,
|
|
|
|
|
* handling system initialization, receiving messages and sending messages.
|
|
|
|
|
*/
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void systemHandler(System& system){
|
|
|
|
|
|
|
|
|
|
void systemHandler(System& system, std::promise<std::map<std::string, float>>& telemetryInfo_pro) {
|
|
|
|
|
// Initialization
|
|
|
|
|
auto telemetry = Telemetry{system};
|
|
|
|
|
auto action = Action{system};
|
|
|
|
|
std::cout << "System " << static_cast<int>(system.get_system_id()) << " get in Thread.(debug)" << std::endl; // debug
|
|
|
|
|
telemetry.subscribe_position([&system](Telemetry::Position position) {
|
|
|
|
|
|
|
|
|
|
// Store telemetry information
|
|
|
|
|
std::map<std::string, float> telemetryInfo;
|
|
|
|
|
|
|
|
|
|
// dev del S
|
|
|
|
|
std::cout << "mark tA" << std::endl;
|
|
|
|
|
std::future<std::map<std::string, float>> telemetryInfo_future = telemetryInfo_pro.get_future(); // dev del
|
|
|
|
|
|
|
|
|
|
std::cout << "mark tB" << std::endl;
|
|
|
|
|
if (!telemetryInfo_future.valid()) {
|
|
|
|
|
std::cout << "telemetryInfo_pro is not valid" << std::endl;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
telemetryInfo["test"] = 3.14;
|
|
|
|
|
telemetryInfo_pro.set_value(telemetryInfo);
|
|
|
|
|
std::cout << "mark tC" << std::endl;
|
|
|
|
|
if (!telemetryInfo_future.valid()) {
|
|
|
|
|
std::cout << "telemetryInfo_pro is not valid" << std::endl;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// dev del E
|
|
|
|
|
|
|
|
|
|
telemetry.subscribe_position([&system, &telemetryInfo, &telemetryInfo_pro](Telemetry::Position position) {
|
|
|
|
|
std::cout << "System ID: " << static_cast<int>(system.get_system_id()) << " Altitude: " << position.relative_altitude_m << " m\n"; // debug
|
|
|
|
|
// Feed To mysql
|
|
|
|
|
});
|
|
|
|
|
|
|
|
|
|
// Store telemetry information
|
|
|
|
|
telemetryInfo["altitude"] = position.relative_altitude_m;
|
|
|
|
|
// Add more telemetry information as needed
|
|
|
|
|
|
|
|
|
|
// Set the telemetry information in the promise
|
|
|
|
|
std::cout << "mark tsA" << std::endl;
|
|
|
|
|
telemetryInfo_pro.set_value(telemetryInfo);
|
|
|
|
|
std::cout << "mark tsB" << std::endl;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
sleep_for(seconds(1)); // debug
|
|
|
|
|
});
|
|
|
|
|
|
|
|
|
|
sleep_for(seconds(5)); // debug
|
|
|
|
|
|
|
|
|
|
std::cout << "Thread Out " << static_cast<int>(system.get_system_id()) << std::endl;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
int main(int argc, char** argv)
|
|
|
|
|
{
|
|
|
|
|
|
|
|
|
|
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(argv[1], initSetting);
|
|
|
|
|
auto i = initializeParameters("config.txt", initSetting);
|
|
|
|
|
if (i == 1){
|
|
|
|
|
std::cerr << "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);
|
|
|
|
|
@ -149,15 +191,32 @@ int main(int argc, char** argv)
|
|
|
|
|
|
|
|
|
|
//
|
|
|
|
|
// std::map<string, float> telemetryInfo;
|
|
|
|
|
auto telemetryInfo_pro = std::promise<std::map<string, float>>{};
|
|
|
|
|
auto telemetryInfo_fut = telemetryInfo_pro.get_future();
|
|
|
|
|
auto telemetryInfo_pro = std::promise<std::map<std::string, float>>{};
|
|
|
|
|
// auto telemetryInfo_fut = telemetryInfo_pro.get_future();
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// dev del S
|
|
|
|
|
|
|
|
|
|
auto telemetryInfo_pro1 = std::promise<std::map<std::string, float>>{};
|
|
|
|
|
auto telemetryInfo_fut1 = telemetryInfo_pro.get_future();
|
|
|
|
|
|
|
|
|
|
std::thread worker([&telemetryInfo_pro1]() {
|
|
|
|
|
// 執行任務並將結果回傳至 promise
|
|
|
|
|
std::map<std::string, float> result;
|
|
|
|
|
result["test"]=3.14;
|
|
|
|
|
telemetryInfo_pro1.set_value(result);
|
|
|
|
|
});
|
|
|
|
|
worker.join();
|
|
|
|
|
// dev del E
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// Let handler progrem deal with System
|
|
|
|
|
std::thread systemHandleThread([&new_system]() {systemHandler(new_system);});
|
|
|
|
|
std::thread systemHandleThread([&new_system, &telemetryInfo_pro]() {systemHandler(new_system, telemetryInfo_pro);});
|
|
|
|
|
|
|
|
|
|
systemHandlerInfos.push_back(systemHandlerInfo{
|
|
|
|
|
static_cast<int>(new_system.get_system_id()),
|
|
|
|
|
std::move(systemHandleThread)
|
|
|
|
|
std::move(systemHandleThread),
|
|
|
|
|
std::move(telemetryInfo_fut1)
|
|
|
|
|
});
|
|
|
|
|
});
|
|
|
|
|
|
|
|
|
|
@ -169,9 +228,13 @@ int main(int argc, char** argv)
|
|
|
|
|
// -- 從 systemHandlerInfos 中 輪巡所有 system 檢查並處理是否有需要丟到 mysql 的資料
|
|
|
|
|
// -- 從 mysql 抓到指令 丟到 systemHandlerInfo
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// for (const auto& info : systemHandlerInfos) {
|
|
|
|
|
// // Access info.systemID, info.systemHandleThread, info.telemetryInfo_fut
|
|
|
|
|
// // Perform operations on each object in systemHandlerInfos
|
|
|
|
|
// info.telemetryInfo_fut.wait();
|
|
|
|
|
// auto x = info.telemetryInfo_fut.get();
|
|
|
|
|
// std::cout << "mark D :" << info.systemID << "; " << x["altitude"] << std::endl;
|
|
|
|
|
// }
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// ==============================================================================
|