diff --git a/src/mavsdk/core/mavlink_parameter_cache.cpp b/src/mavsdk/core/mavlink_parameter_cache.cpp index 8e8dae2af..d45617906 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 a5909b8cd..736d930fb 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 c6dbf904e..03d1afb0f 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 83561d9f3..dc51ca181 100644 --- a/src/mavsdk/core/mavlink_parameter_client.cpp +++ b/src/mavsdk/core/mavlink_parameter_client.cpp @@ -665,7 +665,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()) { @@ -769,8 +770,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)) { @@ -813,36 +813,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 { @@ -1167,25 +1148,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; @@ -1194,6 +1166,7 @@ void MavlinkParameterClient::receive_timeout() } return; } + _timeout_cookie = _timeout_handler.add([this] { receive_timeout(); }, _timeout_s_callback()); } @@ -1201,6 +1174,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 48fe44352..b20026107 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;