From 0a16fc4fff1651a5a8e5113bf2775aee5000ea50 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 18 Jan 2019 11:40:38 +0100 Subject: [PATCH 01/11] plugins: added skeleton for mission_raw plugin This will be a plugin which enables direct/raw access to MAVLink MISSION_ITEM_INT messages. --- plugins/CMakeLists.txt | 1 + plugins/mission/mission.cpp | 10 +- plugins/mission_raw/CMakeLists.txt | 33 +++++ .../include/plugins/mission_raw/mission_raw.h | 133 ++++++++++++++++++ plugins/mission_raw/mission_raw.cpp | 47 +++++++ plugins/mission_raw/mission_raw_impl.cpp | 61 ++++++++ plugins/mission_raw/mission_raw_impl.h | 33 +++++ 7 files changed, 314 insertions(+), 4 deletions(-) create mode 100644 plugins/mission_raw/CMakeLists.txt create mode 100644 plugins/mission_raw/include/plugins/mission_raw/mission_raw.h create mode 100644 plugins/mission_raw/mission_raw.cpp create mode 100644 plugins/mission_raw/mission_raw_impl.cpp create mode 100644 plugins/mission_raw/mission_raw_impl.h 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/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_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..2ebb595bf8 --- /dev/null +++ b/plugins/mission_raw/mission_raw_impl.cpp @@ -0,0 +1,61 @@ +#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); +} + +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); + UNUSED(mission_ack); + + // TODO: need to report that mission might have changed. +} + +void MissionRawImpl::download_mission_async( + const MissionRaw::mission_items_and_result_callback_t &callback) +{ + UNUSED(callback); +} + +void MissionRawImpl::download_mission_cancel() {} + +void MissionRawImpl::subscribe_mission_changed(MissionRaw::mission_changed_callback_t callback) +{ + UNUSED(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..8adaeb3a50 --- /dev/null +++ b/plugins/mission_raw/mission_raw_impl.h @@ -0,0 +1,33 @@ +#pragma once + +#include "system.h" +#include "mavlink_include.h" +#include "plugins/mission_raw/mission_raw.h" +#include "plugin_impl_base.h" + +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); +}; + +} // namespace dronecode_sdk From bf935b5e9f3e0c3d8f034d98ec937928ba4625c9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 22 Jan 2019 09:20:45 +0100 Subject: [PATCH 02/11] ycm_extra_conf: add mission_raw plugin --- .ycm_extra_conf.py | 2 ++ 1 file changed, 2 insertions(+) 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', From c1ae35200e113e8218cd4ebc09826aeb73ce3a0c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 22 Jan 2019 09:58:44 +0100 Subject: [PATCH 03/11] mission_raw: add mission changed subscription --- integration_tests/CMakeLists.txt | 2 + .../mission_raw_mission_changed.cpp | 63 +++++++++++++++++++ plugins/mission_raw/mission_raw_impl.cpp | 17 ++++- plugins/mission_raw/mission_raw_impl.h | 6 ++ 4 files changed, 85 insertions(+), 3 deletions(-) create mode 100644 integration_tests/mission_raw_mission_changed.cpp 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..3d84869add --- /dev/null +++ b/integration_tests/mission_raw_mission_changed.cpp @@ -0,0 +1,63 @@ +#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; +using namespace std::placeholders; // for `_1` + +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); + + auto new_item = std::make_shared(); + new_item->set_position(47.398170327054473, 8.5456490218639658); + new_item->set_relative_altitude(10.0f); + new_item->set_speed(5.0f); + std::vector> mission_items; + 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. + auto prom = std::make_shared>(); + auto future_result = 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 = future_result.wait_for(std::chrono::seconds(2)); + ASSERT_EQ(status, std::future_status::ready); + future_result.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); +} diff --git a/plugins/mission_raw/mission_raw_impl.cpp b/plugins/mission_raw/mission_raw_impl.cpp index 2ebb595bf8..caa5b293f1 100644 --- a/plugins/mission_raw/mission_raw_impl.cpp +++ b/plugins/mission_raw/mission_raw_impl.cpp @@ -40,9 +40,19 @@ void MissionRawImpl::process_mission_ack(const mavlink_message_t &message) { mavlink_mission_ack_t mission_ack; mavlink_msg_mission_ack_decode(&message, &mission_ack); - UNUSED(mission_ack); - // TODO: need to report that mission might have changed. + 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( @@ -55,7 +65,8 @@ void MissionRawImpl::download_mission_cancel() {} void MissionRawImpl::subscribe_mission_changed(MissionRaw::mission_changed_callback_t callback) { - UNUSED(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 index 8adaeb3a50..3b4e40b506 100644 --- a/plugins/mission_raw/mission_raw_impl.h +++ b/plugins/mission_raw/mission_raw_impl.h @@ -4,6 +4,7 @@ #include "mavlink_include.h" #include "plugins/mission_raw/mission_raw.h" #include "plugin_impl_base.h" +#include namespace dronecode_sdk { @@ -28,6 +29,11 @@ class MissionRawImpl : public PluginImplBase { private: void process_mission_ack(const mavlink_message_t &message); + + struct MissionChanged { + std::mutex mutex{}; + MissionRaw::mission_changed_callback_t callback{nullptr}; + } _mission_changed{}; }; } // namespace dronecode_sdk From e2083c118c2f20c46004704d14b427fd160b007b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 22 Jan 2019 14:08:32 +0100 Subject: [PATCH 04/11] mission_raw: add mission item download --- .../mission_raw_mission_changed.cpp | 30 +- plugins/mission_raw/mission_raw_impl.cpp | 261 +++++++++++++++++- plugins/mission_raw/mission_raw_impl.h | 20 ++ 3 files changed, 302 insertions(+), 9 deletions(-) diff --git a/integration_tests/mission_raw_mission_changed.cpp b/integration_tests/mission_raw_mission_changed.cpp index 3d84869add..66a0dbc395 100644 --- a/integration_tests/mission_raw_mission_changed.cpp +++ b/integration_tests/mission_raw_mission_changed.cpp @@ -2,11 +2,9 @@ #include "dronecode_sdk.h" #include "plugins/mission/mission.h" #include "plugins/mission_raw/mission_raw.h" -#include #include using namespace dronecode_sdk; -using namespace std::placeholders; // for `_1` TEST_F(SitlTest, MissionRawMissionChanged) { @@ -45,19 +43,35 @@ TEST_F(SitlTest, MissionRawMissionChanged) LogInfo() << "Uploading mission..."; // 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->upload_mission_async(mission_items, [prom](Mission::Result result) { + 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(); + prom.set_value(); LogInfo() << "Mission uploaded."; }); - auto status = future_result.wait_for(std::chrono::seconds(2)); + auto status = fut.wait_for(std::chrono::seconds(2)); ASSERT_EQ(status, std::future_status::ready); - future_result.get(); + 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 + UNUSED(items); + prom.set_value(); + }); + auto status = fut.wait_for(std::chrono::seconds(2)); + ASSERT_EQ(status, std::future_status::ready); + } } diff --git a/plugins/mission_raw/mission_raw_impl.cpp b/plugins/mission_raw/mission_raw_impl.cpp index caa5b293f1..e0edf9326f 100644 --- a/plugins/mission_raw/mission_raw_impl.cpp +++ b/plugins/mission_raw/mission_raw_impl.cpp @@ -25,6 +25,16 @@ void MissionRawImpl::init() 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() @@ -58,7 +68,256 @@ void MissionRawImpl::process_mission_ack(const mavlink_message_t &message) void MissionRawImpl::download_mission_async( const MissionRaw::mission_items_and_result_callback_t &callback) { - UNUSED(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() {} diff --git a/plugins/mission_raw/mission_raw_impl.h b/plugins/mission_raw/mission_raw_impl.h index 3b4e40b506..9a7bfae83b 100644 --- a/plugins/mission_raw/mission_raw_impl.h +++ b/plugins/mission_raw/mission_raw_impl.h @@ -29,11 +29,31 @@ class MissionRawImpl : public PluginImplBase { 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 From 369bf1d45133d735d532545b962f71db753f0478 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 22 Jan 2019 15:19:09 +0100 Subject: [PATCH 05/11] integration_tests: check downloaded MAVLink items --- .../mission_raw_mission_changed.cpp | 44 ++++++++++++++++--- 1 file changed, 38 insertions(+), 6 deletions(-) diff --git a/integration_tests/mission_raw_mission_changed.cpp b/integration_tests/mission_raw_mission_changed.cpp index 66a0dbc395..3b6419f27b 100644 --- a/integration_tests/mission_raw_mission_changed.cpp +++ b/integration_tests/mission_raw_mission_changed.cpp @@ -2,10 +2,21 @@ #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; @@ -32,12 +43,15 @@ TEST_F(SitlTest, MissionRawMissionChanged) // The mission change callback should not trigger yet. EXPECT_EQ(fut_changed.wait_for(std::chrono::milliseconds(500)), std::future_status::timeout); - auto new_item = std::make_shared(); - new_item->set_position(47.398170327054473, 8.5456490218639658); - new_item->set_relative_altitude(10.0f); - new_item->set_speed(5.0f); std::vector> mission_items; - mission_items.push_back(new_item); + + 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..."; @@ -68,10 +82,28 @@ TEST_F(SitlTest, MissionRawMissionChanged) std::vector> items) { EXPECT_EQ(result, MissionRaw::Result::SUCCESS); // TODO: validate items - UNUSED(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; + } + } +} From 2226dd40d93c9416ecd715957e35931d984f7a15 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 22 Jan 2019 15:19:34 +0100 Subject: [PATCH 06/11] mission: don't cast blindly but round properly --- .../mission_raw_mission_changed.cpp | 17 ++++++++--------- plugins/mission/mission_item_impl.cpp | 4 ++-- 2 files changed, 10 insertions(+), 11 deletions(-) diff --git a/integration_tests/mission_raw_mission_changed.cpp b/integration_tests/mission_raw_mission_changed.cpp index 3b6419f27b..c422dc75ef 100644 --- a/integration_tests/mission_raw_mission_changed.cpp +++ b/integration_tests/mission_raw_mission_changed.cpp @@ -13,9 +13,8 @@ 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); - +static void +validate_items(const std::vector> &items); TEST_F(SitlTest, MissionRawMissionChanged) { @@ -96,14 +95,14 @@ void validate_items(const std::vectorcommand, 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; + 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; + EXPECT_EQ(items[i]->param2, SOME_SPEEDS[i / 2]); + LogWarn() << "i/2: " << i / 2; } } } diff --git a/plugins/mission/mission_item_impl.cpp b/plugins/mission/mission_item_impl.cpp index c0a6e87f48..379a49ece8 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 std::round(_latitude_deg * 1e7); } int32_t MissionItemImpl::get_mavlink_y() const { - return int32_t(_longitude_deg * 1e7); + return std::round(_longitude_deg * 1e7); } float MissionItemImpl::get_mavlink_z() const From 56636a486768d4d8ca20a2adb0a69762751ba601 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 22 Jan 2019 15:23:44 +0100 Subject: [PATCH 07/11] action: fix rounding --- plugins/action/action_impl.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) 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)); From d7f56bcccca84e13fb2cae70c5ff7c9f0dccb186 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 22 Jan 2019 15:23:54 +0100 Subject: [PATCH 08/11] follow_me: fix rounding --- plugins/follow_me/follow_me_impl.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/plugins/follow_me/follow_me_impl.cpp b/plugins/follow_me/follow_me_impl.cpp index 24bb81d859..05926090ae 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 = std::round(_target_location.latitude_deg * 1e7); + const int32_t lon_int = std::round(_target_location.longitude_deg * 1e7); const float alt = static_cast(_target_location.absolute_altitude_m); _mutex.unlock(); From 4b3c0f5fd5d8d4cada87e42af6f4dcbe181bab78 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 22 Jan 2019 15:24:05 +0100 Subject: [PATCH 09/11] gimbal: fix rounding --- plugins/gimbal/gimbal_impl.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) 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(); From aaa4a7e2c3a2fa3584852820f37e519cfb606c02 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 24 Jan 2019 14:20:20 +0100 Subject: [PATCH 10/11] mission: don't cast implicitly --- plugins/mission/mission_item_impl.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/plugins/mission/mission_item_impl.cpp b/plugins/mission/mission_item_impl.cpp index 379a49ece8..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 std::round(_latitude_deg * 1e7); + return int32_t(std::round(_latitude_deg * 1e7)); } int32_t MissionItemImpl::get_mavlink_y() const { - return std::round(_longitude_deg * 1e7); + return int32_t(std::round(_longitude_deg * 1e7)); } float MissionItemImpl::get_mavlink_z() const From aa5952a890ffc5dc71c4fdfa7fca2c346d7afce3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 24 Jan 2019 15:00:32 +0100 Subject: [PATCH 11/11] follow_me: fix casting for MSVC --- plugins/follow_me/follow_me_impl.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/plugins/follow_me/follow_me_impl.cpp b/plugins/follow_me/follow_me_impl.cpp index 05926090ae..90ccbcf6b5 100644 --- a/plugins/follow_me/follow_me_impl.cpp +++ b/plugins/follow_me/follow_me_impl.cpp @@ -254,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 = std::round(_target_location.latitude_deg * 1e7); - const int32_t lon_int = std::round(_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();