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

stable v0.4

Browse Source 
- 修正 disconnected 後不斷重刷的問題
- 修正 相同 sysid 再連接會產生抓錯的問題
- 增加 非 autopilot 裝置被 subscribe telemetry 問題
This commit is contained in:
chiyu1468
2024-01-13 23:35:04 +08:00
parent feb441d2f4
commit 98e399cbd5
4 changed files with 101 additions and 90 deletions
Download Patch File Download Diff File Expand all files Collapse all files
mavone/globals.cpp
+6 -2
View File
@@ -21,8 +21,12 @@ gTelemetryInfo
gVehicleCommand
gHandlerMask
gHandlerMask
這個變數是讓 主迴圈 得知 副迴圈 的連線狀態
gHandlerMask[sysid]["is_connected"]
這個變數是讓 主迴圈 通知 副迴圈 切斷Loop
gHandlerMask[sysid]["cutoff"]
*/
mavone/mavone.cpp
+17 -16
View File
@@ -81,7 +81,6 @@ 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
@@ -91,19 +90,21 @@ int main(int argc, char** argv)
// Avoid Duplicate System ID
bool duplicated;
for(int i=0; i<comingSystems.size(); i++){
// for(int i=0; i<comingSystems.size(); i++){
for(int i=comingSystems.size()-1; i>=0; i--){
sys = comingSystems[i];
if (!sys->is_connected()) {continue;}
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;
break; // this will break the j for loop
}
}
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
@@ -120,12 +121,10 @@ int main(int argc, char** argv)
});
// only get ONE system each time
break;
break; // this will break the i for loop
}
}
// Need some extra time fpr complete init
// sleep_for(milliseconds(500));
});
// MySQL connection is here
@@ -261,12 +260,17 @@ int main(int argc, char** argv)
}
// 刪除被輪尋到的 handlerInfo
// systemHandlerInfos.erase(std::remove_if(systemHandlerInfos.begin(), systemHandlerInfos.end(), [&](const auto& info) {
// return info.handlerState == systemHandlerState::Disconnected;
// }), systemHandlerInfos.end());
// 刪除 Disconnected 的 handlerInfo
for (auto it = systemHandlerInfos.begin(); it != systemHandlerInfos.end(); /* no increment here */) {
if (it->handlerState == systemHandlerState::Disconnected) {
if (it->systemThread.joinable()) {
it->systemThread.join();
}
it = systemHandlerInfos.erase(it);
} else {
++it;
}
}
}
@@ -285,9 +289,6 @@ int main(int argc, char** argv)
handlerInfo.systemThread.join();
}
// delete mavsdk;
// ~mavsdk;
return 0;
}
mavone/mavone.h
+2 -1
View File
@@ -46,7 +46,8 @@ enum class systemHandlerState {
Init, // 剛剛交給 systemHandler 去處理
Prologue, // 建立了 mysql row 並取得 serialNo
Ready, // 收到第一筆並放到 telemetryInfo 後
Disconnected // mavsdk 判斷該系統斷線了
Disconnected, // mavsdk 判斷該系統斷線了
// Void
};
struct systemHandlerInfo {
mavone/systemHandler.cpp
+76 -71
View File
@@ -48,97 +48,100 @@ void systemHandler(System& system) {
//
int _counter;
// std::cout << "Has AutoPilot : " << system.has_autopilot() << std::endl; // debug
// Put all the subscriber init in this section
telemetry.subscribe_position([&system, &telemetryInfo](Telemetry::Position position) {
// 轉換數字到字串用的暫存變數
std::ostringstream num_str_ss;
if (system.has_autopilot()) {
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("");
// 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.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("");
});
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;
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("");
});
// 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;
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("");
// 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.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("");
});
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;
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("");
});
// 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;
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("");
});
// 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";
}
});
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. 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;
std::cout << "(systemHandler.cpp:systemHandler) System " << sysid << " waiting too long without any coming info." << std::endl;
break;
}
}
@@ -161,9 +164,11 @@ void systemHandler(System& system) {
}
// Destroy
gHandlerMask[sysid]["is_connected"] = 0;
// Thread Terminate
std::cout << "(systemHandler.cpp:systemHandler) Thread Out " << sysid << std::endl; //debug
// Destroy
gHandlerMask[sysid]["is_connected"] = 0;
}