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

stable v0.3

Browse Source 
修正 system id 重複獲取問題
修正 handlerState 卡在 Prologue 問題
This commit is contained in:
chiyu1468
2024-01-12 14:20:34 +08:00
parent 60c8a49b7e
commit feb441d2f4
5 changed files with 130 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files
mavone/CMakeLists.txt
+8 -2
View File
@@ -15,10 +15,10 @@ add_library(mavoneSys_lib
add_executable(mavone
mavone.cpp
globals.cpp
)
find_package(MAVSDK REQUIRED)
# find MAVSDK package and accept only when MAVSDK version > 1.4.10
find_package(MAVSDK 1.4.10 REQUIRED)
target_link_libraries(mavone
MAVSDK::mavsdk
@@ -32,3 +32,9 @@ if(NOT MSVC)
else()
add_compile_options(mavone PRIVATE -WX -W2)
endif()
mavone/globals.cpp
+30 -1
View File
@@ -1,6 +1,35 @@
#include "globals.h"
#include <array>
/*
用簡單粗暴的全域變數讓主迴圈跟副迴圈傳遞資料
另外加個 互斥鎖(Mutex) 避免存取衝突
gTelemetryInfo
這個變數是存放 副迴圈 到 主迴圈的資訊 (主要是要讓無人機的 telemetry 藉由主迴圈再寫進資料庫)
資料形式為 systemid[INT] : { 資訊名稱[String] : 資訊內容[String] }
例如
gTelemetryInfo = {
1 : { "vehicle_lat" : "24.16584990", "vehicle_lon" : "120.99977417", "vehicle_head" : "167.01" },
2 : { "vehicle_bat" : "0.97", "drone_air" : "1" }
}
則代表裡面存有 system id 1 的 經緯度與航向 而 system id 2 有電量與滯空狀態
gVehicleCommand
gHandlerMask
*/
std::map<int, std::map<std::string, std::string>> gTelemetryInfo;
std::map<int, std::map<std::string, double>> gVehicleCommand;
std::map<int, std::map<std::string, std::array<double, 2>>> gVehicleCommand;
std::map<int, std::map<std::string, int>> gHandlerMask;
std::mutex gTeleMtx;
mavone/globals.h
+2 -1
View File
@@ -7,7 +7,8 @@
#include <mutex>
extern std::map<int, std::map<std::string, std::string>> gTelemetryInfo;
extern std::map<int, std::map<std::string, double>> gVehicleCommand;
extern std::map<int, std::map<std::string, std::array<double, 2>>> gVehicleCommand;
extern std::map<int, std::map<std::string, int>> gHandlerMask;
extern std::mutex gTeleMtx;
#endif
mavone/mavone.cpp
+66 -45
View File
@@ -81,42 +81,51 @@ int main(int argc, char** argv)
// For store Discover systems in a list
std::vector<systemHandlerInfo> systemHandlerInfos;
// Subscribe to new system discovery
mavsdk.subscribe_on_new_system([&mavsdk, &systemHandlerInfos]() {
// Get the last subscribed system
auto comingSystems = mavsdk.systems();
std::shared_ptr<System> sys = comingSystems.back();
int sysid = static_cast<int>(sys->get_system_id());
// Avoid Duplicate System ID (Component ID is careless, maybe determind by UUID will be better?)
for(int i = 0;i < systemHandlerInfos.size();i++) {
if(sys->get_system_id() == systemHandlerInfos[i].systemID){
comingSystems.pop_back();
if(comingSystems.size() != 0){
sys = comingSystems.back();
} else {
std::cout << "(mavone.cpp:main) Duplicate System ID Collision : " << sysid << std::endl;
return;
std::shared_ptr<System> sys;
int sysid;
// Avoid Duplicate System ID
bool duplicated;
for(int i=0; i<comingSystems.size(); i++){
sys = comingSystems[i];
sysid = static_cast<int>(sys->get_system_id());
duplicated = false;
for(int j=0; j<systemHandlerInfos.size(); j++){
if(sysid == systemHandlerInfos[j].systemID){
duplicated = true;
break;
}
}
}
// make std::shared_ptr<MAVSDK::System> back to MAVSDK::System
System& new_system = *sys;
// std::cout << "system detect : " << sysid << "(Debug)"<< std::endl; // debug
// Let handler progrem deal with System
std::thread systemHandleThread([&new_system]() {systemHandler(new_system);});
gVehicleCommand[sysid]["is_connected"] = 1;
if(duplicated == false){
// Get New System
System& new_system = *sys; // make std::shared_ptr<MAVSDK::System> back to MAVSDK::System
std::cout << "(mavone.cpp:subscribe_on_new_system) system detect sysid : " << sysid << "(Debug)"<< std::endl; // debug
// Let handler progrem deal with System
std::thread systemHandleThread([&new_system]() {systemHandler(new_system);});
// gHandlerMask[sysid]["is_connected"] = 1;
systemHandlerInfos.push_back(systemHandlerInfo{
sysid,
std::move(systemHandleThread),
systemHandlerState::Init,
-1
});
// only get ONE system each time
break;
}
}
// Need some extra time fpr complete init
sleep_for(milliseconds(500));
systemHandlerInfos.push_back(systemHandlerInfo{
sysid,
std::move(systemHandleThread),
systemHandlerState::Init,
-1
});
// sleep_for(milliseconds(500));
});
// MySQL connection is here
@@ -144,15 +153,15 @@ int main(int argc, char** argv)
std::string sqlString;
std::map<std::string,std::string> aTelemetryInfo;
while(!reset){
while (!reset) {
sleep_for(seconds(1)); // debug
// 每個 System 輪詢過去
for( auto& handlerInfo : systemHandlerInfos){
switch(handlerInfo.handlerState){
for ( auto& handlerInfo : systemHandlerInfos) {
switch (handlerInfo.handlerState) {
case systemHandlerState::Init :
std::cout << "(mavone.cpp:main) SystemHandler:" << std::to_string(handlerInfo.systemID) << " at Init " << std::endl; //debug
std::cout << "(mavone.cpp:while) SystemHandler:" << std::to_string(handlerInfo.systemID) << " at Init " << std::endl; //debug
// 用系統時間生成的隨機數 標記目前欄位
std::srand(std::time(0));
@@ -165,7 +174,7 @@ int main(int argc, char** argv)
sqlString = "SELECT SerialNO FROM NodeRed_one WHERE system_id = " + std::to_string(handlerInfo.systemID) + " AND mavsys_connect_status = " +std::to_string(randomValue) + ";";
// std::cout << "MySQL SELECT Query: " << sqlString << std::endl; //debug
res = stmt->executeQuery(sqlString);
if(res->next()) {
if (res->next()) {
// std::cout << "Query Reselt: " << res->getString(1) << std::endl; //debug
handlerInfo.mysqlSN = std::stof(res->getString(1));
}
@@ -179,16 +188,16 @@ int main(int argc, char** argv)
break;
case systemHandlerState::Prologue :
std::cout << "(mavone.cpp:main) SystemHandler:" << std::to_string(handlerInfo.systemID) << " at Prologue " << std::endl; //debug
std::cout << "(mavone.cpp:while) SystemHandler:" << std::to_string(handlerInfo.systemID) << " at Prologue " << std::endl; //debug
// 確認之前的 handlerInfo.mysqlSN 有正常被取得 若沒有整個程式即將關閉
if(handlerInfo.mysqlSN == -1) {
if (handlerInfo.mysqlSN == -1) {
reset = true;
std::cout << "(mavone.cpp:main) handlerInfo.mysqlSN not aquired. Fatal Error. Shutdown all service. " << std::endl;
std::cout << "(mavone.cpp:while) handlerInfo.mysqlSN not aquired. Fatal Error. Shutdown all service. " << std::endl;
}
// 確認系統是否仍連線
if(gVehicleCommand[handlerInfo.systemID]["is_connected"] == 0){
if (gHandlerMask[handlerInfo.systemID]["is_connected"] == 0) {
handlerInfo.handlerState = systemHandlerState::Disconnected;
// 更新 mavsys_connect_status 欄位
sqlString = "UPDATE NodeRed_one SET mavsys_connect_status = 'Disconnected' WHERE SerialNO = " + std::to_string(handlerInfo.mysqlSN) + ";";
@@ -198,12 +207,13 @@ int main(int argc, char** argv)
// MAVSDK 還沒收到第一筆 telemetry 就持續在這個狀態
aTelemetryInfo = gTelemetryInfo[handlerInfo.systemID];
if(aTelemetryInfo.size() == 0){
if (aTelemetryInfo.size() == 0) {
break;
}
// 切換到下一個狀態
handlerInfo.handlerState = systemHandlerState::Ready;
std::cout << "(mavone.cpp:while) SystemHandler:" << std::to_string(handlerInfo.systemID) << " into Ready " << std::endl; //debug
// 更新 mavsys_connect_status 欄位
sqlString = "UPDATE NodeRed_one SET mavsys_connect_status = 'Ready' WHERE SerialNO = " + std::to_string(handlerInfo.mysqlSN) + ";";
stmt->execute(sqlString);
@@ -212,7 +222,7 @@ int main(int argc, char** argv)
// std::cout << "SystemHandler:" << std::to_string(handlerInfo.systemID) << " at Ready " << std::endl; //debug
// 確認系統是否仍連線
if(gVehicleCommand[handlerInfo.systemID]["is_connected"] == 0){
if (gHandlerMask[handlerInfo.systemID]["is_connected"] == 0) {
handlerInfo.handlerState = systemHandlerState::Disconnected;
// 更新 mavsys_connect_status 欄位
sqlString = "UPDATE NodeRed_one SET mavsys_connect_status = 'Disconnected' WHERE SerialNO = " + std::to_string(handlerInfo.mysqlSN) + ";";
@@ -243,30 +253,41 @@ int main(int argc, char** argv)
break;
case systemHandlerState::Disconnected :
std::cout << "SystemHandler:" << std::to_string(handlerInfo.systemID) << " at Disconnected " << std::endl; //debug
std::cout << "(mavone.cpp:while) SystemHandler:" << std::to_string(handlerInfo.systemID) << " at Disconnected. " << std::endl; //debug
reset = true; //debug
// reset = true; //debug
break;
}
}
// 刪除被輪尋到的 handlerInfo
// systemHandlerInfos.erase(std::remove_if(systemHandlerInfos.begin(), systemHandlerInfos.end(), [&](const auto& info) {
// return info.handlerState == systemHandlerState::Disconnected;
// }), systemHandlerInfos.end());
}
// When Progrem Terminate
reset = true;
// make all thread join
for( auto& handlerInfo : systemHandlerInfos){
handlerInfo.systemThread.join();
}
std::cout << "(mavone.cpp:main) Main Loop End !!!!!!" << std::endl;
// terminate MySQL Indicator
delete res;
delete stmt;
delete con;
// make all thread join
for( auto& handlerInfo : systemHandlerInfos){
handlerInfo.systemThread.join();
}
// delete mavsdk;
// ~mavsdk;
return 0;
}
mavone/systemHandler.cpp
+23 -6
View File
@@ -35,11 +35,18 @@ void systemHandler(System& system) {
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
std::cout << "(systemHandler.cpp:systemHandler) System " << sysid << " get in Thread. (debug)" << std::endl; // debug
// Store telemetry information
std::map<std::string,std::string> telemetryInfo;
// 這些變數監控 handler 的行為
gHandlerMask[sysid]["cutoff"] = 0;
gHandlerMask[sysid]["is_connected"] = 1;
bool reset = false;
//
int _counter;
// Put all the subscriber init in this section
telemetry.subscribe_position([&system, &telemetryInfo](Telemetry::Position position) {
@@ -125,28 +132,38 @@ void systemHandler(System& system) {
}
});
// Wait Until telemetryInfo get something.
// Wait Until telemetryInfo get something. and if nothing get from System for 10 sec will cut off this connection.
_counter = 0;
while(telemetryInfo.empty()){
sleep_for(seconds(1));
if(_counter++ > 10) {
reset = true;
std::cout << "mark B " << sysid << " retry : " << _counter << std::endl;
break;
}
}
// Loop
while(system.is_connected()) {
while(system.is_connected() & !reset) {
// Send Telemetry Data
gTeleMtx.lock();
gTelemetryInfo[sysid] = telemetryInfo;
gTeleMtx.unlock();
// Deal Command
// Handler Command
reset = (gHandlerMask[sysid]["cutoff"] != 0);
// Vehicle Command
// for purpose
sleep_for(milliseconds(100));
}
// Destroy
gVehicleCommand[sysid]["is_connected"] = 0;
gHandlerMask[sysid]["is_connected"] = 0;
// Thread Terminate
std::cout << "Thread Out " << sysid << std::endl; //debug
std::cout << "(systemHandler.cpp:systemHandler) Thread Out " << sysid << std::endl; //debug
}