stable v0.2
沒有添加新功能，僅調整程式文件結構以利後續開發。 mavone.cpp => 處理主要迴圈與mysql連接 systemHandler.cpp => 處理 mavsdk_server 迴圈 globals.cpp => 宣告全域變數master
parent
57588d5026
commit
60c8a49b7e
@ -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;
|
||||
@ -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
|
||||
}
|
||||
@ -0,0 +1,7 @@
|
||||
#ifndef SYSTEMHANDLER_H
|
||||
#define SYSTEMHANDLER_H
|
||||
|
||||
|
||||
void systemHandler(System& system);
|
||||
|
||||
#endif
|
||||
Loading…
Reference in New Issue