[BACKPORT v2] get_all_params fixes

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,25 +131,79 @@ bool MavlinkParameterCache::exists(const std::string& param_id) const
     return it != _all_params.end();
 }
 
-std::optional<uint16_t> 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<uint16_t>
+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.
     std::sort(_all_params.begin(), _all_params.end(), [](const auto& lhs, const auto& rhs) {
         return lhs.index < rhs.index;
     });
 
+    std::vector<uint16_t> 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

src/mavsdk/core/mavlink_parameter_cache.h

@@ -2,8 +2,10 @@
 
 #include "param_value.h"
 
+#include <cstdint>
 #include <limits>
 #include <map>
+#include <optional>
 #include <string>
 #include <vector>
 
@@ -44,14 +46,26 @@ class MavlinkParameterCache {
 
     [[nodiscard]] uint16_t count(bool including_extended) const;
 
-    [[nodiscard]] std::optional<uint16_t> next_missing_index(uint16_t count);
+    [[nodiscard]] std::vector<uint16_t> next_missing_indices(uint16_t count, uint16_t chunk_size);
+
+    [[nodiscard]] uint16_t missing_count(uint16_t count) const;
+
+    [[nodiscard]] std::optional<uint16_t> 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<Param> _all_params;
+
+    std::optional<uint16_t> _last_missing_requested{};
 };
 
 } // namespace mavsdk

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<uint16_t> 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<uint16_t> result = {0, 2};
+    EXPECT_EQ(cache.next_missing_indices(3, 10), result);
 }