FirstCommit

mavlinkpassth.cpp

@@ -0,0 +1,51 @@
+#include <mavsdk/mavsdk.h>
+#include <mavsdk/plugins/mavlink_passthrough/mavlink_passthrough.h>
+#include <iostream>
+#include <chrono>
+#include <thread>
+
+using namespace mavsdk;
+using std::chrono::seconds;
+using std::this_thread::sleep_for;
+
+int main(int argc, char** argv)
+{
+    if (argc != 2) {
+        std::cerr << "Usage: " << argv[0] << " <connection_url>\n";
+        return 1;
+    }
+
+    Mavsdk mavsdk;
+    ConnectionResult connection_result = mavsdk.add_any_connection(argv[1]);
+
+    if (connection_result != ConnectionResult::Success) {
+        std::cerr << "Connection failed: " << connection_result << '\n';
+        return 1;
+    }
+
+    auto system = mavsdk.systems().at(0);
+
+    // Instantiate mavlink_passthrough plugin.
+    auto mavlink_passthrough = MavlinkPassthrough{system};
+
+    mavlink_message_t message;
+    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 = 4;
+    mavlink_msg_command_long_encode(mavlink_passthrough.get_our_sysid(),
+                                    mavlink_passthrough.get_our_compid(),
+                                    &message,
+                                    &command);
+    mavlink_passthrough.send_message(message);
+
+    // Wait for the flight mode to change.
+    sleep_for(seconds(1));
+
+    // Check the current flight mode.
+    const Telemetry::FlightMode mode_result = system->telemetry().flight_mode();
+    std::cout << "Current Flight Mode: " << mode_result << '\n';
+
+    return 0;
+}

mavsdkTest.py

@@ -0,0 +1,40 @@
+import asyncio
+from mavsdk import System
+import time
+
+async def print_connect(drone):
+    async for state in drone.core.connection_state():
+        print(f"{state}")
+
+
+async def run():
+    
+    drone = System()
+    await drone.connect(system_address="udp://:14550",  timeout=10)
+
+    # drone = System(mavsdk_server_address='localhost', port=50053)
+    # await drone.connect()
+    
+    print("Connected to drone!")
+
+    asyncio.ensure_future(print_connect(drone))
+
+    drone.core.connection_state()
+
+    while True:
+        await asyncio.sleep(1)
+
+
+asyncio.get_event_loop().run_until_complete(run())
+
+
+
+
+
+
+print("Finish")
+
+
+
+
+

takeoff and land.cpp

@@ -0,0 +1,144 @@
+//
+// 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 <iostream>
+#include <future>
+#include <memory>
+#include <thread>
+
+using namespace mavsdk;
+using std::chrono::seconds;
+using std::this_thread::sleep_for;
+
+
+void usage(const std::string& bin_name)
+{
+    std::cerr << "Usage : " << bin_name << " <connection_url>\n"
+              << "Connection URL format should be :\n"
+              << " For TCP : tcp://[server_host][:server_port]\n"
+              << " For UDP : udp://[bind_host][:bind_port]\n"
+              << " For Serial : serial:///path/to/serial/dev[:baudrate]\n"
+              << "For example, to connect to the simulator use URL: udp://:14540\n";
+}
+
+std::vector<std::shared_ptr<System>> get_systems(Mavsdk& mavsdk)
+{
+    std::cout << "Waiting to discover systems...\n";
+    std::vector<std::shared_ptr<System>> systems;
+
+    mavsdk.subscribe_on_new_system([&mavsdk, &systems]() {
+        auto system = mavsdk.systems().back();
+
+        if (system->has_autopilot()) {
+            std::cout << "Discovered autopilot\n";
+            systems.push_back(system);
+        }
+    });
+
+    // Wait for systems to be discovered.
+    sleep_for(seconds(3));
+
+    return systems;
+}
+
+int main(int argc, char** argv)
+{
+    if (argc != 2) {
+        usage(argv[0]);
+        return 1;
+    }
+
+    Mavsdk mavsdk;
+    ConnectionResult connection_result = mavsdk.add_any_connection(argv[1]);
+
+    if (connection_result != ConnectionResult::Success) {
+        std::cerr << "Connection failed: " << connection_result << '\n';
+        return 1;
+    }
+
+    auto system = get_system(mavsdk);
+    if (!system) {
+        return 1;
+    }
+
+    // Instantiate plugins.
+    auto telemetry = Telemetry{system};
+    auto action = Action{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;
+
+
+    // 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;
+    }
+
+    // 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;
+}