From bc9337ced69635aa87bc62964ed6fbbfef07c9fe Mon Sep 17 00:00:00 2001 From: Seb Horsewell Date: Mon, 27 Sep 2021 16:50:16 +0100 Subject: [PATCH 1/5] Add autopilot_server example --- examples/autopilot_server/CMakeLists.txt | 31 ++ .../autopilot_server/autopilot_server.cpp | 312 ++++++++++++++++++ .../action_server/action_server_impl.cpp | 52 ++- .../action_server/action_server_impl.h | 3 + 4 files changed, 393 insertions(+), 5 deletions(-) create mode 100644 examples/autopilot_server/CMakeLists.txt create mode 100644 examples/autopilot_server/autopilot_server.cpp diff --git a/examples/autopilot_server/CMakeLists.txt b/examples/autopilot_server/CMakeLists.txt new file mode 100644 index 0000000000..c53d9b1987 --- /dev/null +++ b/examples/autopilot_server/CMakeLists.txt @@ -0,0 +1,31 @@ +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) + +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 +) + +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..bf0dcd132d --- /dev/null +++ b/examples/autopilot_server/autopilot_server.cpp @@ -0,0 +1,312 @@ +#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; + +#define ERROR_CONSOLE_TEXT "\033[31m" // Turn text on console red +#define TELEMETRY_CONSOLE_TEXT "\033[34m" // Turn text on console blue +#define NORMAL_CONSOLE_TEXT "\033[0m" // Restore normal console colour + +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(); + prom.set_value(system); + mavsdkTester.subscribe_on_new_system(nullptr); + }); + + std::cout << "Sleeping thread " << std::endl; + std::this_thread::sleep_for(std::chrono::seconds(5)); + + if (mavsdkTester.systems().size() == 0) { + std::cout << "No System Found from Autopilot" << std::endl; + return; + } else { + std::cout << "Setting System" << std::endl; + } + auto system = mavsdkTester.systems().back(); + if (system == nullptr) { + std::cout << "System is null??" << std::endl; + } + + // 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 missionraw = mavsdk::MissionRawServer{system}; + std::cout << "MissionRawServer created" << std::endl; + + auto mission_prom = std::promise{}; + missionraw.subscribe_incoming_mission( + [&mission_prom](MissionRawServer::Result res, MissionRawServer::MissionPlan plan) { + std::cout << "Received Uploaded Mission!" << std::endl; + std::cout << plan << std::endl; + mission_prom.set_value(plan); + }); + missionraw.subscribe_current_item_changed([](MissionRawServer::MissionItem item) { + std::cout << "Current Item Changed!" << std::endl; + std::cout << "Current Item: " << item << std::endl; + }); + missionraw.subscribe_clear_all( + [](uint32_t clear_all) { std::cout << "Clear All Mission!" << std::endl; }); + auto plan = mission_prom.get_future().get(); + missionraw.set_current_item_complete(); + missionraw.set_current_item_complete(); + missionraw.set_current_item_complete(); + missionraw.set_current_item_complete(); + + // Create vehicle telemetry info + TelemetryServer::Position position{55.953251, -3.188267, 0, 0}; + TelemetryServer::PositionVelocityNed position_velocity_ned{{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 + // 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(position_velocity_ned); + 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" << unsigned(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 << ERROR_CONSOLE_TEXT << "Arming failed:" << arm_result << NORMAL_CONSOLE_TEXT + << 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 << ERROR_CONSOLE_TEXT << "Takeoff failed:" << takeoff_result + << NORMAL_CONSOLE_TEXT << 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 { From bd7c41d426f3a185ec665a89af3a718f0dc42ab6 Mon Sep 17 00:00:00 2001 From: Seb Horsewell Date: Tue, 28 Sep 2021 09:37:53 +0100 Subject: [PATCH 2/5] Tidy example subscriptions and naming --- .../autopilot_server/autopilot_server.cpp | 67 ++++++++++--------- 1 file changed, 34 insertions(+), 33 deletions(-) diff --git a/examples/autopilot_server/autopilot_server.cpp b/examples/autopilot_server/autopilot_server.cpp index bf0dcd132d..9862b72c80 100644 --- a/examples/autopilot_server/autopilot_server.cpp +++ b/examples/autopilot_server/autopilot_server.cpp @@ -31,10 +31,6 @@ using std::chrono::seconds; using std::chrono::milliseconds; using std::this_thread::sleep_for; -#define ERROR_CONSOLE_TEXT "\033[31m" // Turn text on console red -#define TELEMETRY_CONSOLE_TEXT "\033[34m" // Turn text on console blue -#define NORMAL_CONSOLE_TEXT "\033[0m" // Restore normal console colour - Mission::MissionItem make_mission_item( double latitude_deg, double longitude_deg, @@ -77,23 +73,27 @@ int main(int argc, char** argv) mavsdkTester.subscribe_on_new_system([&mavsdkTester, &prom]() { std::cout << "Discovered MAVSDK GCS" << std::endl; auto system = mavsdkTester.systems().back(); - prom.set_value(system); mavsdkTester.subscribe_on_new_system(nullptr); + prom.set_value(system); }); - std::cout << "Sleeping thread " << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(5)); - - if (mavsdkTester.systems().size() == 0) { - std::cout << "No System Found from Autopilot" << std::endl; - return; - } else { - std::cout << "Setting System" << std::endl; + 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(); - if (system == nullptr) { - std::cout << "System is null??" << std::endl; - } // Create server plugins auto paramServer = mavsdk::ParamServer{system}; @@ -116,31 +116,32 @@ int main(int argc, char** argv) // Create a mission raw server // This will allow us to receive missions from a GCS - auto missionraw = mavsdk::MissionRawServer{system}; + auto missionRawServer = mavsdk::MissionRawServer{system}; std::cout << "MissionRawServer created" << std::endl; auto mission_prom = std::promise{}; - missionraw.subscribe_incoming_mission( - [&mission_prom](MissionRawServer::Result res, MissionRawServer::MissionPlan plan) { + 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); }); - missionraw.subscribe_current_item_changed([](MissionRawServer::MissionItem item) { - std::cout << "Current Item Changed!" << std::endl; + missionRawServer.subscribe_current_item_changed([](MissionRawServer::MissionItem item) { + std::cout << "Current Mission Item Changed!" << std::endl; std::cout << "Current Item: " << item << std::endl; }); - missionraw.subscribe_clear_all( + missionRawServer.subscribe_clear_all( [](uint32_t clear_all) { std::cout << "Clear All Mission!" << std::endl; }); auto plan = mission_prom.get_future().get(); - missionraw.set_current_item_complete(); - missionraw.set_current_item_complete(); - missionraw.set_current_item_complete(); - missionraw.set_current_item_complete(); + + // 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 position_velocity_ned{{0, 0, 0}, {0, 0, 0}}; + TelemetryServer::PositionVelocityNed positionVelocityNed{{0, 0, 0}, {0, 0, 0}}; TelemetryServer::VelocityNed velocity{}; TelemetryServer::Heading heading{60}; TelemetryServer::RawGps rawGps{ @@ -148,7 +149,7 @@ int main(int argc, char** argv) TelemetryServer::GpsInfo gpsInfo{11, TelemetryServer::FixType::Fix3D}; // As we're acting as an autopilot, lets just make the vehicle jump to 10m altitude on - // takeoff + // successful takeoff actionServer.subscribe_takeoff([&position](ActionServer::Result result, bool takeoff) { if (result == ActionServer::Result::Success) { position.relative_altitude_m = 10; @@ -161,7 +162,7 @@ int main(int argc, char** argv) // Publish the telemetry telemServer.publish_position(position, velocity, heading); telemServer.publish_home(position); - telemServer.publish_position_velocity_ned(position_velocity_ned); + telemServer.publish_position_velocity_ned(positionVelocityNed); telemServer.publish_raw_gps(rawGps, gpsInfo); } }); @@ -238,7 +239,7 @@ int main(int argc, char** argv) auto future_result = prom->get_future(); Mission::MissionPlan mission_plan{}; mission_plan.mission_items = mission_items; - std::cout << "SystemID" << unsigned(system->get_system_id()) << std::endl; + std::cout << "SystemID " << system->get_system_id() << std::endl; mission.upload_mission_async( mission_plan, [prom](Mission::Result result) { prom->set_value(result); }); @@ -283,7 +284,7 @@ int main(int argc, char** argv) const Action::Result arm_result = action.arm(); if (arm_result != Action::Result::Success) { - std::cout << ERROR_CONSOLE_TEXT << "Arming failed:" << arm_result << NORMAL_CONSOLE_TEXT + std::cout << "Arming failed:" << arm_result << std::endl; return 1; } @@ -296,8 +297,8 @@ int main(int argc, char** argv) while (true) { const Action::Result takeoff_result = action.takeoff(); if (takeoff_result != Action::Result::Success) { - std::cout << ERROR_CONSOLE_TEXT << "Takeoff failed:" << takeoff_result - << NORMAL_CONSOLE_TEXT << std::endl; + std::cout << "Takeoff failed!:" << takeoff_result + << std::endl; std::this_thread::sleep_for(std::chrono::seconds(1)); continue; } From a7c568222a99458e4285c6ab6ee88869aa1e44cb Mon Sep 17 00:00:00 2001 From: Seb Horsewell Date: Tue, 28 Sep 2021 09:43:55 +0100 Subject: [PATCH 3/5] Fix style --- examples/autopilot_server/autopilot_server.cpp | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/examples/autopilot_server/autopilot_server.cpp b/examples/autopilot_server/autopilot_server.cpp index 9862b72c80..9fe223fcb5 100644 --- a/examples/autopilot_server/autopilot_server.cpp +++ b/examples/autopilot_server/autopilot_server.cpp @@ -78,13 +78,12 @@ int main(int argc, char** argv) }); std::cout << "Sleeping AP thread... " << std::endl; - for(auto i = 0; i < 3; i++) - { + 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 from Autopilot, trying again in 5 secs..." + << std::endl; + if (i == 2) { std::cout << "No System found after three retries. Aborting..." << std::endl; return; } @@ -121,7 +120,8 @@ int main(int argc, char** argv) auto mission_prom = std::promise{}; missionRawServer.subscribe_incoming_mission( - [&mission_prom, &missionRawServer](MissionRawServer::Result res, MissionRawServer::MissionPlan plan) { + [&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 @@ -284,8 +284,7 @@ int main(int argc, char** argv) const Action::Result arm_result = action.arm(); if (arm_result != Action::Result::Success) { - std::cout << "Arming failed:" << arm_result - << std::endl; + std::cout << "Arming failed:" << arm_result << std::endl; return 1; } @@ -297,8 +296,7 @@ int main(int argc, char** argv) while (true) { const Action::Result takeoff_result = action.takeoff(); if (takeoff_result != Action::Result::Success) { - std::cout << "Takeoff failed!:" << takeoff_result - << std::endl; + std::cout << "Takeoff failed!:" << takeoff_result << std::endl; std::this_thread::sleep_for(std::chrono::seconds(1)); continue; } From 3854a6855a789fd5ee216c96a33679423c2760fc Mon Sep 17 00:00:00 2001 From: Seb Horsewell Date: Tue, 28 Sep 2021 13:49:12 +0100 Subject: [PATCH 4/5] Add autopilot_server top level cmakelists --- examples/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) 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) From c2542f29080e1a10e9bb1cc6d9b0199b7ebf6643 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 28 Sep 2021 15:19:01 +0200 Subject: [PATCH 5/5] autopilot_server: link to threads --- examples/autopilot_server/CMakeLists.txt | 22 ++++++++++++---------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/examples/autopilot_server/CMakeLists.txt b/examples/autopilot_server/CMakeLists.txt index c53d9b1987..b5230d961d 100644 --- a/examples/autopilot_server/CMakeLists.txt +++ b/examples/autopilot_server/CMakeLists.txt @@ -10,18 +10,20 @@ add_executable(autopilot_server ) 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 + 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)