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

Compare commits

base: chiyu1468/MAVSDK_Dev:ad1d676738d9e730d15fba0e8bb25d1e362a2238
Branches Tags
chiyu1468/MAVSDK_Dev:master chiyu1468/MAVSDK_Dev:Dev
..
compare: chiyu1468/MAVSDK_Dev:master
Branches Tags
chiyu1468/MAVSDK_Dev:Dev chiyu1468/MAVSDK_Dev:master
10 Commits
ad1d676738 .. master
Author SHA1 Message Date
chiyu1468 c9147f4314 stable v0.42 
mavone.cpp
    - 修正 status 管理 bug : 還未改變資料庫狀態 就被程式的管理列表刪除
    - 新增 指定本服務於 MAVLink 上的 sysid=197, compid=254 (應該是不會特別去動態調整所以硬寫在裡面)
 2024-02-23 17:06:26 +08:00
chiyu1468 37fcd66585 stable v0.41 
mavone.cpp
  - 優化mysql中 connect state 寫入
systemHandler.cpp
  - 電量 增加 電量%數 電壓
  - 經緯高 電量 in_air 於實體飛控板傳輸頻率
 2024-02-02 15:38:04 +08:00
chiyu1468 98e399cbd5 stable v0.4 
- 修正 disconnected 後不斷重刷的問題
- 修正 相同 sysid 再連接會產生抓錯的問題
- 增加 非 autopilot 裝置被 subscribe telemetry 問題
 2024-01-13 23:35:04 +08:00
chiyu1468 feb441d2f4 stable v0.3 
修正 system id 重複獲取問題
修正 handlerState 卡在 Prologue 問題
 2024-01-12 14:20:34 +08:00
chiyu1468 60c8a49b7e stable v0.2 
沒有添加新功能，僅調整程式文件結構以利後續開發。

mavone.cpp => 處理主要迴圈與mysql連接
systemHandler.cpp => 處理 mavsdk_server 迴圈
globals.cpp => 宣告全域變數
 2023-12-29 17:55:28 +08:00
chiyu1468 57588d5026 for andy debug 2023-12-25 16:46:54 +08:00
chiyu1468 efe5bc4e3c stable v0.1 
mavone 是 專案主要程式
unit function dev 是 一些開發時的測試程式

虛擬機系統可以隨時連線與斷連了
把 mysql table schema 備份在 NodeRed_one.sql
 2023-12-15 09:37:55 +08:00
chiyu1468 ce0fb7454a The first connection success 
第一版把 MAVSDK 的 Telemetry 接上 MySQL 資料庫
額外加了不少的 include
同時也在 CMakeLists 加了 mysqlcppconn 相依庫 (這是 mysql 的官方庫)
但是其中的設定一定會因為環境不同需要修正的
 2023-12-14 00:49:06 +08:00
chiyu1468 872eacb529 GlobalVariable Pass Telemetry 
這版捨棄 future promise 傳遞 改用全域變數傳遞資料
 2023-12-07 14:27:02 +08:00
chiyu1468 e61388992a RunTimeError Example 
記錄錯誤：
雖然編譯可以完成，
但對於 telemetryInfo_pro 這個變量的生命週期 沒有掌握好 (mavone.cpp:214)
導致 Segmentation fault
 2023-12-06 15:57:12 +08:00
