Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 8 additions & 0 deletions docs/en/cpp/api_reference/classmavsdk_1_1_param.md
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
2 changes: 1 addition & 1 deletion proto
89 changes: 89 additions & 0 deletions src/mavsdk/core/mavlink_parameter_client.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
}

Expand Down Expand Up @@ -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
Expand Down
9 changes: 9 additions & 0 deletions src/mavsdk/core/mavlink_parameter_client.h
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down
Loading
Loading