Start Dev Project

多機連上單port，並且顯示所有連線過的系統id

config.txt

@@ -0,0 +1,8 @@
+A=Hello World
+B=42
+C=3.14
+connectPort=udp://:14550
+EOC
+
+EOC 就是結尾了 所以這邊可以打註解
+目前只有 connectPort 有功能

mavone.cpp

@@ -0,0 +1,255 @@
+//
+// 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>
+
+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<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;
+    }
+}
+
+std::shared_ptr<System> get_system(Mavsdk& mavsdk)
+{
+    std::cout << "Waiting to discover system...\n";
+    auto prom = std::promise<std::shared_ptr<System>>{};
+    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<int>(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<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 = 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;
+}