15 changed files with 771 additions and 456 deletions
Expand all files Collapse all files
CMakeLists.txt
-23
View File
@@ -1,23 +0,0 @@
cmake_minimum_required(VERSION 3.10.2)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
project(mavone)
add_executable(mavone
mavone.cpp
)
find_package(MAVSDK REQUIRED)
target_link_libraries(mavone
MAVSDK::mavsdk
pthread
)
if(NOT MSVC)
add_compile_options(mavone PRIVATE -Wall -Wextra)
else()
add_compile_options(mavone PRIVATE -WX -W2)
endif()
config.txt
-8
View File
@@ -1,8 +0,0 @@
A=Hello World
B=42
C=3.14
connectPort=udp://:14550
EOC
EOC 就是結尾了 所以這邊可以打註解
目前只有 connectPort 有功能
mavone.cpp
-425
View File
@@ -1,425 +0,0 @@
//
// 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 <iostream>
#include <fstream>
#include <sstream>
#include <future>
#include <memory>
#include <thread>
#include <vector>
using namespace mavsdk;
using std::chrono::seconds;
using std::this_thread::sleep_for;
struct MavInitParameter {
std::string connectPort;
std::string connectPort2;
int B;
float C;
};
struct systemHandlerInfo {
int systemID;
std::thread systemThread;
};
int initializeParameters(const std::string& filePath, MavInitParameter& initSetting) {
std::ifstream configFile(filePath);
if (!configFile) {
return 1;
}
std::vector<std::string> parameters; // Use a vector to store the parameters
std::string line;
while (std::getline(configFile, line)) {
if (line == "EOC") {
break; // Stop reading when "EOC" is encountered
}
// Split the line into key and value
std::size_t delimiterPos = line.find('=');
if (delimiterPos != std::string::npos) {
std::string key = line.substr(0, delimiterPos);
std::string value = line.substr(delimiterPos + 1);
// Check if the key matches any field in MavInitParameter
if (key == "connectPort") {
initSetting.connectPort = value;
} else if (key == "connectPort2") {
initSetting.connectPort2 = value;
} else if (key == "B") {
initSetting.B = std::stoi(value);
} else if (key == "C") {
initSetting.C = std::stof(value);
}
}
parameters.push_back(line); // Add the parameter to the vector
}
configFile.close();
std::cout << "Parameters read successfully!" << std::endl;
// Print all the parameters
for (const auto& parameter : parameters) {
std::cout << "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};
std::cout << "System " << static_cast<int>(system.get_system_id()) << " get in Thread.(debug)" << std::endl; // debug
telemetry.subscribe_position([&system](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
});
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;
}
// Program Initial Read Setting from config file
struct MavInitParameter initSetting;
auto i = initializeParameters(argv[1], 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);
if (connection_result != ConnectionResult::Success) {
std::cerr << "Connection failed: " << connection_result << '\n';
return 1;
}
// 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();
// 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 << "Duplicate System ID Collision : " << static_cast<int>(sys->get_system_id()) << std::endl;
return;
}
}
}
// 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::map<string, float> telemetryInfo;
auto telemetryInfo_pro = std::promise<std::map<string, float>>{};
auto telemetryInfo_fut = telemetryInfo_pro.get_future();
// Let handler progrem deal with System
std::thread systemHandleThread([&new_system]() {systemHandler(new_system);});
systemHandlerInfos.push_back(systemHandlerInfo{
static_cast<int>(new_system.get_system_id()),
std::move(systemHandleThread)
});
});
// MySQL connection is here
// Main Loop is here
// -- 將這個服務目前的狀態 更新到 mysql 中
// -- 從 systemHandlerInfos 中 輪巡所有 system 檢查並處理是否有需要丟到 mysql 的資料
// -- 從 mysql 抓到指令 丟到 systemHandlerInfo
// ==============================================================================
// 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;
// }
// }
// }
// 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';
// }
// 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
// // 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(15));
// 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;
// }
// // 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";
// // 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";
return 0;
}
// ===================================================
// 1.
// std::cout << "Mark A" << std::endl;
// sleep_for(seconds(3));
// System& s0 = *mavsdk.systems()[0];
// System& s1 = *mavsdk.systems()[1];
// std::cout << static_cast<int>(s0.get_system_id()) << std::endl;
// auto t0 = Telemetry{s0};
// auto t1 = Telemetry{s1};
// t0.subscribe_position([](Telemetry::Position position) {
// std::cout << "System ID: " << "0" << " Altitude: " << position.relative_altitude_m << " m\n";
// });
// t1.subscribe_position([](Telemetry::Position position) {
// std::cout << "System ID: " << "1" << " Altitude: " << position.relative_altitude_m << " m\n";
// });
mavone/CMakeLists.txt
+40
View File
@@ -0,0 +1,40 @@
cmake_minimum_required(VERSION 3.10.2)
set(CMAKE_CXX_STANDARD 17)
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_library(mavoneSys_lib
systemHandler.cpp
globals.cpp
)
add_executable(mavone
mavone.cpp
)
# 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
pthread
mysqlcppconn # for mysqlcppconn
mavoneSys_lib # for the lib i created
)
if(NOT MSVC)
add_compile_options(mavone PRIVATE -Wall -Wextra)
else()
add_compile_options(mavone PRIVATE -WX -W2)
endif()
mavone/NodeRed_one.sql
+62
View File
@@ -0,0 +1,62 @@
-- MySQL dump 10.13 Distrib 8.0.36, for Linux (x86_64)
--
-- Host: 172.17.0.2 Database: Mav_one
-- ------------------------------------------------------
-- Server version 8.0.29
/*!40101 SET @OLD_CHARACTER_SET_CLIENT=@@CHARACTER_SET_CLIENT */;
/*!40101 SET @OLD_CHARACTER_SET_RESULTS=@@CHARACTER_SET_RESULTS */;
/*!40101 SET @OLD_COLLATION_CONNECTION=@@COLLATION_CONNECTION */;
/*!50503 SET NAMES utf8mb4 */;
/*!40103 SET @OLD_TIME_ZONE=@@TIME_ZONE */;
/*!40103 SET TIME_ZONE='+00:00' */;
/*!40014 SET @OLD_UNIQUE_CHECKS=@@UNIQUE_CHECKS, UNIQUE_CHECKS=0 */;
/*!40014 SET @OLD_FOREIGN_KEY_CHECKS=@@FOREIGN_KEY_CHECKS, FOREIGN_KEY_CHECKS=0 */;
/*!40101 SET @OLD_SQL_MODE=@@SQL_MODE, SQL_MODE='NO_AUTO_VALUE_ON_ZERO' */;
/*!40111 SET @OLD_SQL_NOTES=@@SQL_NOTES, SQL_NOTES=0 */;
--
-- Table structure for table `NodeRed_one`
--
DROP TABLE IF EXISTS `NodeRed_one`;
/*!40101 SET @saved_cs_client = @@character_set_client */;
/*!50503 SET character_set_client = utf8mb4 */;
CREATE TABLE `NodeRed_one` (
`SerialNo` int NOT NULL AUTO_INCREMENT,
`create_time` datetime DEFAULT CURRENT_TIMESTAMP,
`update_time` datetime DEFAULT CURRENT_TIMESTAMP ON UPDATE CURRENT_TIMESTAMP,
`mavsys_connect_status` varchar(40) DEFAULT NULL,
`system_id` int NOT NULL,
`vehicle_type` varchar(10) DEFAULT NULL,
`vehicle_mode` varchar(10) DEFAULT NULL,
`vehicle_lat` double DEFAULT NULL,
`vehicle_lon` double DEFAULT NULL,
`vehicle_head` float DEFAULT NULL,
`vehicle_bat_volt` float DEFAULT NULL,
`vehicle_bat_perc` float DEFAULT NULL,
`vehicle_speed` float DEFAULT NULL,
`drone_alt` float DEFAULT NULL,
`drone_air` tinyint DEFAULT NULL,
`drone_arm` tinyint DEFAULT NULL,
`drone_pitch` float DEFAULT NULL,
`drone_roll` float DEFAULT NULL,
`drone_yaw` float DEFAULT NULL,
`StatusText` varchar(45) DEFAULT NULL,
`command_type` varchar(10) DEFAULT NULL,
`command_para1` double DEFAULT NULL,
`command_para2` double DEFAULT NULL,
PRIMARY KEY (`SerialNo`)
) ENGINE=InnoDB AUTO_INCREMENT=118 DEFAULT CHARSET=utf8mb4 COLLATE=utf8mb4_0900_ai_ci;
/*!40101 SET character_set_client = @saved_cs_client */;
/*!40103 SET TIME_ZONE=@OLD_TIME_ZONE */;
/*!40101 SET SQL_MODE=@OLD_SQL_MODE */;
/*!40014 SET FOREIGN_KEY_CHECKS=@OLD_FOREIGN_KEY_CHECKS */;
/*!40014 SET UNIQUE_CHECKS=@OLD_UNIQUE_CHECKS */;
/*!40101 SET CHARACTER_SET_CLIENT=@OLD_CHARACTER_SET_CLIENT */;
/*!40101 SET CHARACTER_SET_RESULTS=@OLD_CHARACTER_SET_RESULTS */;
/*!40101 SET COLLATION_CONNECTION=@OLD_COLLATION_CONNECTION */;
/*!40111 SET SQL_NOTES=@OLD_SQL_NOTES */;
-- Dump completed on 2024-02-02 15:33:46
mavone/config.txt
+10
View File
@@ -0,0 +1,10 @@
MAVSDKListeningPort=udp://:14550
mysqlHost=140.120.108.238
mysqlPort=1880
mysqlUser=root
mysqlPW=wp21qpck57
mysqlDatabase=Mav_one
C=3.14
EOC
EOC 就是結尾了 所以這邊可以打註解
mavone/globals.cpp
+39
View File
@@ -0,0 +1,39 @@
#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
這個變數是讓 主迴圈 得知 副迴圈 的連線狀態
gHandlerMask[sysid]["is_connected"]
這個變數是讓 主迴圈 通知 副迴圈 切斷Loop
gHandlerMask[sysid]["cutoff"]
*/
std::map<int, std::map<std::string, std::string>> gTelemetryInfo;
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
+14
View File
@@ -0,0 +1,14 @@
// globals.h
#ifndef GLOBALS_H
#define GLOBALS_H
#include <map>
#include <string>
#include <mutex>
extern std::map<int, std::map<std::string, std::string>> gTelemetryInfo;
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
+340
View File
@@ -0,0 +1,340 @@
#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) {
return 1;
}
std::vector<std::string> parameters; // Use a vector to store the parameters
std::string line;
while (std::getline(configFile, line)) {
if (line == "EOC") {
break; // Stop reading when "EOC" is encountered
}
// Split the line into key and value
std::size_t delimiterPos = line.find('=');
if (delimiterPos != std::string::npos) {
std::string key = line.substr(0, delimiterPos);
std::string value = line.substr(delimiterPos + 1);
// Check if the key matches any field in MavInitParameter
if (key == "MAVSDKListeningPort") {
initSetting.connectPort = 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);
}
}
parameters.push_back(line); // Add the parameter to the vector
}
configFile.close();
std::cout << "(mavone.cpp:initializeParameters) Parameters read successfully!" << std::endl;
// Print all the parameters
for (const auto& parameter : parameters) {
std::cout << "(mavone.cpp:initializeParameters) Line: " << parameter << std::endl;
}
return 0;
}
int main(int argc, char** argv)
{
reset = false;
if (argc != 2) {
std::cout << "(mavone.cpp:main) 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);
if (i == 1){
std::cerr << "(mavone.cpp:main) Failed to open config file: " << argv[1] << std::endl;
}
// Start MAVSDK Server and connect to it.
Mavsdk mavsdk;
// Set this MAVLINK service has system id 197, component id 254, NOT send heartbeat
mavsdk::Mavsdk::Configuration mavserver_configuration(197,254,false);
mavsdk.set_configuration(mavserver_configuration);
ConnectionResult connection_result = mavsdk.add_any_connection(initSetting.connectPort);
if (connection_result != ConnectionResult::Success) {
std::cerr << "(mavone.cpp:main) Connection failed: " << connection_result << '\n';
return 1;
}
// 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;
int sysid;
// Avoid Duplicate System ID
bool duplicated;
// 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; // 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
// 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; // this will break the i for loop
}
}
});
// 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 << "(mavone.cpp:main) 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;
while (!reset) {
sleep_for(seconds(1)); // debug
// 每個 System 輪詢過去
for ( auto& handlerInfo : systemHandlerInfos) {
switch (handlerInfo.handlerState) {
case systemHandlerState::Init :
std::cout << "(mavone.cpp:while) 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, mavsys_connect_status) 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 mavsys_connect_status = " +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;
// 更新 mavsys_connect_status 欄位
sqlString = "UPDATE NodeRed_one SET mavsys_connect_status = 'Prologue' WHERE SerialNO = " + std::to_string(handlerInfo.mysqlSN) + ";";
stmt->execute(sqlString);
break;
case systemHandlerState::Prologue :
std::cout << "(mavone.cpp:while) SystemHandler:" << std::to_string(handlerInfo.systemID) << " at Prologue " << std::endl; //debug
// 確認之前的 handlerInfo.mysqlSN 有正常被取得 若沒有整個程式即將關閉
if (handlerInfo.mysqlSN == -1) {
reset = true;
std::cout << "(mavone.cpp:while) handlerInfo.mysqlSN not aquired. Fatal Error. Shutdown all service. " << std::endl;
}
// 確認系統是否仍連線
if (gHandlerMask[handlerInfo.systemID]["is_connected"] == 0) {
handlerInfo.handlerState = systemHandlerState::Disconnected;
break;
}
// MAVSDK 還沒收到第一筆 telemetry 就持續在這個狀態
aTelemetryInfo = gTelemetryInfo[handlerInfo.systemID];
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);
case systemHandlerState::Ready :
// std::cout << "SystemHandler:" << std::to_string(handlerInfo.systemID) << " at Ready " << std::endl; //debug
// 確認系統是否仍連線
if (gHandlerMask[handlerInfo.systemID]["is_connected"] == 0) {
handlerInfo.handlerState = systemHandlerState::Disconnected;
break;
}
// 把 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;
case systemHandlerState::Disconnected :
std::cout << "(mavone.cpp:while) SystemHandler:" << std::to_string(handlerInfo.systemID) << " at Disconnected. " << std::endl; //debug
// 更新 mavsys_connect_status 欄位
sqlString = "UPDATE NodeRed_one SET mavsys_connect_status = 'Disconnected' WHERE SerialNO = " + std::to_string(handlerInfo.mysqlSN) + ";";
stmt->execute(sqlString);
// 變更狀態
handlerInfo.handlerState = systemHandlerState::Terminate;
// reset = true; //debug
break;
}
}
// 刪除 Disconnected 的 handlerInfo
for (auto it = systemHandlerInfos.begin(); it != systemHandlerInfos.end(); /* no increment here */) {
if (it->handlerState == systemHandlerState::Terminate) {
// 這個 if 如果不加 則副程序在結束以前就會被主程序放生 執行時會出現 SIGABRT 錯誤
if (it->systemThread.joinable()) {
it->systemThread.join();
}
it = systemHandlerInfos.erase(it);
} else {
++it;
}
}
}
// When Progrem Terminate
reset = true;
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();
}
return 0;
}
// ===================================================
// 1.
// std::cout << "Mark A" << std::endl;
// sleep_for(seconds(3));
// System& s0 = *mavsdk.systems()[0];
// System& s1 = *mavsdk.systems()[1];
// std::cout << static_cast<int>(s0.get_system_id()) << std::endl;
// auto t0 = Telemetry{s0};
// auto t1 = Telemetry{s1};
// t0.subscribe_position([](Telemetry::Position position) {
// std::cout << "System ID: " << "0" << " Altitude: " << position.relative_altitude_m << " m\n";
// });
// t1.subscribe_position([](Telemetry::Position position) {
// std::cout << "System ID: " << "1" << " Altitude: " << position.relative_altitude_m << " m\n";
// });
mavone/mavone.h
+64
View File
@@ -0,0 +1,64 @@
#ifndef MAVONE_H
#define MAVONE_H
#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 <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;
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 後
Disconnected, // mavsdk 判斷該系統斷線了
Terminate, // 準備要從管理列表中刪除
// Void
};
struct systemHandlerInfo {
int systemID;
std::thread systemThread;
systemHandlerState handlerState;
int mysqlSN;
};
bool reset;
#endif
mavone/systemHandler.cpp
+195
View File
@@ -0,0 +1,195 @@
#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 << "(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;
// std::cout << "Has AutoPilot : " << system.has_autopilot() << std::endl; // debug
// Put all the subscriber init in this section
if (system.has_autopilot()) {
auto set_rate_result = telemetry.set_rate_position(1.0);
if (set_rate_result != mavsdk::Telemetry::Result::Success) {
std::cout << "(systemHandler.cpp:systemHandler) Setting position rate failed:" << set_rate_result << std::endl;
}
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("");
});
// set_rate_result = telemetry.set_rate_attitude_euler(1.0); // only in v2.0
// if (set_rate_result != mavsdk::Telemetry::Result::Success) {
// std::cout << "(systemHandler.cpp:systemHandler) Setting euler rate failed:" << set_rate_result << std::endl;
// }
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("");
});
set_rate_result = telemetry.set_rate_battery(1.0);
if (set_rate_result != mavsdk::Telemetry::Result::Success) {
std::cout << "(systemHandler.cpp:systemHandler) Setting battery rate failed:" << set_rate_result << std::endl;
}
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_perc"] = num_str_ss.str();
num_str_ss.str("");
num_str_ss << std::fixed << std::setprecision(4) << battery.voltage_v;
telemetryInfo["vehicle_bat_volt"] = num_str_ss.str();
num_str_ss.str("");
});
set_rate_result = telemetry.set_rate_in_air(1.0);
if (set_rate_result != mavsdk::Telemetry::Result::Success) {
std::cout << "(systemHandler.cpp:systemHandler) Setting in_air rate failed:" << set_rate_result << std::endl;
}
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 << "(systemHandler.cpp:systemHandler) System " << sysid << " waiting too long without any coming info." << std::endl;
break;
}
}
// Loop
while(system.is_connected() & !reset) {
// Send Telemetry Data
gTeleMtx.lock();
gTelemetryInfo[sysid] = telemetryInfo;
gTeleMtx.unlock();
// Handler Command
reset = (gHandlerMask[sysid]["cutoff"] != 0);
// Vehicle Command
// for purpose
sleep_for(milliseconds(100));
}
// Thread Terminate
std::cout << "(systemHandler.cpp:systemHandler) Thread Out " << sysid << std::endl; //debug
// Destroy
gHandlerMask[sysid]["is_connected"] = 0;
}
mavone/systemHandler.h
+7
View File
@@ -0,0 +1,7 @@
#ifndef SYSTEMHANDLER_H
#define SYSTEMHANDLER_H
void systemHandler(System& system);
#endif
mavlinkpassth.cpp → unit function dev/mavlinkpassth.cpp
View File
mavsdkTest.py → unit function dev/mavsdkTest.py
View File
takeoff and land.cpp → unit function dev/takeoff and land.cpp
View File