You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
425 lines
14 KiB
C++
425 lines
14 KiB
C++
//
|
|
// 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";
|
|
// });
|