From 30d2693417fe2afdfe494ace3e0f5b8ee5331f03 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 16 Feb 2024 14:21:51 +1300 Subject: [PATCH 1/2] core: fix autopilot for missions and params It turns out we were using our instead of the system's autopilot type (e.g. PX4 or ArduPilot) in the transfer from the mission client and param client. This should fix the timeout happening for ArduPilot (as ArduPilot responds to MISSION_COUNT with MISSION_REQUEST instead of MISSION_REQUEST_INT). Signed-off-by: Julian Oes --- src/mavsdk/core/autopilot.h | 2 +- src/mavsdk/core/autopilot_callback.h | 10 ++++++++++ .../core/mavlink_mission_transfer_client.cpp | 17 +++++++++++------ .../core/mavlink_mission_transfer_client.h | 10 ++++++++-- .../mavlink_mission_transfer_client_test.cpp | 7 ++++++- src/mavsdk/core/mavlink_parameter_client.cpp | 10 ++++++---- src/mavsdk/core/mavlink_parameter_client.h | 3 +++ src/mavsdk/core/system_impl.cpp | 4 +++- src/mavsdk/core/timeout_s_callback.h | 6 ++++++ 9 files changed, 54 insertions(+), 15 deletions(-) create mode 100644 src/mavsdk/core/autopilot_callback.h diff --git a/src/mavsdk/core/autopilot.h b/src/mavsdk/core/autopilot.h index e950520217..f5b339e668 100644 --- a/src/mavsdk/core/autopilot.h +++ b/src/mavsdk/core/autopilot.h @@ -8,4 +8,4 @@ enum class Autopilot { ArduPilot, }; -} // namespace mavsdk \ No newline at end of file +} // namespace mavsdk diff --git a/src/mavsdk/core/autopilot_callback.h b/src/mavsdk/core/autopilot_callback.h new file mode 100644 index 0000000000..b9ce27299c --- /dev/null +++ b/src/mavsdk/core/autopilot_callback.h @@ -0,0 +1,10 @@ +#pragma once + +#include "autopilot.h" +#include + +namespace mavsdk { + +using AutopilotCallback = std::function; + +} // namespace mavsdk diff --git a/src/mavsdk/core/mavlink_mission_transfer_client.cpp b/src/mavsdk/core/mavlink_mission_transfer_client.cpp index b326586926..b97bf70552 100644 --- a/src/mavsdk/core/mavlink_mission_transfer_client.cpp +++ b/src/mavsdk/core/mavlink_mission_transfer_client.cpp @@ -9,11 +9,13 @@ MavlinkMissionTransferClient::MavlinkMissionTransferClient( Sender& sender, MavlinkMessageHandler& message_handler, TimeoutHandler& timeout_handler, - TimeoutSCallback timeout_s_callback) : + TimeoutSCallback timeout_s_callback, + AutopilotCallback autopilot_callback) : _sender(sender), _message_handler(message_handler), _timeout_handler(timeout_handler), - _timeout_s_callback(std::move(timeout_s_callback)) + _timeout_s_callback(std::move(timeout_s_callback)), + _autopilot_callback(std::move(autopilot_callback)) { if (const char* env_p = std::getenv("MAVSDK_MISSION_TRANSFER_DEBUGGING")) { if (std::string(env_p) == "1") { @@ -49,7 +51,8 @@ MavlinkMissionTransferClient::upload_items_async( callback, progress_callback, _debugging, - target_system_id); + target_system_id, + _autopilot_callback()); _work_queue.push_back(ptr); @@ -181,12 +184,14 @@ MavlinkMissionTransferClient::UploadWorkItem::UploadWorkItem( ResultCallback callback, ProgressCallback progress_callback, bool debugging, - uint8_t target_system_id) : + uint8_t target_system_id, + Autopilot autopilot) : WorkItem(sender, message_handler, timeout_handler, type, timeout_s, debugging), _items(items), _callback(callback), _progress_callback(progress_callback), - _target_system_id(target_system_id) + _target_system_id(target_system_id), + _autopilot(autopilot) { _message_handler.register_one( MAVLINK_MSG_ID_MISSION_REQUEST, @@ -322,7 +327,7 @@ void MavlinkMissionTransferClient::UploadWorkItem::process_mission_request( mavlink_mission_request_t request; mavlink_msg_mission_request_decode(&request_message, &request); - if (_sender.autopilot() == Autopilot::ArduPilot) { + if (_autopilot == Autopilot::ArduPilot) { // ArduCopter 3.6 sends MISSION_REQUEST (not _INT) but actually accepts ITEM_INT in reply // FIXME: this will mess with the sequence number. diff --git a/src/mavsdk/core/mavlink_mission_transfer_client.h b/src/mavsdk/core/mavlink_mission_transfer_client.h index 6ac379d682..cfe83fdfb8 100644 --- a/src/mavsdk/core/mavlink_mission_transfer_client.h +++ b/src/mavsdk/core/mavlink_mission_transfer_client.h @@ -6,6 +6,8 @@ #include #include #include +#include "autopilot.h" +#include "autopilot_callback.h" #include "mavlink_address.h" #include "mavlink_include.h" #include "mavlink_message_handler.h" @@ -110,7 +112,8 @@ class MavlinkMissionTransferClient { ResultCallback callback, ProgressCallback progress_callback, bool debugging, - uint8_t target_system_id); + uint8_t target_system_id, + Autopilot autopilot); ~UploadWorkItem() override; void start() override; @@ -147,6 +150,7 @@ class MavlinkMissionTransferClient { unsigned _retries_done{0}; uint8_t _target_system_id; + Autopilot _autopilot; }; class DownloadWorkItem : public WorkItem { @@ -272,7 +276,8 @@ class MavlinkMissionTransferClient { Sender& sender, MavlinkMessageHandler& message_handler, TimeoutHandler& timeout_handler, - TimeoutSCallback get_timeout_s_callback); + TimeoutSCallback get_timeout_s_callback, + AutopilotCallback autopilot_callback); ~MavlinkMissionTransferClient() = default; @@ -307,6 +312,7 @@ class MavlinkMissionTransferClient { MavlinkMessageHandler& _message_handler; TimeoutHandler& _timeout_handler; TimeoutSCallback _timeout_s_callback; + AutopilotCallback _autopilot_callback; LockedQueue _work_queue{}; diff --git a/src/mavsdk/core/mavlink_mission_transfer_client_test.cpp b/src/mavsdk/core/mavlink_mission_transfer_client_test.cpp index 32840269c3..bcb98de1f8 100644 --- a/src/mavsdk/core/mavlink_mission_transfer_client_test.cpp +++ b/src/mavsdk/core/mavlink_mission_transfer_client_test.cpp @@ -34,7 +34,12 @@ class MavlinkMissionTransferClientTest : public ::testing::Test { MavlinkMissionTransferClientTest() : ::testing::Test(), timeout_handler(time), - mmt(mock_sender, message_handler, timeout_handler, []() { return timeout_s; }) + mmt( + mock_sender, + message_handler, + timeout_handler, + []() { return timeout_s; }, + []() { return Autopilot::Px4; }) {} void SetUp() override diff --git a/src/mavsdk/core/mavlink_parameter_client.cpp b/src/mavsdk/core/mavlink_parameter_client.cpp index 293c6a82f7..1bd8d44363 100644 --- a/src/mavsdk/core/mavlink_parameter_client.cpp +++ b/src/mavsdk/core/mavlink_parameter_client.cpp @@ -15,6 +15,7 @@ MavlinkParameterClient::MavlinkParameterClient( MavlinkMessageHandler& message_handler, TimeoutHandler& timeout_handler, TimeoutSCallback timeout_s_callback, + AutopilotCallback autopilot_callback, uint8_t target_system_id, uint8_t target_component_id, bool use_extended) : @@ -22,6 +23,7 @@ MavlinkParameterClient::MavlinkParameterClient( _message_handler(message_handler), _timeout_handler(timeout_handler), _timeout_s_callback(std::move(timeout_s_callback)), + _autopilot_callback(std::move(autopilot_callback)), _target_system_id(target_system_id), _target_component_id(target_component_id), _use_extended(use_extended) @@ -115,7 +117,7 @@ void MavlinkParameterClient::set_param_int_async( // PX4 only uses int32_t, so we can be sure and don't need to check the exact type first // by getting the param, or checking the cache. - if (_sender.autopilot() == Autopilot::Px4) { + if (_autopilot_callback() == Autopilot::Px4) { ParamValue value_to_set; value_to_set.set(static_cast(value)); set_param_async(name, value_to_set, callback, cookie); @@ -499,7 +501,7 @@ bool MavlinkParameterClient::send_set_param_message(WorkItemSet& work_item) return message; }); } else { - const float value_set = (_sender.autopilot() == Autopilot::ArduPilot) ? + const float value_set = (_autopilot_callback() == Autopilot::ArduPilot) ? work_item.param_value.get_4_float_bytes_cast() : work_item.param_value.get_4_float_bytes_bytewise(); @@ -632,8 +634,8 @@ void MavlinkParameterClient::process_param_value(const mavlink_message_t& messag ParamValue received_value; const bool set_value_success = received_value.set_from_mavlink_param_value( param_value, - (_sender.autopilot() == Autopilot::ArduPilot) ? ParamValue::Conversion::Cast : - ParamValue::Conversion::Bitwise); + (_autopilot_callback() == Autopilot::ArduPilot) ? ParamValue::Conversion::Cast : + ParamValue::Conversion::Bitwise); if (!set_value_success) { LogWarn() << "Got ill-formed param_ext_value message (param_type unknown)"; return; diff --git a/src/mavsdk/core/mavlink_parameter_client.h b/src/mavsdk/core/mavlink_parameter_client.h index 0184ad0c0f..5b38f1d45f 100644 --- a/src/mavsdk/core/mavlink_parameter_client.h +++ b/src/mavsdk/core/mavlink_parameter_client.h @@ -1,5 +1,6 @@ #pragma once +#include "autopilot_callback.h" #include "log.h" #include "mavlink_include.h" #include "timeout_s_callback.h" @@ -34,6 +35,7 @@ class MavlinkParameterClient : public MavlinkParameterSubscription { MavlinkMessageHandler& message_handler, TimeoutHandler& timeout_handler, TimeoutSCallback timeout_s_callback, + AutopilotCallback autopilot_callback, uint8_t target_system_id, uint8_t target_component_id = MAV_COMP_ID_AUTOPILOT1, bool use_extended_protocol = false); @@ -195,6 +197,7 @@ class MavlinkParameterClient : public MavlinkParameterSubscription { MavlinkMessageHandler& _message_handler; TimeoutHandler& _timeout_handler; TimeoutSCallback _timeout_s_callback; + AutopilotCallback _autopilot_callback; uint8_t _target_system_id = 0; uint8_t _target_component_id = MAV_COMP_ID_AUTOPILOT1; bool _use_extended = false; diff --git a/src/mavsdk/core/system_impl.cpp b/src/mavsdk/core/system_impl.cpp index 77a8129fed..5474263519 100644 --- a/src/mavsdk/core/system_impl.cpp +++ b/src/mavsdk/core/system_impl.cpp @@ -30,7 +30,8 @@ SystemImpl::SystemImpl(MavsdkImpl& mavsdk_impl) : _mavsdk_impl.default_server_component_impl().sender(), _mavlink_message_handler, _mavsdk_impl.timeout_handler, - [this]() { return timeout_s(); }), + [this]() { return timeout_s(); }, + [this]() { return autopilot(); }), _request_message( *this, _command_sender, _mavlink_message_handler, _mavsdk_impl.timeout_handler), _mavlink_ftp_client(*this) @@ -1348,6 +1349,7 @@ MavlinkParameterClient* SystemImpl::param_sender(uint8_t component_id, bool exte _mavlink_message_handler, _mavsdk_impl.timeout_handler, [this]() { return timeout_s(); }, + [this]() { return autopilot(); }, get_system_id(), component_id, extended), diff --git a/src/mavsdk/core/timeout_s_callback.h b/src/mavsdk/core/timeout_s_callback.h index 38bff53f06..78677b2996 100644 --- a/src/mavsdk/core/timeout_s_callback.h +++ b/src/mavsdk/core/timeout_s_callback.h @@ -1,3 +1,9 @@ #pragma once +#include + +namespace mavsdk { + using TimeoutSCallback = std::function; + +} // namespace mavsdk From dc919d02e373e7cac9c1498d8effaecdca3b52ee Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 16 Feb 2024 14:25:04 +1300 Subject: [PATCH 2/2] mission_raw: fix sequence for ArduPilot We need to enter the home item before adding the sequence numbering. Signed-off-by: Julian Oes --- src/mavsdk/plugins/mission_raw/mission_import.cpp | 12 ++++++------ .../plugins/mission_raw/mission_import_test.cpp | 9 ++++++--- 2 files changed, 12 insertions(+), 9 deletions(-) diff --git a/src/mavsdk/plugins/mission_raw/mission_import.cpp b/src/mavsdk/plugins/mission_raw/mission_import.cpp index e5cddac4ec..82c1fed1f6 100644 --- a/src/mavsdk/plugins/mission_raw/mission_import.cpp +++ b/src/mavsdk/plugins/mission_raw/mission_import.cpp @@ -112,12 +112,6 @@ MissionImport::import_mission(const Json::Value& root, Autopilot autopilot) mission_items[0].current = 1; } - // Don't forget sequence number - unsigned sequence = 0; - for (auto& mission_item : mission_items) { - mission_item.seq = sequence++; - } - // Add home position at 0 for ArduPilot if (autopilot == Autopilot::ArduPilot) { const auto home = mission["plannedHomePosition"]; @@ -146,6 +140,12 @@ MissionImport::import_mission(const Json::Value& root, Autopilot autopilot) } } + // Don't forget sequence number + unsigned sequence = 0; + for (auto& mission_item : mission_items) { + mission_item.seq = sequence++; + } + // Returning an empty vector is ok here if there were really no mission items. return {mission_items}; } diff --git a/src/mavsdk/plugins/mission_raw/mission_import_test.cpp b/src/mavsdk/plugins/mission_raw/mission_import_test.cpp index d10d50f586..f0088e7f94 100644 --- a/src/mavsdk/plugins/mission_raw/mission_import_test.cpp +++ b/src/mavsdk/plugins/mission_raw/mission_import_test.cpp @@ -1,3 +1,4 @@ +#include #include #include #include // for `std::ifstream` @@ -406,6 +407,11 @@ TEST(MissionRaw, ImportSamplePlanWithArduPilot) { auto reference_items = create_reference_items(); + // We need to adjust the sequence plus one because we're about to add in home + // at 0. + + std::for_each(reference_items.begin(), reference_items.end(), [](auto& item) { ++item.seq; }); + auto home_item = MissionRaw::MissionItem{ 0, MAV_FRAME_GLOBAL_INT, @@ -423,9 +429,6 @@ TEST(MissionRaw, ImportSamplePlanWithArduPilot) reference_items.insert(reference_items.begin(), home_item); - // There is no need to increment the sequence for all items, the sequence - // still starts at 0 after home. - std::ifstream file{path_prefix("qgroundcontrol_sample.plan")}; ASSERT_TRUE(file);