diff --git a/src/mavsdk/core/mavlink_parameter_cache.cpp b/src/mavsdk/core/mavlink_parameter_cache.cpp index e580ad709..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 @@ -130,7 +131,29 @@ bool MavlinkParameterCache::exists(const std::string& param_id) const return it != _all_params.end(); } -std::optional MavlinkParameterCache::next_missing_index(uint16_t count) +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(); +} + +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::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. @@ -138,17 +161,49 @@ 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 (_all_params.size() <= i) { - // We have reached the end but it's not complete yet. - return i; + if (exists(i)) { + continue; } - if (_all_params[i].index > i) { - // We have found a hole to fill. - return i; + + result.push_back(i); + _last_missing_requested = {i}; + + if (result.size() == chunk_size) { + break; } } - return {}; + + return result; +} + +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 420d5a281..736d930fb 100644 --- a/src/mavsdk/core/mavlink_parameter_cache.h +++ b/src/mavsdk/core/mavlink_parameter_cache.h @@ -2,8 +2,10 @@ #include "param_value.h" +#include #include #include +#include #include #include @@ -44,14 +46,26 @@ 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(); private: [[nodiscard]] bool exists(const std::string& param_id) const; + [[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 dac6ed781..dc51ca181 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 { @@ -490,8 +491,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); } @@ -664,7 +665,13 @@ 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. + return; } // We need to use a unique pointer here to remove the lock from the work queue manually "early" @@ -763,6 +770,8 @@ void MavlinkParameterClient::process_param_value(const mavlink_message_t& messag } }, [&](WorkItemGetAll& item) { + 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)) { case MavlinkParameterCache::AddNewParamResult::AlreadyExists: @@ -773,14 +782,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(); @@ -793,33 +809,22 @@ 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(); - } - - 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, {}); + 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; } - return; } } else { // update the timeout handler, messages are still coming in. @@ -913,7 +918,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(); @@ -926,8 +931,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); @@ -1127,9 +1132,10 @@ 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()); + [this] { receive_timeout(); }, + _timeout_s_callback() * _get_all_timeout_factor); } else { if (item.callback) { auto callback = item.callback; @@ -1141,21 +1147,17 @@ void MavlinkParameterClient::receive_timeout() } else { item.rerequesting = true; - 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; @@ -1164,6 +1166,7 @@ void MavlinkParameterClient::receive_timeout() } return; } + _timeout_cookie = _timeout_handler.add([this] { receive_timeout(); }, _timeout_s_callback()); } @@ -1171,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 7f07813e9..b20026107 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; @@ -193,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;