diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 5239cac95e..9b12c2b171 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -7,6 +7,7 @@ if (WERROR) add_compile_options(-Werror) endif() +add_subdirectory(autopilot_server) add_subdirectory(battery) add_subdirectory(calibrate) add_subdirectory(camera) diff --git a/examples/autopilot_server/CMakeLists.txt b/examples/autopilot_server/CMakeLists.txt new file mode 100644 index 0000000000..b5230d961d --- /dev/null +++ b/examples/autopilot_server/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 3.10.2) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +project(autopilot_server) + +add_executable(autopilot_server + autopilot_server.cpp +) + +find_package(MAVSDK REQUIRED) +find_package(Threads REQUIRED) + +target_link_libraries(autopilot_server + MAVSDK::mavsdk_telemetry + MAVSDK::mavsdk_telemetry_server + MAVSDK::mavsdk_mission_raw_server + MAVSDK::mavsdk_mission + MAVSDK::mavsdk_param_server + MAVSDK::mavsdk_param + MAVSDK::mavsdk_action + MAVSDK::mavsdk_action_server + MAVSDK::mavsdk_mavlink_passthrough + MAVSDK::mavsdk + Threads::Threads +) + +if(NOT MSVC) + add_compile_options(autopilot_server PRIVATE -Wall -Wextra) +else() + add_compile_options(autopilot_server PRIVATE -WX -W2) +endif() diff --git a/examples/autopilot_server/autopilot_server.cpp b/examples/autopilot_server/autopilot_server.cpp new file mode 100644 index 0000000000..9fe223fcb5 --- /dev/null +++ b/examples/autopilot_server/autopilot_server.cpp @@ -0,0 +1,311 @@ +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* + This example runs a MAVLink "autopilot" utilising the MAVSDK server plugins + on a seperate thread. This uses two MAVSDK instances, one GCS, one autopilot. + + The main thread acts as a GCS and reads telemetry, parameters, transmits across + a mission, clears the mission, arms the vehicle and then triggers a vehicle takeoff. + + The autopilot thread handles all the servers and triggers callbacks, publishes telemetry, + handles and stores parameters, prints received missions and sets the vehicle height to 10m on + successful takeoff request. +*/ + +using namespace mavsdk; + +using std::chrono::seconds; +using std::chrono::milliseconds; +using std::this_thread::sleep_for; + +Mission::MissionItem make_mission_item( + double latitude_deg, + double longitude_deg, + float relative_altitude_m, + float speed_m_s, + bool is_fly_through, + float gimbal_pitch_deg, + float gimbal_yaw_deg, + Mission::MissionItem::CameraAction camera_action) +{ + Mission::MissionItem new_item{}; + new_item.latitude_deg = latitude_deg; + new_item.longitude_deg = longitude_deg; + new_item.relative_altitude_m = relative_altitude_m; + new_item.speed_m_s = speed_m_s; + new_item.is_fly_through = is_fly_through; + new_item.gimbal_pitch_deg = gimbal_pitch_deg; + new_item.gimbal_yaw_deg = gimbal_yaw_deg; + new_item.camera_action = camera_action; + return new_item; +} + +int main(int argc, char** argv) +{ + // We run the server plugins on a seperate thead so we can use the main + // thread as a ground station. + std::thread autopilotThread([]() { + mavsdk::Mavsdk mavsdkTester; + mavsdk::Mavsdk::Configuration configuration( + mavsdk::Mavsdk::Configuration::UsageType::Autopilot); + mavsdkTester.set_configuration(configuration); + + auto result = mavsdkTester.add_any_connection("udp://127.0.0.1:14551"); + if (result == mavsdk::ConnectionResult::Success) { + std::cout << "Connected autopilot server side!" << std::endl; + } + + auto prom = std::promise>{}; + auto fut = prom.get_future(); + mavsdkTester.subscribe_on_new_system([&mavsdkTester, &prom]() { + std::cout << "Discovered MAVSDK GCS" << std::endl; + auto system = mavsdkTester.systems().back(); + mavsdkTester.subscribe_on_new_system(nullptr); + prom.set_value(system); + }); + + std::cout << "Sleeping AP thread... " << std::endl; + for (auto i = 0; i < 3; i++) { + std::this_thread::sleep_for(std::chrono::seconds(5)); + if (mavsdkTester.systems().size() == 0) { + std::cout << "No System Found from Autopilot, trying again in 5 secs..." + << std::endl; + if (i == 2) { + std::cout << "No System found after three retries. Aborting..." << std::endl; + return; + } + } else { + std::cout << "Setting System" << std::endl; + break; + } + } + auto system = mavsdkTester.systems().back(); + + // Create server plugins + auto paramServer = mavsdk::ParamServer{system}; + auto telemServer = mavsdk::TelemetryServer{system}; + auto actionServer = mavsdk::ActionServer{system}; + + // These are needed for MAVSDK at the moment + paramServer.provide_param_int("CAL_ACC0_ID", 1); + paramServer.provide_param_int("CAL_GYRO0_ID", 1); + paramServer.provide_param_int("CAL_MAG0_ID", 1); + paramServer.provide_param_int("SYS_HITL", 0); + paramServer.provide_param_int("MIS_TAKEOFF_ALT", 0); + // Add a custom param + paramServer.provide_param_int("my_param", 1); + + // Allow the vehicle to change modes, takeoff and arm + actionServer.set_allowable_flight_modes({true, true, true}); + actionServer.set_allow_takeoff(true); + actionServer.set_armable(true, true); + + // Create a mission raw server + // This will allow us to receive missions from a GCS + auto missionRawServer = mavsdk::MissionRawServer{system}; + std::cout << "MissionRawServer created" << std::endl; + + auto mission_prom = std::promise{}; + missionRawServer.subscribe_incoming_mission( + [&mission_prom, + &missionRawServer](MissionRawServer::Result res, MissionRawServer::MissionPlan plan) { + std::cout << "Received Uploaded Mission!" << std::endl; + std::cout << plan << std::endl; + // Unsubscribe so we only recieve one mission + missionRawServer.subscribe_incoming_mission(nullptr); + mission_prom.set_value(plan); + }); + missionRawServer.subscribe_current_item_changed([](MissionRawServer::MissionItem item) { + std::cout << "Current Mission Item Changed!" << std::endl; + std::cout << "Current Item: " << item << std::endl; + }); + missionRawServer.subscribe_clear_all( + [](uint32_t clear_all) { std::cout << "Clear All Mission!" << std::endl; }); + auto plan = mission_prom.get_future().get(); + + // Set current item to complete to progress the current item state + missionRawServer.set_current_item_complete(); + + // Create vehicle telemetry info + TelemetryServer::Position position{55.953251, -3.188267, 0, 0}; + TelemetryServer::PositionVelocityNed positionVelocityNed{{0, 0, 0}, {0, 0, 0}}; + TelemetryServer::VelocityNed velocity{}; + TelemetryServer::Heading heading{60}; + TelemetryServer::RawGps rawGps{ + 0, 55.953251, -3.188267, 0, NAN, NAN, 0, NAN, 0, 0, 0, 0, 0, 0}; + TelemetryServer::GpsInfo gpsInfo{11, TelemetryServer::FixType::Fix3D}; + + // As we're acting as an autopilot, lets just make the vehicle jump to 10m altitude on + // successful takeoff + actionServer.subscribe_takeoff([&position](ActionServer::Result result, bool takeoff) { + if (result == ActionServer::Result::Success) { + position.relative_altitude_m = 10; + } + }); + + while (true) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // Publish the telemetry + telemServer.publish_position(position, velocity, heading); + telemServer.publish_home(position); + telemServer.publish_position_velocity_ned(positionVelocityNed); + telemServer.publish_raw_gps(rawGps, gpsInfo); + } + }); + + // Now this is the main thread, we run client plugins to act as the GCS + // to communicate with the autopilot server plugins. + mavsdk::Mavsdk mavsdk; + mavsdk::Mavsdk::Configuration configuration( + mavsdk::Mavsdk::Configuration::UsageType::GroundStation); + mavsdk.set_configuration(configuration); + + auto result = mavsdk.add_any_connection("udp://:14551"); + if (result == mavsdk::ConnectionResult::Success) { + std::cout << "Connected!" << std::endl; + } + + auto prom = std::promise>{}; + auto fut = prom.get_future(); + mavsdk.subscribe_on_new_system([&mavsdk, &prom]() { + auto system = mavsdk.systems().back(); + + if (system->has_autopilot()) { + std::cout << "Discovered Autopilot from Client" << std::endl; + + // Unsubscribe again as we only want to find one system. + mavsdk.subscribe_on_new_system(nullptr); + prom.set_value(system); + } else { + std::cout << "No MAVSDK found" << std::endl; + } + }); + + if (fut.wait_for(std::chrono::seconds(10)) == std::future_status::timeout) { + std::cout << "No autopilot found, exiting" << std::endl; + return 1; + } + + std::this_thread::sleep_for(std::chrono::seconds(10)); + + auto system = fut.get(); + + // Create plugins + auto action = mavsdk::Action{system}; + auto param = mavsdk::Param{system}; + auto telemetry = mavsdk::Telemetry{system}; + + std::this_thread::sleep_for(std::chrono::seconds(5)); + auto mission = mavsdk::Mission{system}; + std::this_thread::sleep_for(std::chrono::seconds(5)); + + // Check for our custom param we have set in the server thread + auto res = param.get_param_int("my_param"); + if (res.first == mavsdk::Param::Result::Success) { + std::cout << "Found Param my_param: " << res.second << std::endl; + } + + // Create a mission to send to our mission server + std::cout << "Creating Mission" << std::endl; + std::vector mission_items; + mission_items.push_back(make_mission_item( + 47.398170327054473, + 8.5456490218639658, + 10.0f, + 5.0f, + false, + 20.0f, + 60.0f, + Mission::MissionItem::CameraAction::None)); + { + std::cout << "Uploading mission..." << std::endl; + // We only have the upload_mission function asynchronous for now, so we wrap it using + // std::future. + auto prom = std::make_shared>(); + auto future_result = prom->get_future(); + Mission::MissionPlan mission_plan{}; + mission_plan.mission_items = mission_items; + std::cout << "SystemID " << system->get_system_id() << std::endl; + mission.upload_mission_async( + mission_plan, [prom](Mission::Result result) { prom->set_value(result); }); + + const Mission::Result result = future_result.get(); + if (result != Mission::Result::Success) { + std::cout << "Mission upload failed (" << result << "), exiting." << std::endl; + return 1; + } + std::cout << "Mission uploaded." << std::endl; + + // Now clear the mission! + mission.clear_mission_async( + [](Mission::Result callback) { std::cout << "Clear Mission Request" << std::endl; }); + mission.subscribe_mission_progress([](Mission::MissionProgress progress) { + std::cout << "Current: " << progress.current << std::endl; + std::cout << "Total: " << progress.total << std::endl; + }); + } + + std::this_thread::sleep_for(std::chrono::seconds(20)); + + // We want to listen to the altitude of the drone at 1 Hz. + auto set_rate_result = telemetry.set_rate_position(1.0); + if (set_rate_result != mavsdk::Telemetry::Result::Success) { + std::cout << "Setting rate failed:" << set_rate_result << std::endl; + return 1; + } + + // Set up callback to monitor altitude while the vehicle is in flight + telemetry.subscribe_position([](mavsdk::Telemetry::Position position) { + std::cout << "Altitude: " << position.relative_altitude_m << " m" << std::endl; + }); + + // Check if vehicle is ready to arm + while (telemetry.health_all_ok() != true) { + std::cout << "Vehicle is getting ready to arm" << std::endl; + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + + // Arm vehicle + std::cout << "Arming..." << std::endl; + const Action::Result arm_result = action.arm(); + + if (arm_result != Action::Result::Success) { + std::cout << "Arming failed:" << arm_result << std::endl; + return 1; + } + + action.set_takeoff_altitude(10.f); + + // Take off + std::cout << "Taking off..." << std::endl; + bool takenOff = false; + while (true) { + const Action::Result takeoff_result = action.takeoff(); + if (takeoff_result != Action::Result::Success) { + std::cout << "Takeoff failed!:" << takeoff_result << std::endl; + std::this_thread::sleep_for(std::chrono::seconds(1)); + continue; + } + break; + } + + while (true) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + + return 0; +} \ No newline at end of file diff --git a/src/plugins/action_server/action_server_impl.cpp b/src/plugins/action_server/action_server_impl.cpp index 615ee6a54a..2a482ae278 100644 --- a/src/plugins/action_server/action_server_impl.cpp +++ b/src/plugins/action_server/action_server_impl.cpp @@ -102,6 +102,49 @@ void ActionServerImpl::init() }, this); + _parent->register_mavlink_command_handler( + MAV_CMD_NAV_TAKEOFF, + [this](const MavlinkCommandReceiver::CommandLong& command) { + if (_allow_takeoff) { + if (_takeoff_callback) { + _parent->call_user_callback( + [this]() { _takeoff_callback(ActionServer::Result::Success, true); }); + } + + mavlink_message_t msg; + mavlink_msg_command_ack_pack( + _parent->get_own_system_id(), + _parent->get_own_component_id(), + &msg, + command.command, + MAV_RESULT::MAV_RESULT_ACCEPTED, + std::numeric_limits::max(), + 0, + command.origin_system_id, + command.origin_component_id); + return msg; + } else { + if (_takeoff_callback) { + _parent->call_user_callback([this]() { + _takeoff_callback(ActionServer::Result::CommandDenied, false); + }); + } + mavlink_message_t msg; + mavlink_msg_command_ack_pack( + _parent->get_own_system_id(), + _parent->get_own_component_id(), + &msg, + command.command, + MAV_RESULT::MAV_RESULT_UNSUPPORTED, + std::numeric_limits::max(), + 0, + command.origin_system_id, + command.origin_component_id); + return msg; + } + }, + this); + // Flight mode _parent->register_mavlink_command_handler( MAV_CMD_DO_SET_MODE, @@ -218,7 +261,8 @@ void ActionServerImpl::subscribe_flight_mode_change(ActionServer::FlightModeChan void ActionServerImpl::subscribe_takeoff(ActionServer::TakeoffCallback callback) { - UNUSED(callback); + std::lock_guard lock(_callback_mutex); + _takeoff_callback = callback; } void ActionServerImpl::subscribe_land(ActionServer::LandCallback callback) @@ -243,10 +287,8 @@ void ActionServerImpl::subscribe_terminate(ActionServer::TerminateCallback callb ActionServer::Result ActionServerImpl::set_allow_takeoff(bool allow_takeoff) { - UNUSED(allow_takeoff); - - // TODO :) - return {}; + _allow_takeoff = allow_takeoff; + return ActionServer::Result::Success; } ActionServer::Result ActionServerImpl::set_armable(bool armable, bool force_armable) diff --git a/src/plugins/action_server/action_server_impl.h b/src/plugins/action_server/action_server_impl.h index 7dbd528fc0..3a79521de2 100644 --- a/src/plugins/action_server/action_server_impl.h +++ b/src/plugins/action_server/action_server_impl.h @@ -45,12 +45,15 @@ class ActionServerImpl : public PluginImplBase { private: ActionServer::ArmDisarmCallback _arm_disarm_callback{nullptr}; ActionServer::FlightModeChangeCallback _flight_mode_change_callback{nullptr}; + ActionServer::TakeoffCallback _takeoff_callback{nullptr}; + std::mutex _callback_mutex; std::atomic _armable = false; std::atomic _force_armable = false; std::atomic _disarmable = false; std::atomic _force_disarmable = false; + std::atomic _allow_takeoff = false; union px4_custom_mode { struct {