diff --git a/.ycm_extra_conf.py b/.ycm_extra_conf.py index acfbc83b8a..98efb51e65 100644 --- a/.ycm_extra_conf.py +++ b/.ycm_extra_conf.py @@ -83,6 +83,8 @@ '-I', 'plugins/mission/include', '-I', +'plugins/mission_raw/include', +'-I', 'plugins/offboard/include', '-I', 'plugins/params_raw/include', diff --git a/integration_tests/CMakeLists.txt b/integration_tests/CMakeLists.txt index 4876447788..f97f0b1bb4 100644 --- a/integration_tests/CMakeLists.txt +++ b/integration_tests/CMakeLists.txt @@ -39,6 +39,7 @@ add_executable(integration_tests_runner mission.cpp mission_rtl.cpp mission_survey.cpp + mission_raw_mission_changed.cpp offboard_velocity.cpp params_raw.cpp path_checker.cpp @@ -66,6 +67,7 @@ target_link_libraries(integration_tests_runner dronecode_sdk_telemetry dronecode_sdk_action dronecode_sdk_mission + dronecode_sdk_mission_raw dronecode_sdk_offboard dronecode_sdk_log_files dronecode_sdk_info diff --git a/integration_tests/mission_raw_mission_changed.cpp b/integration_tests/mission_raw_mission_changed.cpp new file mode 100644 index 0000000000..c422dc75ef --- /dev/null +++ b/integration_tests/mission_raw_mission_changed.cpp @@ -0,0 +1,108 @@ +#include "integration_test_helper.h" +#include "dronecode_sdk.h" +#include "plugins/mission/mission.h" +#include "plugins/mission_raw/mission_raw.h" +#include +#include + +using namespace dronecode_sdk; + +static constexpr double SOME_LATITUDES[] = {47.398170, 47.398175}; +static constexpr double SOME_LONGITUDES[] = {8.545649, 8.545654}; +static constexpr float SOME_ALTITUDES[] = {5.0f, 7.5f}; +static constexpr float SOME_SPEEDS[] = {4.0f, 5.0f}; +static constexpr unsigned NUM_SOME_ITEMS = sizeof(SOME_LATITUDES) / sizeof(SOME_LATITUDES[0]); + +static void +validate_items(const std::vector> &items); + +TEST_F(SitlTest, MissionRawMissionChanged) +{ + DronecodeSDK dc; + + ConnectionResult ret = dc.add_udp_connection(); + ASSERT_EQ(ret, ConnectionResult::SUCCESS); + + // Wait for system to connect via heartbeat. + std::this_thread::sleep_for(std::chrono::seconds(2)); + ASSERT_TRUE(dc.is_connected()); + + System &system = dc.system(); + ASSERT_TRUE(system.has_autopilot()); + + auto mission = std::make_shared(system); + auto mission_raw = std::make_shared(system); + + std::promise prom_changed{}; + std::future fut_changed = prom_changed.get_future(); + + LogInfo() << "Subscribe for mission changed notification"; + mission_raw->subscribe_mission_changed([&prom_changed]() { prom_changed.set_value(); }); + + // The mission change callback should not trigger yet. + EXPECT_EQ(fut_changed.wait_for(std::chrono::milliseconds(500)), std::future_status::timeout); + + std::vector> mission_items; + + for (unsigned i = 0; i < NUM_SOME_ITEMS; ++i) { + auto new_item = std::make_shared(); + new_item->set_position(SOME_LATITUDES[i], SOME_LONGITUDES[i]); + new_item->set_relative_altitude(SOME_ALTITUDES[i]); + new_item->set_speed(SOME_SPEEDS[i]); + mission_items.push_back(new_item); + } + + { + LogInfo() << "Uploading mission..."; + // We only have the upload_mission function asynchronous for now, so we wrap it using + // std::future. + std::promise prom{}; + std::future fut = prom.get_future(); + mission->upload_mission_async(mission_items, [&prom](Mission::Result result) { + ASSERT_EQ(result, Mission::Result::SUCCESS); + prom.set_value(); + LogInfo() << "Mission uploaded."; + }); + + auto status = fut.wait_for(std::chrono::seconds(2)); + ASSERT_EQ(status, std::future_status::ready); + fut.get(); + } + + // The mission change callback should have triggered now because we have uploaded a mission. + EXPECT_EQ(fut_changed.wait_for(std::chrono::milliseconds(500)), std::future_status::ready); + + { + std::promise prom{}; + std::future fut = prom.get_future(); + LogInfo() << "Download raw mission items."; + mission_raw->download_mission_async( + [&prom](MissionRaw::Result result, + std::vector> items) { + EXPECT_EQ(result, MissionRaw::Result::SUCCESS); + // TODO: validate items + validate_items(items); + prom.set_value(); + }); + auto status = fut.wait_for(std::chrono::seconds(2)); + ASSERT_EQ(status, std::future_status::ready); + } +} + +void validate_items(const std::vector> &items) +{ + for (unsigned i = 0; i < items.size(); ++i) { + // Even items are waypoints, odd ones are the speed commands. + if (i % 2 == 0) { + EXPECT_EQ(items[i]->command, 16); // MAV_CMD_NAV_WAYPOINT + EXPECT_EQ(items[i]->x, std::round(SOME_LATITUDES[i / 2] * 1e7)); + EXPECT_EQ(items[i]->y, std::round(SOME_LONGITUDES[i / 2] * 1e7)); + EXPECT_EQ(items[i]->z, SOME_ALTITUDES[i / 2]); + LogWarn() << "i/2: " << i / 2; + } else { + EXPECT_EQ(items[i]->command, 178); // MAV_CMD_DO_CHANGE_SPEED + EXPECT_EQ(items[i]->param2, SOME_SPEEDS[i / 2]); + LogWarn() << "i/2: " << i / 2; + } + } +} diff --git a/plugins/CMakeLists.txt b/plugins/CMakeLists.txt index 9bddd2b573..7c18366f99 100644 --- a/plugins/CMakeLists.txt +++ b/plugins/CMakeLists.txt @@ -18,6 +18,7 @@ add_subdirectory(info) add_subdirectory(log_files) #add_subdirectory(logging) # Not implemented completely add_subdirectory(mission) +add_subdirectory(mission_raw) add_subdirectory(offboard) add_subdirectory(params_raw) add_subdirectory(telemetry) diff --git a/plugins/action/action_impl.cpp b/plugins/action/action_impl.cpp index 084f8d935f..15c47c5de1 100644 --- a/plugins/action/action_impl.cpp +++ b/plugins/action/action_impl.cpp @@ -2,6 +2,7 @@ #include "dronecode_sdk_impl.h" #include "global_include.h" #include "px4_custom_mode.h" +#include namespace dronecode_sdk { @@ -158,8 +159,8 @@ Action::Result ActionImpl::goto_location(double latitude_deg, command.command = MAV_CMD_DO_REPOSITION; command.target_component_id = _parent->get_autopilot_id(); command.params.param4 = to_rad_from_deg(yaw_deg); - command.params.x = (int32_t)(latitude_deg * 1E7); - command.params.y = (int32_t)(longitude_deg * 1E7); + command.params.x = int32_t(std::round(latitude_deg * 1e7)); + command.params.y = int32_t(std::round(longitude_deg * 1e7)); command.params.z = altitude_amsl_m; return action_result_from_command_result(_parent->send_command(command)); diff --git a/plugins/follow_me/follow_me_impl.cpp b/plugins/follow_me/follow_me_impl.cpp index 24bb81d859..90ccbcf6b5 100644 --- a/plugins/follow_me/follow_me_impl.cpp +++ b/plugins/follow_me/follow_me_impl.cpp @@ -2,6 +2,7 @@ #include "system.h" #include "global_include.h" #include "px4_custom_mode.h" +#include namespace dronecode_sdk { @@ -253,8 +254,8 @@ void FollowMeImpl::send_target_location() // LogDebug() << debug_str << "Lat: " << _target_location.latitude_deg << " Lon: " << // _target_location.longitude_deg << // " Alt: " << _target_location.absolute_altitude_m; - const int32_t lat_int = static_cast(_target_location.latitude_deg * 1e7); - const int32_t lon_int = static_cast(_target_location.longitude_deg * 1e7); + const int32_t lat_int = int32_t(std::round(_target_location.latitude_deg * 1e7)); + const int32_t lon_int = int32_t(std::round(_target_location.longitude_deg * 1e7)); const float alt = static_cast(_target_location.absolute_altitude_m); _mutex.unlock(); diff --git a/plugins/gimbal/gimbal_impl.cpp b/plugins/gimbal/gimbal_impl.cpp index 093f313021..fc3ee45283 100644 --- a/plugins/gimbal/gimbal_impl.cpp +++ b/plugins/gimbal/gimbal_impl.cpp @@ -1,6 +1,7 @@ #include "gimbal_impl.h" #include "global_include.h" #include +#include namespace dronecode_sdk { @@ -61,8 +62,8 @@ GimbalImpl::set_roi_location(double latitude_deg, double longitude_deg, float al MAVLinkCommands::CommandInt command{}; command.command = MAV_CMD_DO_SET_ROI_LOCATION; - command.params.x = int32_t(latitude_deg * 1e7); - command.params.y = int32_t(longitude_deg * 1e7); + command.params.x = int32_t(std::round(latitude_deg * 1e7)); + command.params.y = int32_t(std::round(longitude_deg * 1e7)); command.params.z = altitude_m; command.target_component_id = _parent->get_autopilot_id(); @@ -77,8 +78,8 @@ void GimbalImpl::set_roi_location_async(double latitude_deg, MAVLinkCommands::CommandInt command{}; command.command = MAV_CMD_DO_SET_ROI_LOCATION; - command.params.x = int32_t(latitude_deg * 1e7); - command.params.y = int32_t(longitude_deg * 1e7); + command.params.x = int32_t(std::round(latitude_deg * 1e7)); + command.params.y = int32_t(std::round(longitude_deg * 1e7)); command.params.z = altitude_m; command.target_component_id = _parent->get_autopilot_id(); diff --git a/plugins/mission/mission.cpp b/plugins/mission/mission.cpp index 72fb54b2b5..c2b7caff98 100644 --- a/plugins/mission/mission.cpp +++ b/plugins/mission/mission.cpp @@ -65,18 +65,20 @@ const char *Mission::result_str(Result result) switch (result) { case Result::SUCCESS: return "Success"; - case Result::BUSY: - return "Busy"; case Result::ERROR: return "Error"; case Result::TOO_MANY_MISSION_ITEMS: return "Too many mission items"; - case Result::INVALID_ARGUMENT: - return "Invalid argument"; + case Result::BUSY: + return "Busy"; case Result::TIMEOUT: return "Timeout"; + case Result::INVALID_ARGUMENT: + return "Invalid argument"; case Result::UNSUPPORTED: return "Mission downloaded from system is unsupported"; + case Result::NO_MISSION_AVAILABLE: + return "No mission available"; case Result::FAILED_TO_OPEN_QGC_PLAN: return "Failed to open QGC plan"; case Result::FAILED_TO_PARSE_QGC_PLAN: diff --git a/plugins/mission/mission_item_impl.cpp b/plugins/mission/mission_item_impl.cpp index c0a6e87f48..8b5bfde68b 100644 --- a/plugins/mission/mission_item_impl.cpp +++ b/plugins/mission/mission_item_impl.cpp @@ -108,12 +108,12 @@ float MissionItemImpl::get_mavlink_param4() const int32_t MissionItemImpl::get_mavlink_x() const { - return int32_t(_latitude_deg * 1e7); + return int32_t(std::round(_latitude_deg * 1e7)); } int32_t MissionItemImpl::get_mavlink_y() const { - return int32_t(_longitude_deg * 1e7); + return int32_t(std::round(_longitude_deg * 1e7)); } float MissionItemImpl::get_mavlink_z() const diff --git a/plugins/mission_raw/CMakeLists.txt b/plugins/mission_raw/CMakeLists.txt new file mode 100644 index 0000000000..f310159fd1 --- /dev/null +++ b/plugins/mission_raw/CMakeLists.txt @@ -0,0 +1,33 @@ +add_library(dronecode_sdk_mission_raw ${PLUGIN_LIBRARY_TYPE} + mission_raw.cpp + mission_raw_impl.cpp +) + +include_directories( + ${PROJECT_SOURCE_DIR}/core +) + +set_target_properties(dronecode_sdk_mission_raw + PROPERTIES COMPILE_FLAGS ${warnings} +) + +target_link_libraries(dronecode_sdk_mission_raw + dronecode_sdk +) + +install(FILES + include/plugins/mission_raw/mission_raw.h + DESTINATION ${dronecode_sdk_install_include_dir} +) + +install(TARGETS dronecode_sdk_mission_raw + #EXPORT dronecode_sdk-targets + DESTINATION ${dronecode_sdk_install_lib_dir} +) + +target_include_directories(dronecode_sdk_mission_raw +PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/include +PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR} +) diff --git a/plugins/mission_raw/include/plugins/mission_raw/mission_raw.h b/plugins/mission_raw/include/plugins/mission_raw/mission_raw.h new file mode 100644 index 0000000000..c24a5e7a59 --- /dev/null +++ b/plugins/mission_raw/include/plugins/mission_raw/mission_raw.h @@ -0,0 +1,133 @@ +#pragma once + +#include +#include +#include +#include "plugin_base.h" + +namespace dronecode_sdk { + +class MissionRawImpl; +class System; + +/** + * @brief The MissionRaw class enables direct direct access to MAVLink + * mission items. + */ +class MissionRaw : public PluginBase { +public: + /** + * @brief Constructor. Creates the plugin for a specific System. + * + * The plugin is typically created as shown below: + * + * ```cpp + * auto mission_raw = std::make_shared(system); + * ``` + * + * @param system The specific system associated with this plugin. + */ + explicit MissionRaw(System &system); + + /** + * @brief Destructor (internal use only). + */ + ~MissionRaw(); + + /** + * @brief Possible results returned for mission requests. + */ + enum class Result { + UNKNOWN, /**< @brief Unknown error. */ + SUCCESS, /**< @brief %Request succeeded. */ + ERROR, /**< @brief Error. */ + BUSY, /**< @brief %Vehicle busy. */ + TIMEOUT, /**< @brief Request timed out. */ + INVALID_ARGUMENT, /**< @brief Invalid argument. */ + NO_MISSION_AVAILABLE, /**< @brief No mission available on system. */ + CANCELLED /**< @brief Mission upload or download has been cancelled. */ + }; + + /** + * @brief Gets a human-readable English string for an MissionRaw::Result. + * + * @param result Enum for which string is required. + * @return Human readable string for the MissionRaw::Result. + */ + static const char *result_str(Result result); + + /** + * @brief Mission item identical to MAVLink MISSION_ITEM_INT. + */ + struct MavlinkMissionItemInt { + uint8_t target_system; /**< @brief System ID. */ + uint8_t target_component; /**< @brief Component ID. */ + uint16_t seq; /**< @brief Sequence. */ + uint8_t frame; /**< @brief The coordinate system of the waypoint. */ + uint16_t command; /**< @brief The scheduled action for the waypoint. */ + uint8_t current; /**< @brief false:0, true:1. */ + uint8_t autocontinue; /**< @brief Autocontinue to next waypoint. */ + float param1; /**< @brief PARAM1, see MAV_CMD enum. */ + float param2; /**< @brief PARAM2, see MAV_CMD enum. */ + float param3; /**< @brief PARAM3, see MAV_CMD enum. */ + float param4; /**< @brief PARAM4, see MAV_CMD enum. */ + int32_t x; /**< @brief PARAM5 / local: x position in meters * 1e4, global: latitude in + degrees * 10^7. */ + int32_t y; /**< @brief PARAM6 / y position: local: x position in meters * 1e4, global: + longitude in degrees *10^7. */ + float z; /**< @brief PARAM7 / local: Z coordinate, global: altitude (relative or absolute, + depending on frame). */ + uint8_t mission_type; /**< @brief Mission type. */ + }; + + /** + * @brief Type for vector of mission items. + */ + typedef std::function>)> + mission_items_and_result_callback_t; + + /** + * @brief Downloads a vector of mission items from the system (asynchronous). + * + * The method will fail if any of the downloaded mission items are not supported + * by the Dronecode SDK API. + * + * @param callback Callback to receive mission items and result of this request. + */ + void download_mission_async(mission_items_and_result_callback_t callback); + + /** + * @brief Cancel a mission download (asynchronous). + * + * This cancels an ongoing mission download. The mission download will fail + * with the result `Result::CANCELLED`. + */ + void download_mission_cancel(); + + /** + * @brief Callback type to signal if the mission has changed. + */ + typedef std::function mission_changed_callback_t; + + /** + * @brief Subscribes to mission progress (asynchronous). + * + * @param callback Callback to receive mission progress. + */ + void subscribe_mission_changed(mission_changed_callback_t callback); + + /** + * @brief Copy constructor (object is not copyable). + */ + MissionRaw(const MissionRaw &) = delete; + /** + * @brief Equality operator (object is not copyable). + */ + const MissionRaw &operator=(const MissionRaw &) = delete; + +private: + /** @private Underlying implementation, set at instantiation */ + std::unique_ptr _impl; +}; + +} // namespace dronecode_sdk diff --git a/plugins/mission_raw/mission_raw.cpp b/plugins/mission_raw/mission_raw.cpp new file mode 100644 index 0000000000..2246bd0aba --- /dev/null +++ b/plugins/mission_raw/mission_raw.cpp @@ -0,0 +1,47 @@ +#include "plugins/mission_raw/mission_raw.h" +#include "mission_raw_impl.h" +#include + +namespace dronecode_sdk { + +MissionRaw::MissionRaw(System &system) : PluginBase(), _impl{new MissionRawImpl(system)} {} + +MissionRaw::~MissionRaw() {} + +void MissionRaw::download_mission_async(MissionRaw::mission_items_and_result_callback_t callback) +{ + _impl->download_mission_async(callback); +} + +void MissionRaw::download_mission_cancel() +{ + _impl->download_mission_cancel(); +} + +const char *MissionRaw::result_str(Result result) +{ + switch (result) { + case Result::SUCCESS: + return "Success"; + case Result::BUSY: + return "Busy"; + case Result::ERROR: + return "Error"; + case Result::INVALID_ARGUMENT: + return "Invalid argument"; + case Result::TIMEOUT: + return "Timeout"; + case Result::CANCELLED: + return "Cancelled"; + case Result::UNKNOWN: + default: + return "Unknown"; + } +} + +void MissionRaw::subscribe_mission_changed(mission_changed_callback_t callback) +{ + _impl->subscribe_mission_changed(callback); +} + +} // namespace dronecode_sdk diff --git a/plugins/mission_raw/mission_raw_impl.cpp b/plugins/mission_raw/mission_raw_impl.cpp new file mode 100644 index 0000000000..e0edf9326f --- /dev/null +++ b/plugins/mission_raw/mission_raw_impl.cpp @@ -0,0 +1,331 @@ +#include "mission_raw_impl.h" +#include "system.h" +#include "global_include.h" +#include // for `std::ifstream` +#include // for `std::stringstream` +#include + +namespace dronecode_sdk { + +using namespace std::placeholders; // for `_1` + +MissionRawImpl::MissionRawImpl(System &system) : PluginImplBase(system) +{ + _parent->register_plugin(this); +} + +MissionRawImpl::~MissionRawImpl() +{ + _parent->unregister_plugin(this); +} + +void MissionRawImpl::init() +{ + _parent->register_mavlink_message_handler( + MAVLINK_MSG_ID_MISSION_ACK, + std::bind(&MissionRawImpl::process_mission_ack, this, _1), + this); + + _parent->register_mavlink_message_handler( + MAVLINK_MSG_ID_MISSION_COUNT, + std::bind(&MissionRawImpl::process_mission_count, this, _1), + this); + + _parent->register_mavlink_message_handler( + MAVLINK_MSG_ID_MISSION_ITEM_INT, + std::bind(&MissionRawImpl::process_mission_item_int, this, _1), + this); +} + +void MissionRawImpl::deinit() +{ + _parent->unregister_all_mavlink_message_handlers(this); +} + +void MissionRawImpl::enable() {} + +void MissionRawImpl::disable() {} + +void MissionRawImpl::process_mission_ack(const mavlink_message_t &message) +{ + mavlink_mission_ack_t mission_ack; + mavlink_msg_mission_ack_decode(&message, &mission_ack); + + if (mission_ack.type != MAV_MISSION_ACCEPTED) { + return; + } + + // We assume that if the vehicle sends an ACCEPTED ack might have received + // a new mission. In that case we need to notify our user. + std::lock_guard lock(_mission_changed.mutex); + if (_mission_changed.callback) { + // Local copy because we can't make a copy of member variable. + auto callback = _mission_changed.callback; + _parent->call_user_callback([callback]() { callback(); }); + } +} + +void MissionRawImpl::download_mission_async( + const MissionRaw::mission_items_and_result_callback_t &callback) +{ + { + std::lock_guard lock(_mission_download.mutex); + if (_mission_download.state != MissionDownload::State::NONE) { + if (callback) { + std::vector> empty_vec{}; + _parent->call_user_callback( + [callback, empty_vec]() { callback(MissionRaw::Result::BUSY, empty_vec); }); + } + return; + } + + _mission_download.state = MissionDownload::State::REQUEST_LIST; + _mission_download.retries = 0; + _mission_download.mavlink_mission_items_downloaded.clear(); + _mission_download.callback = callback; + } + + _parent->register_timeout_handler( + std::bind(&MissionRawImpl::do_download_step, this), 0.0, nullptr); +} + +void MissionRawImpl::do_download_step() +{ + std::lock_guard lock(_mission_download.mutex); + switch (_mission_download.state) { + case MissionDownload::State::NONE: + LogWarn() << "Invalid state: do_download_step and State::NONE"; + break; + case MissionDownload::State::REQUEST_LIST: + request_list(); + break; + case MissionDownload::State::REQUEST_ITEM: + request_item(); + break; + case MissionDownload::State::SHOULD_ACK: + send_ack(); + break; + } +} + +void MissionRawImpl::request_list() +{ + // Requires _mission_download.mutex. + + if (_mission_download.retries++ >= 3) { + // We tried multiple times without success, let's give up. + _mission_download.state = MissionDownload::State::NONE; + if (_mission_download.callback) { + std::vector> empty_vec{}; + auto callback = _mission_download.callback; + _parent->call_user_callback( + [callback, empty_vec]() { callback(MissionRaw::Result::TIMEOUT, empty_vec); }); + } + return; + } + + // LogDebug() << "Requesting mission list (" << _mission_download.retries << ")"; + + mavlink_message_t message; + mavlink_msg_mission_request_list_pack(GCSClient::system_id, + GCSClient::component_id, + &message, + _parent->get_system_id(), + _parent->get_autopilot_id(), + MAV_MISSION_TYPE_MISSION); + + if (!_parent->send_message(message)) { + // This is a terrible and unexpected error. Therefore we don't even + // retry but just give up. + _mission_download.state = MissionDownload::State::NONE; + if (_mission_download.callback) { + std::vector> empty_vec{}; + auto callback = _mission_download.callback; + _parent->call_user_callback( + [callback, empty_vec]() { callback(MissionRaw::Result::ERROR, empty_vec); }); + } + return; + } + + // We retry the list request and mission item request, so we use the lower timeout. + _parent->register_timeout_handler( + std::bind(&MissionRawImpl::do_download_step, this), RETRY_TIMEOUT_S, &_timeout_cookie); +} + +void MissionRawImpl::process_mission_count(const mavlink_message_t &message) +{ + // LogDebug() << "Received mission count"; + + std::lock_guard lock(_mission_download.mutex); + if (_mission_download.state != MissionDownload::State::REQUEST_LIST) { + return; + } + + mavlink_mission_count_t mission_count; + mavlink_msg_mission_count_decode(&message, &mission_count); + + _mission_download.num_mission_items_to_download = mission_count.count; + _mission_download.next_mission_item_to_download = 0; + _mission_download.retries = 0; + + // We can get rid of the timeout and schedule and do the next step straightaway. + _parent->unregister_timeout_handler(_timeout_cookie); + + _mission_download.state = MissionDownload::State::REQUEST_ITEM; + + // Let's just add this to the queue, this way we don't block the receive thread + // and let go of the mutex as well. + _parent->register_timeout_handler( + std::bind(&MissionRawImpl::do_download_step, this), 0.0, nullptr); +} + +void MissionRawImpl::request_item() +{ + // Requires _mission_download.mutex. + + if (_mission_download.retries++ >= 3) { + // We tried multiple times without success, let's give up. + _mission_download.state = MissionDownload::State::NONE; + if (_mission_download.callback) { + std::vector> empty_vec{}; + auto callback = _mission_download.callback; + _parent->call_user_callback( + [callback, empty_vec]() { callback(MissionRaw::Result::TIMEOUT, empty_vec); }); + } + return; + } + + // LogDebug() << "Requesting mission item " << _mission_download.next_mission_item_to_download + // << " (" << _mission_download.retries << ")"; + + mavlink_message_t message; + mavlink_msg_mission_request_int_pack(GCSClient::system_id, + GCSClient::component_id, + &message, + _parent->get_system_id(), + _parent->get_autopilot_id(), + _mission_download.next_mission_item_to_download, + MAV_MISSION_TYPE_MISSION); + + if (!_parent->send_message(message)) { + // This is a terrible and unexpected error. Therefore we don't even + // retry but just give up. + _mission_download.state = MissionDownload::State::NONE; + if (_mission_download.callback) { + std::vector> empty_vec{}; + auto callback = _mission_download.callback; + _parent->call_user_callback( + [callback, empty_vec]() { callback(MissionRaw::Result::ERROR, empty_vec); }); + } + return; + } + + // We retry the list request and mission item request, so we use the lower timeout. + _parent->register_timeout_handler( + std::bind(&MissionRawImpl::do_download_step, this), RETRY_TIMEOUT_S, &_timeout_cookie); +} + +void MissionRawImpl::process_mission_item_int(const mavlink_message_t &message) +{ + // LogDebug() << "Received mission item int"; + + std::lock_guard lock(_mission_download.mutex); + if (_mission_download.state != MissionDownload::State::REQUEST_ITEM) { + return; + } + + mavlink_mission_item_int_t mission_item_int; + mavlink_msg_mission_item_int_decode(&message, &mission_item_int); + + if (mission_item_int.seq != _mission_download.next_mission_item_to_download) { + LogWarn() << "Received mission item " << int(mission_item_int.seq) << " instead of " + << _mission_download.next_mission_item_to_download << " (ignored)"; + + // The timeout will happen anyway and retry for this case. + return; + } + + auto new_item = std::make_shared(); + + new_item->target_system = mission_item_int.target_system; + new_item->target_component = mission_item_int.target_component; + new_item->seq = mission_item_int.seq; + new_item->frame = mission_item_int.frame; + new_item->command = mission_item_int.command; + new_item->current = mission_item_int.current; + new_item->autocontinue = mission_item_int.autocontinue; + new_item->param1 = mission_item_int.param1; + new_item->param2 = mission_item_int.param2; + new_item->param3 = mission_item_int.param3; + new_item->param4 = mission_item_int.param4; + new_item->x = mission_item_int.x; + new_item->y = mission_item_int.y; + new_item->z = mission_item_int.z; + new_item->mission_type = mission_item_int.mission_type; + + _mission_download.mavlink_mission_items_downloaded.push_back(new_item); + ++_mission_download.next_mission_item_to_download; + _mission_download.retries = 0; + + if (_mission_download.next_mission_item_to_download == + _mission_download.num_mission_items_to_download) { + _mission_download.state = MissionDownload::State::SHOULD_ACK; + } + + // We can remove timeout. + _parent->unregister_timeout_handler(_timeout_cookie); + + // And schedule the next request. + _parent->register_timeout_handler( + std::bind(&MissionRawImpl::do_download_step, this), 0.0, nullptr); +} + +void MissionRawImpl::send_ack() +{ + // Requires _mission_download.mutex. + + // LogDebug() << "Sending ack"; + + mavlink_message_t message; + mavlink_msg_mission_ack_pack(GCSClient::system_id, + GCSClient::component_id, + &message, + _parent->get_system_id(), + _parent->get_autopilot_id(), + MAV_MISSION_ACCEPTED, + MAV_MISSION_TYPE_MISSION); + + if (!_parent->send_message(message)) { + // This is a terrible and unexpected error. Therefore we don't even + // retry but just give up. + _mission_download.state = MissionDownload::State::NONE; + if (_mission_download.callback) { + std::vector> empty_vec{}; + auto callback = _mission_download.callback; + _parent->call_user_callback( + [callback, empty_vec]() { callback(MissionRaw::Result::ERROR, empty_vec); }); + } + return; + } + + // We did it, we are done. + _mission_download.state = MissionDownload::State::NONE; + + if (_mission_download.callback) { + std::vector> vec_copy = + _mission_download.mavlink_mission_items_downloaded; + auto callback = _mission_download.callback; + _parent->call_user_callback( + [callback, vec_copy]() { callback(MissionRaw::Result::SUCCESS, vec_copy); }); + } +} + +void MissionRawImpl::download_mission_cancel() {} + +void MissionRawImpl::subscribe_mission_changed(MissionRaw::mission_changed_callback_t callback) +{ + std::lock_guard lock(_mission_changed.mutex); + _mission_changed.callback = callback; +} + +} // namespace dronecode_sdk diff --git a/plugins/mission_raw/mission_raw_impl.h b/plugins/mission_raw/mission_raw_impl.h new file mode 100644 index 0000000000..9a7bfae83b --- /dev/null +++ b/plugins/mission_raw/mission_raw_impl.h @@ -0,0 +1,59 @@ +#pragma once + +#include "system.h" +#include "mavlink_include.h" +#include "plugins/mission_raw/mission_raw.h" +#include "plugin_impl_base.h" +#include + +namespace dronecode_sdk { + +class MissionRawImpl : public PluginImplBase { +public: + MissionRawImpl(System &system); + ~MissionRawImpl(); + + void init() override; + void deinit() override; + + void enable() override; + void disable() override; + + void download_mission_async(const MissionRaw::mission_items_and_result_callback_t &callback); + void download_mission_cancel(); + + void subscribe_mission_changed(MissionRaw::mission_changed_callback_t callback); + + MissionRawImpl(const MissionRawImpl &) = delete; + const MissionRawImpl &operator=(const MissionRawImpl &) = delete; + +private: + void process_mission_ack(const mavlink_message_t &message); + void process_mission_count(const mavlink_message_t &message); + void process_mission_item_int(const mavlink_message_t &message); + void do_download_step(); + void request_list(); + void request_item(); + void send_ack(); + + struct MissionChanged { + std::mutex mutex{}; + MissionRaw::mission_changed_callback_t callback{nullptr}; + } _mission_changed{}; + + struct MissionDownload { + std::mutex mutex{}; + enum class State { NONE, REQUEST_LIST, REQUEST_ITEM, SHOULD_ACK } state{State::NONE}; + unsigned retries{0}; + std::vector> + mavlink_mission_items_downloaded{}; + MissionRaw::mission_items_and_result_callback_t callback{nullptr}; + unsigned num_mission_items_to_download{0}; + unsigned next_mission_item_to_download{0}; + } _mission_download{}; + + void *_timeout_cookie{nullptr}; + static constexpr double RETRY_TIMEOUT_S = 0.250; +}; + +} // namespace dronecode_sdk