From 5f571580f4567e1791fb93580247ff8d1eb3ae51 Mon Sep 17 00:00:00 2001 From: chiyu1468 Date: Thu, 30 Nov 2023 18:18:48 +0800 Subject: [PATCH] Start Dev Project MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 多機連上單port，並且顯示所有連線過的系統id --- config.txt | 8 ++ mavone.cpp | 255 +++++++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 263 insertions(+) create mode 100644 config.txt create mode 100644 mavone.cpp diff --git a/config.txt b/config.txt new file mode 100644 index 0000000..c582ce3 --- /dev/null +++ b/config.txt @@ -0,0 +1,8 @@ +A=Hello World +B=42 +C=3.14 +connectPort=udp://:14550 +EOC + +EOC 就是結尾了 所以這邊可以打註解 +目前只有 connectPort 有功能 \ No newline at end of file diff --git a/mavone.cpp b/mavone.cpp new file mode 100644 index 0000000..8f1a119 --- /dev/null +++ b/mavone.cpp @@ -0,0 +1,255 @@ +// +// Simple example to demonstrate how takeoff and land using MAVSDK. +// + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +struct MavInitParameter { + std::string connectPort; + std::string connectPort2; + int B; + float C; +}; + +using namespace mavsdk; +using std::chrono::seconds; +using std::this_thread::sleep_for; + +void initializeParameters(const std::string& filePath, MavInitParameter& initSetting) { + std::ifstream configFile(filePath); + if (!configFile) { + std::cerr << "Failed to open config file: " << filePath << std::endl; + return; + } + + std::vector 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; + } +} + +std::shared_ptr get_system(Mavsdk& mavsdk) +{ + std::cout << "Waiting to discover system...\n"; + auto prom = std::promise>{}; + auto fut = prom.get_future(); + + // We wait for new systems to be discovered, once we find one that has an + // autopilot, we decide to use it. + mavsdk.subscribe_on_new_system([&mavsdk, &prom]() { + auto system = mavsdk.systems().back(); + + if (system->has_autopilot()) { + std::cout << "Discovered autopilot\n"; + + // Unsubscribe again as we only want to find one system. + // mavsdk.subscribe_on_new_system(nullptr); + prom.set_value(system); + } + }); + + // We usually receive heartbeats at 1Hz, therefore we should find a + // system after around 3 seconds max, surely. + if (fut.wait_for(seconds(3)) == std::future_status::timeout) { + std::cerr << "No autopilot found.\n"; + return {}; + } + + // Get discovered system now. + return fut.get(); +} + +int main(int argc, char** argv) +{ + if (argc != 2) { + std::cout << "Config File Needed!" << std::endl; + return 1; + } + + // std::string configFilePath = "config.txt"; + struct MavInitParameter initSetting; + initializeParameters(argv[1], initSetting); + + Mavsdk mavsdk; + ConnectionResult connection_result = mavsdk.add_any_connection(initSetting.connectPort); + + // mavsdk.subscribe_on_new_system([]() { + // std::cout << "Discovered new system\n"; + // }); + + // sleep_for(seconds(3)); + + if (connection_result != ConnectionResult::Success) { + std::cerr << "Connection failed: " << connection_result << '\n'; + return 1; + } + + auto system = get_system(mavsdk); + if (!system) { + return 1; + } + + std::cout << "MarkA \n"; + + for(int i = 0; i < 10; ++i) { + sleep_for(seconds(5)); + + for (auto system : mavsdk.systems()) { + std::cout << "Found system with system ID: " << static_cast(system->get_system_id()) + << ", connected: " << (system->is_connected() ? "yes" : "no") << '\n'; + } + + std::cout << "link check " << i << " time. \n"; + } + + + return 1; + + + + // Instantiate plugins. + auto telemetry = Telemetry{system}; + auto action = Action{system}; + + // Instantiate mavlink_passthrough plugin. + auto mavlink_passthrough = MavlinkPassthrough{system}; + + auto x = static_cast(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 = telemetry.set_rate_position(0.2); + 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 + telemetry.subscribe_position([](Telemetry::Position position) { + std::cout << "Altitude: " << position.relative_altitude_m << " m\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(10)); + + 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; +}