core/param/param_server: add PARAM_ERROR support

This adds support for the new PARAM_ERROR message which makes the param
protocol more robust in case things go wrong. Instead of waiting for a
timeout, we actually get an error.

Author: julianoes

docs/en/cpp/api_reference/classmavsdk_1_1_param.md

@@ -152,6 +152,14 @@ Value | Description
 <span id="classmavsdk_1_1_param_1afde69c8b60c41e2f21db148d211881dfa1119faf72ba0dfb23aeea644fed960ad"></span> `NoSystem` | No system connected. 
 <span id="classmavsdk_1_1_param_1afde69c8b60c41e2f21db148d211881dfa1fc93bc695e2e3e1903029eb77228234"></span> `ParamValueTooLong` | [Param](classmavsdk_1_1_param.md) value too long (> 128). 
 <span id="classmavsdk_1_1_param_1afde69c8b60c41e2f21db148d211881dfad7c8c85bf79bbe1b7188497c32c3b0ca"></span> `Failed` | Operation failed.. 
+<span id="classmavsdk_1_1_param_1afde69c8b60c41e2f21db148d211881dfac47c93cf49de211bba5d62d65225f128"></span> `DoesNotExist` | Parameter does not exist. 
+<span id="classmavsdk_1_1_param_1afde69c8b60c41e2f21db148d211881dfaf3bd760221d37f5f1a973f094ea02478"></span> `ValueOutOfRange` | Parameter value does not fit within accepted range. 
+<span id="classmavsdk_1_1_param_1afde69c8b60c41e2f21db148d211881dfa4b33a686be73bb172407d73b26356275"></span> `PermissionDenied` | Caller is not permitted to set the value of this parameter. 
+<span id="classmavsdk_1_1_param_1afde69c8b60c41e2f21db148d211881dfab43f027b5acb9fb70fafef6a3de8e646"></span> `ComponentNotFound` | Unknown component specified. 
+<span id="classmavsdk_1_1_param_1afde69c8b60c41e2f21db148d211881dfa131fb182a881796e7606ed6da27f1197"></span> `ReadOnly` | Parameter is read-only. 
+<span id="classmavsdk_1_1_param_1afde69c8b60c41e2f21db148d211881dfa849c937b23fd724a14c4597eef61ce29"></span> `TypeUnsupported` | Parameter data type is not supported by flight stack. 
+<span id="classmavsdk_1_1_param_1afde69c8b60c41e2f21db148d211881dfa2782c4707c245684aa95545098e1b70b"></span> `TypeMismatch` | Parameter type does not match expected type. 
+<span id="classmavsdk_1_1_param_1afde69c8b60c41e2f21db148d211881dfa61d8b4448684ca1db22e9894cf995e0a"></span> `ReadFail` | Parameter exists but reading failed. 
 
 ## Member Function Documentation
 

proto

@@ -56,6 +56,12 @@ MavlinkParameterClient::MavlinkParameterClient(
             MAVLINK_MSG_ID_PARAM_VALUE,
             [this](const mavlink_message_t& message) { process_param_value(message); },
             this);
+
+        // PARAM_ERROR is only for standard protocol (not extended)
+        _message_handler.register_one(
+            MAVLINK_MSG_ID_PARAM_ERROR,
+            [this](const mavlink_message_t& message) { process_param_error(message); },
+            this);
     }
 }
 
@@ -1051,6 +1057,89 @@ void MavlinkParameterClient::process_param_ext_ack(const mavlink_message_t& mess
         work->work_item_variant);
 }
 
