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 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);