Merge pull request #1788 from mavlink/pr-fix-get-all-param-timeout

params: fix double promise call

src/integration_tests/param.cpp

@@ -162,6 +162,8 @@ TEST_F(SitlTest, GetAllParams)
     for (const auto& mixed_param : all_mixed) {
         std::cout << mixed_param.first << " : " << mixed_param.second << '\n';
     }
+
+    std::this_thread::sleep_for(std::chrono::seconds(2));
 }
 
 TEST_F(SitlTest, APParam)

src/mavsdk/core/mavlink_command_sender.cpp

@@ -293,6 +293,10 @@ void MavlinkCommandSender::receive_timeout(const CommandIdentification& identifi
                       << " s, retries to do: " << work->retries_to_do << "  ("
                       << work->identification.command << ").";
 
+            if (work->identification.command == MAV_CMD_REQUEST_MESSAGE) {
+                LogWarn() << "Request was for msg ID: " << work->identification.maybe_param1;
+            }
+
             mavlink_message_t message = create_mavlink_message(work->command);
             if (!_parent.send_message(message)) {
                 LogErr() << "connection send error in retransmit (" << work->identification.command

src/mavsdk/core/mavlink_parameters.cpp

@@ -594,7 +594,7 @@ MAVLinkParameters::get_param_custom(const std::string& name)
 
 void MAVLinkParameters::get_all_params_async(const GetAllParamsCallback& callback)
 {
-    std::unique_lock<std::mutex> lock(_all_params_mutex);
+    std::lock_guard<std::mutex> lock(_all_params_mutex);
 
     _all_params_callback = callback;
 
@@ -609,7 +609,6 @@ void MAVLinkParameters::get_all_params_async(const GetAllParamsCallback& callbac
 
     if (!_sender.send_message(msg)) {
         LogErr() << "Failed to send param list request!";
-        lock.unlock();
         callback(std::map<std::string, ParamValue>{});
     }
 
@@ -767,11 +766,10 @@ void MAVLinkParameters::do_work()
     switch (work->type) {
         case WorkItem::Type::Set: {
             if (!work->exact_type_known) {
-                std::unique_lock<std::mutex> lock(_all_params_mutex);
+                std::lock_guard<std::mutex> lock(_all_params_mutex);
                 const auto it = _all_params.find(work->param_name);
                 if (it == _all_params.end()) {
                     LogErr() << "Don't know the type of param_set";
-                    lock.unlock();
                     if (std::get_if<SetParamCallback>(&work->callback)) {
                         const auto& callback = std::get<SetParamCallback>(work->callback);
                         if (callback) {
@@ -986,14 +984,15 @@ void MAVLinkParameters::process_param_value(const mavlink_message_t& message)
     std::string param_id = extract_safe_param_id(param_value.param_id);
 
     {
-        std::unique_lock<std::mutex> lock(_all_params_mutex);
+        std::lock_guard<std::mutex> lock(_all_params_mutex);
         _all_params[param_id] = received_value;
 
         // check if we are looking for param list
         if (_all_params_callback) {
             if (param_value.param_index + 1 == param_value.param_count) {
-                lock.unlock();
+                _timeout_handler.remove(_all_params_timeout_cookie);
                 _all_params_callback(_all_params);
+                _all_params_callback = nullptr;
             } else {
                 _timeout_handler.remove(_all_params_timeout_cookie);
 
@@ -1346,10 +1345,9 @@ void MAVLinkParameters::process_param_ext_set(const mavlink_message_t& message)
 void MAVLinkParameters::receive_timeout()
 {
     {
-        std::unique_lock<std::mutex> lock(_all_params_mutex);
+        std::lock_guard<std::mutex> lock(_all_params_mutex);
         // first check if we are waiting for param list response
         if (_all_params_callback) {
-            lock.unlock();
             _all_params_callback({});
             return;
         }

src/mavsdk/core/server_component_impl.cpp

@@ -21,7 +21,37 @@ ServerComponentImpl::ServerComponentImpl(MavsdkImpl& mavsdk_impl, uint8_t compon
         [this]() { return _mavsdk_impl.timeout_s(); },
         true),
     _mavlink_request_message_handler(mavsdk_impl, *this, _mavlink_command_receiver)
-{}
+{
+    register_mavlink_command_handler(
+        MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES,
+        [this](const MavlinkCommandReceiver::CommandLong& command) {
+            send_autopilot_version();
+            return make_command_ack_message(command, MAV_RESULT_ACCEPTED);
+        },
+        this);
+
+    register_mavlink_command_handler(
+        MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES,
+        [this](const MavlinkCommandReceiver::CommandInt& command) {
+            send_autopilot_version();
+            return make_command_ack_message(command, MAV_RESULT_ACCEPTED);
+        },
+        this);
+
+    _mavlink_request_message_handler.register_handler(
+        MAVLINK_MSG_ID_AUTOPILOT_VERSION,
+        [this](const MavlinkRequestMessageHandler::Params&) {
+            send_autopilot_version();
+            return MAV_RESULT_ACCEPTED;
+        },
+        this);
+}
+
+ServerComponentImpl::~ServerComponentImpl()
+{
+    unregister_all_mavlink_command_handlers(this);
+    _mavlink_request_message_handler.unregister_all_handlers(this);
+}
 
 void ServerComponentImpl::register_plugin(ServerPluginImplBase* server_plugin_impl)
 {

src/mavsdk/core/server_component_impl.h

@@ -21,7 +21,7 @@ class ServerPluginImplBase;
 class ServerComponentImpl {
 public:
     ServerComponentImpl(MavsdkImpl& mavsdk_impl, uint8_t component_id);
-    ~ServerComponentImpl() = default;
+    ~ServerComponentImpl();
 
     void register_plugin(ServerPluginImplBase* server_plugin_impl);
     void unregister_plugin(ServerPluginImplBase* server_plugin_impl);

src/system_tests/CMakeLists.txt

@@ -5,6 +5,7 @@ add_executable(system_tests_runner
     action_arm_disarm.cpp
     system_tests_helper.cpp
     param_set_and_get.cpp
+    param_get_all.cpp
     param_custom_set_and_get.cpp
     mission_raw_upload.cpp
 )

src/system_tests/param_get_all.cpp

@@ -0,0 +1,79 @@
+#include "log.h"
+#include "mavsdk.h"
+#include "system_tests_helper.h"
+#include "plugins/param/param.h"
+#include "plugins/param_server/param_server.h"
+#include <chrono>
+#include <utility>
+#include <vector>
+#include <thread>
+
+using namespace mavsdk;
+
+static constexpr unsigned num_params_per_type = 100;
+
+std::vector<std::pair<std::string, float>> generate_float_params()
+{
+    std::vector<std::pair<std::string, float>> params;
+
+    for (unsigned i = 0; i < num_params_per_type; ++i) {
+        params.emplace_back(std::string("TEST_BLA") + std::to_string(i), 42.0f + i);
+    }
+
+    return params;
+}
+
+std::vector<std::pair<std::string, int>> generate_int_params()
+{
+    std::vector<std::pair<std::string, int>> params;
+
+    for (unsigned i = 0; i < num_params_per_type; ++i) {
+        params.emplace_back(std::string("TEST_FOO") + std::to_string(i), 42.0f + i);
+    }
+
+    return params;
+}
+
+TEST(SystemTest, ParamGetAll)
+{
+    Mavsdk mavsdk_groundstation;
+    mavsdk_groundstation.set_configuration(
+        Mavsdk::Configuration{Mavsdk::Configuration::UsageType::GroundStation});
+
+    Mavsdk mavsdk_autopilot;
+    mavsdk_autopilot.set_configuration(
+        Mavsdk::Configuration{Mavsdk::Configuration::UsageType::Autopilot});
+
+    ASSERT_EQ(mavsdk_groundstation.add_any_connection("udp://:17000"), ConnectionResult::Success);
+    ASSERT_EQ(
+        mavsdk_autopilot.add_any_connection("udp://127.0.0.1:17000"), ConnectionResult::Success);
+
+    auto param_server = ParamServer{
+        mavsdk_autopilot.server_component_by_type(Mavsdk::ServerComponentType::Autopilot)};
+
+    auto fut = wait_for_first_system_detected(mavsdk_groundstation);
+    ASSERT_EQ(fut.wait_for(std::chrono::seconds(10)), std::future_status::ready);
+    auto system = fut.get();
+
+    ASSERT_TRUE(system->has_autopilot());
+
+    auto param = Param{system};
+
+    // Add many params
+    for (const auto& float_param : generate_float_params()) {
+        EXPECT_EQ(
+            param_server.provide_param_float(float_param.first, float_param.second),
+            ParamServer::Result::Success);
+    }
+    for (const auto& int_param : generate_int_params()) {
+        EXPECT_EQ(
+            param_server.provide_param_int(int_param.first, int_param.second),
+            ParamServer::Result::Success);
+    }
+
+    auto all_params = param.get_all_params();
+
+    EXPECT_EQ(all_params.float_params.size(), generate_float_params().size());
+    EXPECT_EQ(all_params.int_params.size(), generate_int_params().size());
+    std::this_thread::sleep_for(std::chrono::seconds(1));
+}