commit 82fb39d111390d16624cbdc4880dcbc74505a9ce Author: chiyu1468 Date: Tue Nov 28 10:26:06 2023 +0800 FirstCommit diff --git a/mavlinkpassth.cpp b/mavlinkpassth.cpp new file mode 100644 index 0000000..e688fb3 --- /dev/null +++ b/mavlinkpassth.cpp @@ -0,0 +1,51 @@ +#include +#include +#include +#include +#include + +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] << " \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; +} \ No newline at end of file diff --git a/mavsdkTest.py b/mavsdkTest.py new file mode 100644 index 0000000..bcfe49f --- /dev/null +++ b/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") + + + + + diff --git a/takeoff and land.cpp b/takeoff and land.cpp new file mode 100644 index 0000000..52b4f8f --- /dev/null +++ b/takeoff and land.cpp @@ -0,0 +1,144 @@ +// +// Simple example to demonstrate how takeoff and land using MAVSDK. +// + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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 << " \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> get_systems(Mavsdk& mavsdk) +{ + std::cout << "Waiting to discover systems...\n"; + std::vector> 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(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; +}