+void MavlinkParameterClient::process_param_error(const mavlink_message_t& message)
+{
+    mavlink_param_error_t param_error;
+    mavlink_msg_param_error_decode(&message, &param_error);
+    const auto safe_param_id = extract_safe_param_id(param_error.param_id);
+
+    if (_parameter_debugging) {
+        LogDebug() << "process param_error: " << safe_param_id
+                   << " error code: " << (int)param_error.error;
+    }
+
+    // See comments on process_param_value for use of unique_ptr
+    auto work_queue_guard = std::make_unique<LockedQueue<WorkItem>::Guard>(_work_queue);
+    auto work = work_queue_guard->get_front();
+    if (!work) {
+        return;
+    }
+    if (!work->already_requested) {
+        return;
+    }
+
+    // Map MAV_PARAM_ERROR to Result
+    auto map_param_error_to_result = [](uint8_t error_code) -> Result {
+        switch (error_code) {
+            case 1: // MAV_PARAM_ERROR_DOES_NOT_EXIST
+                return Result::DoesNotExist;
+            case 2: // MAV_PARAM_ERROR_VALUE_OUT_OF_RANGE
+                return Result::ValueOutOfRange;
+            case 3: // MAV_PARAM_ERROR_PERMISSION_DENIED
+                return Result::PermissionDenied;
+            case 4: // MAV_PARAM_ERROR_COMPONENT_NOT_FOUND
+                return Result::ComponentNotFound;
+            case 5: // MAV_PARAM_ERROR_READ_ONLY
+                return Result::ReadOnly;
+            case 6: // MAV_PARAM_ERROR_TYPE_UNSUPPORTED
+                return Result::TypeUnsupported;
+            case 7: // MAV_PARAM_ERROR_TYPE_MISMATCH
+                return Result::TypeMismatch;
+            case 8: // MAV_PARAM_ERROR_READ_FAIL
+                return Result::ReadFail;
+            default:
+                return Result::UnknownError;
+        }
+    };
+
+    std::visit(
+        overloaded{
+            [&](WorkItemSet& item) {
+                if (item.param_name != safe_param_id) {
+                    // No match, let's just return the borrowed work item.
+                    return;
+                }
+                _timeout_handler.remove(_timeout_cookie);
+                work_queue_guard->pop_front();
+                if (item.callback) {
+                    auto callback = item.callback;
+                    auto result = map_param_error_to_result(param_error.error);
+                    work_queue_guard.reset();
+                    callback(result);
+                }
+            },
+            [&](WorkItemGet& item) {
+                if (!validate_id_or_index(
+                        item.param_identifier,
+                        safe_param_id,
+                        static_cast<int16_t>(param_error.param_index))) {
+                    LogWarn() << "Got unexpected PARAM_ERROR on work item";
+                    // No match, let's just return the borrowed work item.
+                    return;
+                }
+                _timeout_handler.remove(_timeout_cookie);
+                work_queue_guard->pop_front();
+                if (item.callback) {
+                    auto callback = item.callback;
+                    auto result = map_param_error_to_result(param_error.error);
+                    work_queue_guard.reset();
+                    callback(result, ParamValue{});
+                }
+            },
+            [&](WorkItemGetAll&) { LogWarn() << "Unexpected PARAM_ERROR response for GetAll."; }},
+        work->work_item_variant);
+}
+
 void MavlinkParameterClient::receive_timeout()
 {
     // See comments on process_param_value for use of unique_ptr

src/mavsdk/core/mavlink_parameter_client.cpp

@@ -58,6 +58,14 @@ class MavlinkParameterClient : public MavlinkParameterSubscription {
         StringTypeUnsupported,
         InconsistentData,
         UnknownError,
+        DoesNotExist,
+        ValueOutOfRange,
+        PermissionDenied,
+        ComponentNotFound,
+        ReadOnly,
+        TypeUnsupported,
+        TypeMismatch,
+        ReadFail,
     };
 
     Result set_param(const std::string& name, const ParamValue& value);
@@ -187,6 +195,7 @@ class MavlinkParameterClient : public MavlinkParameterSubscription {
     void process_param_value(const mavlink_message_t& message);
     void process_param_ext_value(const mavlink_message_t& message);
     void process_param_ext_ack(const mavlink_message_t& message);
+    void process_param_error(const mavlink_message_t& message);
     void receive_timeout();
 
     bool send_set_param_message(WorkItemSet& work_item);