From 521904a0d16311227c714728d42c0f3c9413a6a7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 12 Dec 2024 12:20:56 +1300 Subject: [PATCH 1/6] core: fix parameter cache It turns out the logic to check which params were missing was broken. This would retransmit way too many parameters, so the process takes longer but would still end successful, eventually. --- src/mavsdk/core/mavlink_parameter_cache.cpp | 44 +++++++++++++++++---- src/mavsdk/core/mavlink_parameter_cache.h | 3 ++ 2 files changed, 40 insertions(+), 7 deletions(-) diff --git a/src/mavsdk/core/mavlink_parameter_cache.cpp b/src/mavsdk/core/mavlink_parameter_cache.cpp index e580ad709b..64c67270bb 100644 --- a/src/mavsdk/core/mavlink_parameter_cache.cpp +++ b/src/mavsdk/core/mavlink_parameter_cache.cpp @@ -130,6 +130,14 @@ bool MavlinkParameterCache::exists(const std::string& param_id) const return it != _all_params.end(); } +bool MavlinkParameterCache::exists(uint16_t param_index) const +{ + auto it = std::find_if(_all_params.begin(), _all_params.end(), [&](const auto& param) { + return (param_index == param.index); + }); + return it != _all_params.end(); +} + std::optional MavlinkParameterCache::next_missing_index(uint16_t count) { // Extended doesn't matter here because we use this function in the sender @@ -139,16 +147,38 @@ std::optional MavlinkParameterCache::next_missing_index(uint16_t count }); for (unsigned i = 0; i < count; ++i) { - if (_all_params.size() <= i) { - // We have reached the end but it's not complete yet. - return i; - } - if (_all_params[i].index > i) { - // We have found a hole to fill. - return i; + if (!exists(i)) { + return {i}; } } return {}; } +void MavlinkParameterCache::print_missing(uint16_t count) +{ + // Extended doesn't matter here because we use this function in the sender + // which is always either all extended or not. + std::sort(_all_params.begin(), _all_params.end(), [](const auto& lhs, const auto& rhs) { + return lhs.index < rhs.index; + }); + + LogDebug() << "Available: "; + for (auto param : _all_params) { + LogDebug() << param.index << ": " << param.id; + } + LogDebug() << "Available count: " << _all_params.size(); + + unsigned missing = 0; + LogDebug() << "Missing: "; + for (unsigned i = 0; i < count; ++i) { + if (!exists(i)) { + // We have reached the end but it's not complete yet. + LogDebug() << i; + ++missing; + } + } + + LogDebug() << "Missing count: " << missing; +} + } // namespace mavsdk diff --git a/src/mavsdk/core/mavlink_parameter_cache.h b/src/mavsdk/core/mavlink_parameter_cache.h index 420d5a281c..12c118dde7 100644 --- a/src/mavsdk/core/mavlink_parameter_cache.h +++ b/src/mavsdk/core/mavlink_parameter_cache.h @@ -46,10 +46,13 @@ class MavlinkParameterCache { [[nodiscard]] std::optional next_missing_index(uint16_t count); + void print_missing(uint16_t count); + void clear(); private: [[nodiscard]] bool exists(const std::string& param_id) const; + [[nodiscard]] bool exists(uint16_t param_index) const; std::vector _all_params; }; From 95b688c81d17213056dc35d58f5da457379c0bd4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 12 Dec 2024 12:22:35 +1300 Subject: [PATCH 2/6] core: trigger param retransmission later When downloading params over a SiK radio there can be bursts where many params arrive, and longer periods where we have to wait. Therefore, wait a bit before triggering the retransmission. --- src/mavsdk/core/mavlink_parameter_client.cpp | 7 ++++--- src/mavsdk/core/mavlink_parameter_client.h | 2 ++ 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/mavsdk/core/mavlink_parameter_client.cpp b/src/mavsdk/core/mavlink_parameter_client.cpp index c45c3e5c2d..5d9b86b776 100644 --- a/src/mavsdk/core/mavlink_parameter_client.cpp +++ b/src/mavsdk/core/mavlink_parameter_client.cpp @@ -494,8 +494,8 @@ void MavlinkParameterClient::do_work() } work->already_requested = true; // We want to get notified if a timeout happens - _timeout_cookie = - _timeout_handler.add([this] { receive_timeout(); }, _timeout_s_callback()); + _timeout_cookie = _timeout_handler.add( + [this] { receive_timeout(); }, _timeout_s_callback() * _get_all_timeout_factor); }}, work->work_item_variant); } @@ -1133,7 +1133,8 @@ void MavlinkParameterClient::receive_timeout() // We want to get notified if a timeout happens _timeout_cookie = _timeout_handler.add( - [this] { receive_timeout(); }, _timeout_s_callback()); + [this] { receive_timeout(); }, + _timeout_s_callback() * _get_all_timeout_factor); } else { if (item.callback) { auto callback = item.callback; diff --git a/src/mavsdk/core/mavlink_parameter_client.h b/src/mavsdk/core/mavlink_parameter_client.h index 7f07813e9d..48fe443525 100644 --- a/src/mavsdk/core/mavlink_parameter_client.h +++ b/src/mavsdk/core/mavlink_parameter_client.h @@ -152,6 +152,8 @@ class MavlinkParameterClient : public MavlinkParameterSubscription { friend std::ostream& operator<<(std::ostream&, const Result&); private: + const double _get_all_timeout_factor = 4.0; + struct WorkItemSet { const std::string param_name; const ParamValue param_value; From 70a639c6cff38ae5cbe3141d554f8d2373def293 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 12 Dec 2024 12:24:49 +1300 Subject: [PATCH 3/6] core: ignore PX4's _HASH_CHECK param This comes with index -1, so it's confusing. --- src/mavsdk/core/mavlink_parameter_client.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/mavsdk/core/mavlink_parameter_client.cpp b/src/mavsdk/core/mavlink_parameter_client.cpp index 5d9b86b776..514537cc16 100644 --- a/src/mavsdk/core/mavlink_parameter_client.cpp +++ b/src/mavsdk/core/mavlink_parameter_client.cpp @@ -5,6 +5,7 @@ #include "plugin_base.h" #include #include +#include #include namespace mavsdk { @@ -669,6 +670,10 @@ void MavlinkParameterClient::process_param_value(const mavlink_message_t& messag if (_parameter_debugging) { LogDebug() << "process_param_value: " << safe_param_id << " " << received_value; + + if (param_value.param_index == std::numeric_limits::max()) { + // Ignore PX4's _HASH_CHECK param. + return; } // We need to use a unique pointer here to remove the lock from the work queue manually "early" From 704c49f3b768543e16fea495816126e4267998f3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 12 Dec 2024 12:25:44 +1300 Subject: [PATCH 4/6] core: fix param retransmission logic (and debug) This fixes the logic of retransmission in case we started retransmitting too early and are still receiving parameters from the initial burst. In that case, we accept whatever comes in, while also requesting what we haven't yet received. --- src/mavsdk/core/mavlink_parameter_client.cpp | 82 +++++++++++++------- 1 file changed, 52 insertions(+), 30 deletions(-) diff --git a/src/mavsdk/core/mavlink_parameter_client.cpp b/src/mavsdk/core/mavlink_parameter_client.cpp index 514537cc16..7a3dfb132e 100644 --- a/src/mavsdk/core/mavlink_parameter_client.cpp +++ b/src/mavsdk/core/mavlink_parameter_client.cpp @@ -669,7 +669,8 @@ void MavlinkParameterClient::process_param_value(const mavlink_message_t& messag } if (_parameter_debugging) { - LogDebug() << "process_param_value: " << safe_param_id << " " << received_value; + LogDebug() << "process_param_value: " << safe_param_id << " " << received_value << ", index: " << param_value.param_index; + } if (param_value.param_index == std::numeric_limits::max()) { // Ignore PX4's _HASH_CHECK param. @@ -772,6 +773,9 @@ void MavlinkParameterClient::process_param_value(const mavlink_message_t& messag } }, [&](WorkItemGetAll& item) { + + auto maybe_current_missing_index = _param_cache.next_missing_index(item.count); + switch (_param_cache.add_new_param( safe_param_id, received_value, param_value.param_index)) { case MavlinkParameterCache::AddNewParamResult::AlreadyExists: @@ -782,14 +786,21 @@ void MavlinkParameterClient::process_param_value(const mavlink_message_t& messag // params. case MavlinkParameterCache::AddNewParamResult::Ok: - item.count = param_value.param_count; + if (item.count != param_value.param_count) { + item.count = param_value.param_count; + if (_parameter_debugging) { + LogDebug() << "Count is now " << item.count; + } + } + if (_parameter_debugging) { - LogDebug() << "Count is now " << item.count; + LogDebug() << "Received param: " << param_value.param_index; } + if (_param_cache.count(_use_extended) == param_value.param_count) { _timeout_handler.remove(_timeout_cookie); if (_parameter_debugging) { - LogDebug() << "Param set complete: " + LogDebug() << "Getting all parameters complete: " << (_use_extended ? "extended" : "not extended"); } work_queue_guard->pop_front(); @@ -802,33 +813,39 @@ void MavlinkParameterClient::process_param_value(const mavlink_message_t& messag } } else { if (_parameter_debugging) { - LogDebug() << "Count expected " << _param_cache.count(_use_extended) - << " so far " << param_value.param_count; + LogDebug() << "Received " << _param_cache.count(_use_extended) + << " of " << param_value.param_count; } if (item.rerequesting) { - auto maybe_next_missing_index = - _param_cache.next_missing_index(item.count); - if (!maybe_next_missing_index.has_value()) { - LogErr() << "logic error, there should a missing index"; - assert(false); - } - if (_parameter_debugging) { - LogDebug() << "Requesting missing parameter " - << (int)maybe_next_missing_index.value(); - } + if (maybe_current_missing_index.has_value() && + maybe_current_missing_index.value() == param_value.param_index) { + + // We got what we were waiting for, request next one. + auto maybe_next_missing_index = + _param_cache.next_missing_index(item.count); + + if (!maybe_next_missing_index.has_value()) { + LogErr() << "logic error, there should a missing index"; + assert(false); + } + + if (_parameter_debugging) { + LogDebug() << "Requesting missing parameter " + << (int)maybe_next_missing_index.value(); + } - std::array param_id_buff{}; - if (!send_get_param_message( - param_id_buff, maybe_next_missing_index.value())) { - LogErr() << "Send message failed"; - work_queue_guard->pop_front(); - if (item.callback) { - auto callback = item.callback; - work_queue_guard.reset(); - callback(Result::ConnectionError, {}); + std::array param_id_buff{}; + if (!send_get_param_message( + param_id_buff, maybe_next_missing_index.value())) { + LogErr() << "Send message failed"; + work_queue_guard->pop_front(); + if (item.callback) { + auto callback = item.callback; + work_queue_guard.reset(); + callback(Result::ConnectionError, {}); + } } - return; } } else { // update the timeout handler, messages are still coming in. @@ -922,7 +939,7 @@ void MavlinkParameterClient::process_param_ext_value(const mavlink_message_t& me if (_param_cache.count(_use_extended) == param_ext_value.param_count) { _timeout_handler.remove(_timeout_cookie); if (_parameter_debugging) { - LogDebug() << "Param set complete: " + LogDebug() << "Getting all parameters complete: " << (_use_extended ? "extended" : "not extended"); } work_queue_guard->pop_front(); @@ -935,8 +952,8 @@ void MavlinkParameterClient::process_param_ext_value(const mavlink_message_t& me } } else { if (_parameter_debugging) { - LogDebug() << "Count expected " << _param_cache.count(_use_extended) - << " but is " << param_ext_value.param_count; + LogDebug() << "Received " << _param_cache.count(_use_extended) + << " of " << param_ext_value.param_count; } // update the timeout handler, messages are still coming in. _timeout_handler.refresh(_timeout_cookie); @@ -1136,7 +1153,7 @@ void MavlinkParameterClient::receive_timeout() return; } - // We want to get notified if a timeout happens + // We want to get notified if a timeout happens. _timeout_cookie = _timeout_handler.add( [this] { receive_timeout(); }, _timeout_s_callback() * _get_all_timeout_factor); @@ -1151,6 +1168,11 @@ void MavlinkParameterClient::receive_timeout() } else { item.rerequesting = true; + + if (_parameter_debugging) { + _param_cache.print_missing(item.count); + } + auto maybe_next_missing_index = _param_cache.next_missing_index(item.count); if (!maybe_next_missing_index.has_value()) { LogErr() << "logic error, there should a missing index"; From fd15b8201527b866d120db097ef4feb171f86b88 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 12 Dec 2024 12:31:10 +1300 Subject: [PATCH 5/6] core: add info regarding missing parameters --- src/mavsdk/core/mavlink_parameter_cache.cpp | 13 +++++++++++++ src/mavsdk/core/mavlink_parameter_cache.h | 3 +++ src/mavsdk/core/mavlink_parameter_client.cpp | 2 ++ 3 files changed, 18 insertions(+) diff --git a/src/mavsdk/core/mavlink_parameter_cache.cpp b/src/mavsdk/core/mavlink_parameter_cache.cpp index 64c67270bb..8e8dae2af4 100644 --- a/src/mavsdk/core/mavlink_parameter_cache.cpp +++ b/src/mavsdk/core/mavlink_parameter_cache.cpp @@ -138,6 +138,19 @@ bool MavlinkParameterCache::exists(uint16_t param_index) const return it != _all_params.end(); } +uint16_t MavlinkParameterCache::missing_count(uint16_t count) const +{ + uint16_t missing = 0; + + for (uint16_t i = 0; i < count; ++i) { + if (!exists(i)) { + ++missing; + } + } + + return missing; +} + std::optional MavlinkParameterCache::next_missing_index(uint16_t count) { // Extended doesn't matter here because we use this function in the sender diff --git a/src/mavsdk/core/mavlink_parameter_cache.h b/src/mavsdk/core/mavlink_parameter_cache.h index 12c118dde7..a5909b8cdf 100644 --- a/src/mavsdk/core/mavlink_parameter_cache.h +++ b/src/mavsdk/core/mavlink_parameter_cache.h @@ -2,6 +2,7 @@ #include "param_value.h" +#include #include #include #include @@ -46,6 +47,8 @@ class MavlinkParameterCache { [[nodiscard]] std::optional next_missing_index(uint16_t count); + [[nodiscard]] uint16_t missing_count(uint16_t count) const; + void print_missing(uint16_t count); void clear(); diff --git a/src/mavsdk/core/mavlink_parameter_client.cpp b/src/mavsdk/core/mavlink_parameter_client.cpp index 7a3dfb132e..8c5dc8365c 100644 --- a/src/mavsdk/core/mavlink_parameter_client.cpp +++ b/src/mavsdk/core/mavlink_parameter_client.cpp @@ -830,6 +830,8 @@ void MavlinkParameterClient::process_param_value(const mavlink_message_t& messag assert(false); } + LogInfo() << "Requesting " << _param_cache.missing_count() << " parameters missed during initial burst."; + if (_parameter_debugging) { LogDebug() << "Requesting missing parameter " << (int)maybe_next_missing_index.value(); From 80ae7492fe0baf94a6ffd92a3bcdb835936ce662 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 12 Dec 2024 13:46:56 +1300 Subject: [PATCH 6/6] core: speed up param retransmissions Instead of requesting the parameters which were missed in the initial burst one by one, we can now request them in chunks, e.g. 10 at a time. This speeds up the process quite a bit. --- src/mavsdk/core/mavlink_parameter_cache.cpp | 20 ++++- src/mavsdk/core/mavlink_parameter_cache.h | 10 ++- .../core/mavlink_parameter_cache_test.cpp | 38 +++++++-- src/mavsdk/core/mavlink_parameter_client.cpp | 83 +++++++++---------- src/mavsdk/core/mavlink_parameter_client.h | 2 + 5 files changed, 97 insertions(+), 56 deletions(-) diff --git a/src/mavsdk/core/mavlink_parameter_cache.cpp b/src/mavsdk/core/mavlink_parameter_cache.cpp index 8e8dae2af4..d45617906b 100644 --- a/src/mavsdk/core/mavlink_parameter_cache.cpp +++ b/src/mavsdk/core/mavlink_parameter_cache.cpp @@ -120,6 +120,7 @@ uint16_t MavlinkParameterCache::count(bool including_extended) const void MavlinkParameterCache::clear() { _all_params.clear(); + _last_missing_requested = {}; } bool MavlinkParameterCache::exists(const std::string& param_id) const @@ -151,7 +152,8 @@ uint16_t MavlinkParameterCache::missing_count(uint16_t count) const return missing; } -std::optional MavlinkParameterCache::next_missing_index(uint16_t count) +std::vector +MavlinkParameterCache::next_missing_indices(uint16_t count, uint16_t chunk_size) { // Extended doesn't matter here because we use this function in the sender // which is always either all extended or not. @@ -159,12 +161,22 @@ std::optional MavlinkParameterCache::next_missing_index(uint16_t count return lhs.index < rhs.index; }); + std::vector result; + for (unsigned i = 0; i < count; ++i) { - if (!exists(i)) { - return {i}; + if (exists(i)) { + continue; + } + + result.push_back(i); + _last_missing_requested = {i}; + + if (result.size() == chunk_size) { + break; } } - return {}; + + return result; } void MavlinkParameterCache::print_missing(uint16_t count) diff --git a/src/mavsdk/core/mavlink_parameter_cache.h b/src/mavsdk/core/mavlink_parameter_cache.h index a5909b8cdf..736d930fb5 100644 --- a/src/mavsdk/core/mavlink_parameter_cache.h +++ b/src/mavsdk/core/mavlink_parameter_cache.h @@ -5,6 +5,7 @@ #include #include #include +#include #include #include @@ -45,10 +46,15 @@ class MavlinkParameterCache { [[nodiscard]] uint16_t count(bool including_extended) const; - [[nodiscard]] std::optional next_missing_index(uint16_t count); + [[nodiscard]] std::vector next_missing_indices(uint16_t count, uint16_t chunk_size); [[nodiscard]] uint16_t missing_count(uint16_t count) const; + [[nodiscard]] std::optional last_missing_requested() const + { + return _last_missing_requested; + } + void print_missing(uint16_t count); void clear(); @@ -58,6 +64,8 @@ class MavlinkParameterCache { [[nodiscard]] bool exists(uint16_t param_index) const; std::vector _all_params; + + std::optional _last_missing_requested{}; }; } // namespace mavsdk diff --git a/src/mavsdk/core/mavlink_parameter_cache_test.cpp b/src/mavsdk/core/mavlink_parameter_cache_test.cpp index c6dbf904ed..03d1afb0fb 100644 --- a/src/mavsdk/core/mavlink_parameter_cache_test.cpp +++ b/src/mavsdk/core/mavlink_parameter_cache_test.cpp @@ -67,6 +67,7 @@ TEST(MavlinkParameterCache, AddingAndUpdating) MavlinkParameterCache::UpdateExistingParamResult::WrongType, cache.update_existing_param(custom_value1.id, int_value1.value)); } + TEST(MavlinkParameterCache, MissingIndicesSorted) { MavlinkParameterCache cache; @@ -78,18 +79,37 @@ TEST(MavlinkParameterCache, MissingIndicesSorted) cache.add_new_param("PARAM1", value); // No result when the count matches the contents. - EXPECT_EQ(cache.next_missing_index(2), std::nullopt); + EXPECT_EQ(cache.next_missing_indices(2, 10).size(), 0); + // The next entry when the count is bigger. - EXPECT_EQ(cache.next_missing_index(3), 2); - EXPECT_EQ(cache.next_missing_index(10), 2); + ASSERT_EQ(cache.next_missing_indices(3, 10).size(), 1); + EXPECT_EQ(cache.next_missing_indices(3, 10)[0], 2); + EXPECT_EQ(cache.next_missing_indices(10, 10)[0], 2); + + cache.add_new_param("PARAM2", value, 5); + std::vector result = {2, 3, 4}; + EXPECT_EQ(cache.next_missing_indices(6, 10), result); +} - cache.add_new_param("PARAM1", value, 5); - EXPECT_EQ(cache.next_missing_index(6), 2); +TEST(MavlinkParameterCache, MissingInChunks) +{ + MavlinkParameterCache cache; + ParamValue value; + // We use all the same value, we don't care about that part. + value.set_int(42); - // What about when we add something + cache.add_new_param("PARAM0", value); + cache.add_new_param("PARAM1", value); cache.add_new_param("PARAM2", value); - EXPECT_EQ(cache.next_missing_index(6), 3); + cache.add_new_param("PARAM3", value); + + // All remaining one are returned. + EXPECT_EQ(cache.next_missing_indices(10, 10).size(), 6); + + // Only up to chunk size are returned + EXPECT_EQ(cache.next_missing_indices(10, 3).size(), 3); } + TEST(MavlinkParameterCache, MissingIndicesNotSorted) { MavlinkParameterCache cache; @@ -99,8 +119,8 @@ TEST(MavlinkParameterCache, MissingIndicesNotSorted) cache.add_new_param("PARAM0", value, 1); cache.add_new_param("PARAM1", value, 3); - cache.add_new_param("PARAM2", value, 0); // It should still work when not sorted. - EXPECT_EQ(cache.next_missing_index(3), 2); + std::vector result = {0, 2}; + EXPECT_EQ(cache.next_missing_indices(3, 10), result); } diff --git a/src/mavsdk/core/mavlink_parameter_client.cpp b/src/mavsdk/core/mavlink_parameter_client.cpp index 8c5dc8365c..a65f7753f4 100644 --- a/src/mavsdk/core/mavlink_parameter_client.cpp +++ b/src/mavsdk/core/mavlink_parameter_client.cpp @@ -669,7 +669,8 @@ void MavlinkParameterClient::process_param_value(const mavlink_message_t& messag } if (_parameter_debugging) { - LogDebug() << "process_param_value: " << safe_param_id << " " << received_value << ", index: " << param_value.param_index; + LogDebug() << "process_param_value: " << safe_param_id << " " << received_value + << ", index: " << param_value.param_index; } if (param_value.param_index == std::numeric_limits::max()) { @@ -773,8 +774,7 @@ void MavlinkParameterClient::process_param_value(const mavlink_message_t& messag } }, [&](WorkItemGetAll& item) { - - auto maybe_current_missing_index = _param_cache.next_missing_index(item.count); + auto maybe_current_missing_index = _param_cache.last_missing_requested(); switch (_param_cache.add_new_param( safe_param_id, received_value, param_value.param_index)) { @@ -817,36 +817,17 @@ void MavlinkParameterClient::process_param_value(const mavlink_message_t& messag << " of " << param_value.param_count; } if (item.rerequesting) { - - if (maybe_current_missing_index.has_value() && - maybe_current_missing_index.value() == param_value.param_index) { - - // We got what we were waiting for, request next one. - auto maybe_next_missing_index = - _param_cache.next_missing_index(item.count); - - if (!maybe_next_missing_index.has_value()) { - LogErr() << "logic error, there should a missing index"; - assert(false); - } - - LogInfo() << "Requesting " << _param_cache.missing_count() << " parameters missed during initial burst."; - - if (_parameter_debugging) { - LogDebug() << "Requesting missing parameter " - << (int)maybe_next_missing_index.value(); - } - - std::array param_id_buff{}; - if (!send_get_param_message( - param_id_buff, maybe_next_missing_index.value())) { - LogErr() << "Send message failed"; + if (maybe_current_missing_index == param_value.param_index) { + // Looks like the last one of the previous retransmission chunk + // was done, start another one. + if (!request_next_missing(item.count)) { work_queue_guard->pop_front(); if (item.callback) { auto callback = item.callback; work_queue_guard.reset(); callback(Result::ConnectionError, {}); } + return; } } } else { @@ -1171,25 +1152,16 @@ void MavlinkParameterClient::receive_timeout() } else { item.rerequesting = true; - if (_parameter_debugging) { - _param_cache.print_missing(item.count); - } - - auto maybe_next_missing_index = _param_cache.next_missing_index(item.count); - if (!maybe_next_missing_index.has_value()) { - LogErr() << "logic error, there should a missing index"; - assert(false); - } + LogInfo() << "Requesting " << _param_cache.missing_count(item.count) << " of " + << item.count << " parameters missed during initial burst."; if (_parameter_debugging) { - LogDebug() << "Requesting missing parameter " - << (int)maybe_next_missing_index.value(); + _param_cache.print_missing(item.count); } - std::array param_id_buff{}; - - if (!send_get_param_message(param_id_buff, maybe_next_missing_index.value())) { - LogErr() << "Send message failed"; + // To speed retransmissions up, we request params in chunks, otherwise the + // latency back and forth makes this quite slow. + if (!request_next_missing(item.count)) { work_queue_guard->pop_front(); if (item.callback) { auto callback = item.callback; @@ -1198,6 +1170,7 @@ void MavlinkParameterClient::receive_timeout() } return; } + _timeout_cookie = _timeout_handler.add([this] { receive_timeout(); }, _timeout_s_callback()); } @@ -1205,6 +1178,32 @@ void MavlinkParameterClient::receive_timeout() work->work_item_variant); } +bool MavlinkParameterClient::request_next_missing(uint16_t count) +{ + // Requesting 10 at a time seems to work on SiK radios. + const uint16_t chunk_size = 10; + + auto next_missing_indices = _param_cache.next_missing_indices(count, chunk_size); + if (next_missing_indices.empty()) { + LogErr() << "logic error, there should a missing index"; + return false; + } + + for (auto next_missing_index : next_missing_indices) { + if (_parameter_debugging) { + LogDebug() << "Requesting missing parameter " << (int)next_missing_index; + } + + std::array param_id_buff{}; + + if (!send_get_param_message(param_id_buff, next_missing_index)) { + LogErr() << "Send message failed"; + return false; + } + } + return true; +} + std::ostream& operator<<(std::ostream& str, const MavlinkParameterClient::Result& result) { switch (result) { diff --git a/src/mavsdk/core/mavlink_parameter_client.h b/src/mavsdk/core/mavlink_parameter_client.h index 48fe443525..b20026107c 100644 --- a/src/mavsdk/core/mavlink_parameter_client.h +++ b/src/mavsdk/core/mavlink_parameter_client.h @@ -195,6 +195,8 @@ class MavlinkParameterClient : public MavlinkParameterSubscription { const std::array& param_id_buff, int16_t param_index); bool send_request_list_message(); + bool request_next_missing(uint16_t count); + Sender& _sender; MavlinkMessageHandler& _message_handler; TimeoutHandler& _timeout_handler;