diff --git a/docs/en/cpp/api_reference/classmavsdk_1_1_param.md b/docs/en/cpp/api_reference/classmavsdk_1_1_param.md index 70d63f148d..433b419e9e 100644 --- a/docs/en/cpp/api_reference/classmavsdk_1_1_param.md +++ b/docs/en/cpp/api_reference/classmavsdk_1_1_param.md @@ -152,6 +152,14 @@ Value | Description `NoSystem` | No system connected. `ParamValueTooLong` | [Param](classmavsdk_1_1_param.md) value too long (> 128). `Failed` | Operation failed.. + `DoesNotExist` | Parameter does not exist. + `ValueOutOfRange` | Parameter value does not fit within accepted range. + `PermissionDenied` | Caller is not permitted to set the value of this parameter. + `ComponentNotFound` | Unknown component specified. + `ReadOnly` | Parameter is read-only. + `TypeUnsupported` | Parameter data type is not supported by flight stack. + `TypeMismatch` | Parameter type does not match expected type. + `ReadFail` | Parameter exists but reading failed. ## Member Function Documentation diff --git a/proto b/proto index 411ab32595..08c45fcc54 160000 --- a/proto +++ b/proto @@ -1 +1 @@ -Subproject commit 411ab325952df07e84c36b6052ba8b303baf1091 +Subproject commit 08c45fcc54e3aa3aa5bf8279e0146f4ebf065372 diff --git a/src/mavsdk/core/mavlink_parameter_client.cpp b/src/mavsdk/core/mavlink_parameter_client.cpp index 315b0f314b..1a8b9aec61 100644 --- a/src/mavsdk/core/mavlink_parameter_client.cpp +++ b/src/mavsdk/core/mavlink_parameter_client.cpp @@ -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, ¶m_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::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(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 diff --git a/src/mavsdk/core/mavlink_parameter_client.h b/src/mavsdk/core/mavlink_parameter_client.h index d0f2d14cc7..350d778efc 100644 --- a/src/mavsdk/core/mavlink_parameter_client.h +++ b/src/mavsdk/core/mavlink_parameter_client.h @@ -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); diff --git a/src/mavsdk/core/mavlink_parameter_server.cpp b/src/mavsdk/core/mavlink_parameter_server.cpp index f615cd208c..0881b61bb2 100644 --- a/src/mavsdk/core/mavlink_parameter_server.cpp +++ b/src/mavsdk/core/mavlink_parameter_server.cpp @@ -197,67 +197,89 @@ void MavlinkParameterServer::process_param_set_internally( LogDebug() << "Param set request" << (extended ? " extended" : "") << ": " << param_id << " with " << value_to_set; } - std::lock_guard lock(_all_params_mutex); - // for checking if the update actually changed the value - const auto opt_before_update = _param_cache.param_by_id(param_id, extended); - const auto result = _param_cache.update_existing_param(param_id, value_to_set); - const auto param_count = _param_cache.count(extended); - switch (result) { - case MavlinkParameterCache::UpdateExistingParamResult::MissingParam: { - // We do not allow clients to add a new parameter to the parameter set, only to update - // existing parameters. In this case, we cannot even respond with anything, since this - // parameter simply does not exist. - LogErr() << "Got param_set for non-existing parameter:" << param_id; - return; - } - case MavlinkParameterCache::UpdateExistingParamResult::WrongType: { - // Non-extended: we respond with the unchanged parameter. - // Extended: we nack with failed. + std::string error_param_id; + int16_t error_param_index = -1; + uint8_t error_code = 0; + bool send_error = false; + + { + std::lock_guard lock(_all_params_mutex); + // for checking if the update actually changed the value + const auto opt_before_update = _param_cache.param_by_id(param_id, extended); + const auto result = _param_cache.update_existing_param(param_id, value_to_set); + const auto param_count = _param_cache.count(extended); + + switch (result) { + case MavlinkParameterCache::UpdateExistingParamResult::MissingParam: { + // We do not allow clients to add a new parameter to the parameter set, only to + // update existing parameters. Send PARAM_ERROR for standard protocol. + LogErr() << "Got param_set for non-existing parameter:" << param_id; + + if (!extended) { + // Prepare to send PARAM_ERROR outside the lock + error_param_id = param_id; + error_param_index = -1; + error_code = 1; // MAV_PARAM_ERROR_DOES_NOT_EXIST + send_error = true; + } + return; + } + case MavlinkParameterCache::UpdateExistingParamResult::WrongType: { + // Non-extended: send PARAM_ERROR with TYPE_MISMATCH + // Extended: we nack with failed. - LogErr() << "Got param_set with wrong type for parameter: " << param_id; + LogErr() << "Got param_set with wrong type for parameter: " << param_id; - const auto curr_param = _param_cache.param_by_id(param_id, extended).value(); + const auto curr_param = _param_cache.param_by_id(param_id, extended).value(); - if (extended) { - auto new_work = std::make_shared( - curr_param.id, curr_param.value, WorkItemAck{PARAM_ACK_FAILED}); - _work_queue.push_back(new_work); + if (extended) { + auto new_work = std::make_shared( + curr_param.id, curr_param.value, WorkItemAck{PARAM_ACK_FAILED}); + _work_queue.push_back(new_work); - } else { - auto new_work = std::make_shared( - curr_param.id, - curr_param.value, - WorkItemValue{curr_param.index, param_count, extended}); - _work_queue.push_back(new_work); + } else { + // Prepare to send PARAM_ERROR outside the lock + error_param_id = curr_param.id; + error_param_index = static_cast(curr_param.index); + error_code = 7; // MAV_PARAM_ERROR_TYPE_MISMATCH + send_error = true; + } + return; } - return; + case MavlinkParameterCache::UpdateExistingParamResult::Ok: { + LogWarn() << "Update existing params!"; + const auto updated_parameter = _param_cache.param_by_id(param_id, extended).value(); + // The param set doesn't differentiate between an update that actually changed the + // value e.g. 0 to 1 and an update that had no effect e.g. 0 to 0. + if (opt_before_update.has_value() && + opt_before_update.value().value == updated_parameter.value) { + LogDebug() << "Update had no effect: " << updated_parameter.value; + } else { + LogDebug() << "Updated param to :" << updated_parameter.value; + find_and_call_subscriptions_value_changed( + updated_parameter.id, updated_parameter.value); + } + if (extended) { + auto new_work = std::make_shared( + updated_parameter.id, + updated_parameter.value, + WorkItemAck{PARAM_ACK_ACCEPTED}); + _work_queue.push_back(new_work); + } else { + auto new_work = std::make_shared( + updated_parameter.id, + updated_parameter.value, + WorkItemValue{updated_parameter.index, param_count, extended}); + _work_queue.push_back(new_work); + } + } break; } - case MavlinkParameterCache::UpdateExistingParamResult::Ok: { - LogWarn() << "Update existing params!"; - const auto updated_parameter = _param_cache.param_by_id(param_id, extended).value(); - // The param set doesn't differentiate between an update that actually changed the value - // e.g. 0 to 1 and an update that had no effect e.g. 0 to 0. - if (opt_before_update.has_value() && - opt_before_update.value().value == updated_parameter.value) { - LogDebug() << "Update had no effect: " << updated_parameter.value; - } else { - LogDebug() << "Updated param to :" << updated_parameter.value; - find_and_call_subscriptions_value_changed( - updated_parameter.id, updated_parameter.value); - } - if (extended) { - auto new_work = std::make_shared( - updated_parameter.id, updated_parameter.value, WorkItemAck{PARAM_ACK_ACCEPTED}); - _work_queue.push_back(new_work); - } else { - auto new_work = std::make_shared( - updated_parameter.id, - updated_parameter.value, - WorkItemValue{updated_parameter.index, param_count, extended}); - _work_queue.push_back(new_work); - } - } break; + } + + // Send PARAM_ERROR outside the lock if needed + if (send_error) { + send_param_error(error_param_id, error_param_index, error_code); } } @@ -375,42 +397,60 @@ void MavlinkParameterServer::process_param_ext_request_read(const mavlink_messag void MavlinkParameterServer::internal_process_param_request_read_by_id( const std::string& id, const bool extended) { - std::lock_guard lock(_all_params_mutex); - const auto param_opt = _param_cache.param_by_id(id, extended); - - if (!param_opt.has_value()) { - LogWarn() << "Ignoring request_read message " << (extended ? "extended " : "") - << "- param name not found: " << id; - return; + { + std::lock_guard lock(_all_params_mutex); + const auto param_opt = _param_cache.param_by_id(id, extended); + + if (!param_opt.has_value()) { + LogWarn() << "Ignoring request_read message " << (extended ? "extended " : "") + << "- param name not found: " << id; + // Release lock before sending PARAM_ERROR + } else { + const auto& param = param_opt.value(); + const auto param_count = _param_cache.count(extended); + assert(param.index < param_count); + auto new_work = std::make_shared( + param.id, param.value, WorkItemValue{param.index, param_count, extended}); + _work_queue.push_back(new_work); + + _last_extended = extended; + return; + } } - const auto& param = param_opt.value(); - const auto param_count = _param_cache.count(extended); - assert(param.index < param_count); - auto new_work = std::make_shared( - param.id, param.value, WorkItemValue{param.index, param_count, extended}); - _work_queue.push_back(new_work); - _last_extended = extended; + // Send PARAM_ERROR outside the lock + if (!extended) { + send_param_error(id, -1, 1); // MAV_PARAM_ERROR_DOES_NOT_EXIST = 1 + } } void MavlinkParameterServer::internal_process_param_request_read_by_index( std::uint16_t index, bool extended) { - std::lock_guard lock(_all_params_mutex); - const auto param_opt = _param_cache.param_by_index(index, extended); - - if (!param_opt.has_value()) { - LogWarn() << "Ignoring request_read message " << (extended ? "extended " : "") - << "- param index not found: " << index; - return; + { + std::lock_guard lock(_all_params_mutex); + const auto param_opt = _param_cache.param_by_index(index, extended); + + if (!param_opt.has_value()) { + LogWarn() << "Ignoring request_read message " << (extended ? "extended " : "") + << "- param index not found: " << index; + // Release lock before sending PARAM_ERROR + } else { + const auto& param = param_opt.value(); + const auto param_count = _param_cache.count(extended); + + assert(param.index < param_count); + auto new_work = std::make_shared( + param.id, param.value, WorkItemValue{param.index, param_count, extended}); + _work_queue.push_back(new_work); + return; + } } - const auto& param = param_opt.value(); - const auto param_count = _param_cache.count(extended); - assert(param.index < param_count); - auto new_work = std::make_shared( - param.id, param.value, WorkItemValue{param.index, param_count, extended}); - _work_queue.push_back(new_work); + // Send PARAM_ERROR outside the lock + if (!extended) { + send_param_error("", static_cast(index), 1); // MAV_PARAM_ERROR_DOES_NOT_EXIST = 1 + } } void MavlinkParameterServer::process_param_request_list(const mavlink_message_t& message) @@ -546,6 +586,34 @@ void MavlinkParameterServer::do_work() work->work_item_variant); } +void MavlinkParameterServer::send_param_error( + const std::string& param_id, int16_t param_index, uint8_t error_code) +{ + if (_parameter_debugging) { + LogDebug() << "Sending PARAM_ERROR for " << param_id << " (index: " << param_index + << ") with error code: " << (int)error_code; + } + + const auto param_id_buffer = param_id_to_message_buffer(param_id); + + if (!_sender.queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) { + mavlink_message_t message; + mavlink_msg_param_error_pack_chan( + mavlink_address.system_id, + mavlink_address.component_id, + channel, + &message, + 0, // target_system - 0 for broadcast + 0, // target_component - 0 for broadcast + param_id_buffer.data(), + param_index, + error_code); + return message; + })) { + LogErr() << "Error: Send PARAM_ERROR message failed"; + } +} + std::ostream& operator<<(std::ostream& str, const MavlinkParameterServer::Result& result) { switch (result) { diff --git a/src/mavsdk/core/mavlink_parameter_server.h b/src/mavsdk/core/mavlink_parameter_server.h index 6326f0f551..16814df058 100644 --- a/src/mavsdk/core/mavlink_parameter_server.h +++ b/src/mavsdk/core/mavlink_parameter_server.h @@ -42,6 +42,12 @@ class MavlinkParameterServer : public MavlinkParameterSubscription { TooManyParams, ParamNotFound, Unknown, + ValueOutOfRange, + PermissionDenied, + ReadOnly, + TypeUnsupported, + TypeMismatch, + ReadFail, }; Result provide_server_param(const std::string& name, const ParamValue& param_value); @@ -83,6 +89,8 @@ class MavlinkParameterServer : public MavlinkParameterSubscription { bool target_matches(uint16_t target_sys_id, uint16_t target_comp_id, bool is_request); void log_target_mismatch(uint16_t target_sys_id, uint16_t target_comp_id); + void send_param_error(const std::string& param_id, int16_t param_index, uint8_t error_code); + static std::variant extract_request_read_param_identifier(int16_t param_index, const char* param_id); diff --git a/src/mavsdk/plugins/camera_server/camera_server_impl.cpp b/src/mavsdk/plugins/camera_server/camera_server_impl.cpp index 3a2001ae42..6af99645e3 100644 --- a/src/mavsdk/plugins/camera_server/camera_server_impl.cpp +++ b/src/mavsdk/plugins/camera_server/camera_server_impl.cpp @@ -2151,7 +2151,7 @@ void CameraServerImpl::send_tracking_status_with_interval(uint32_t interval_us) &message, CAMERA_TRACKING_STATUS_FLAGS_IDLE, CAMERA_TRACKING_MODE_NONE, - CAMERA_TRACKING_TARGET_DATA_NONE, + 0, // used to be CAMERA_TRACKING_TARGET_DATA_NONE 0.0f, 0.0f, 0.0f, diff --git a/src/mavsdk/plugins/param/include/plugins/param/param.h b/src/mavsdk/plugins/param/include/plugins/param/param.h index 89f25cafef..6126143bce 100644 --- a/src/mavsdk/plugins/param/include/plugins/param/param.h +++ b/src/mavsdk/plugins/param/include/plugins/param/param.h @@ -179,6 +179,14 @@ class Param : public PluginBase { NoSystem, /**< @brief No system connected. */ ParamValueTooLong, /**< @brief Param value too long (> 128). */ Failed, /**< @brief Operation failed.. */ + DoesNotExist, /**< @brief Parameter does not exist. */ + ValueOutOfRange, /**< @brief Parameter value does not fit within accepted range. */ + PermissionDenied, /**< @brief Caller is not permitted to set the value of this parameter. */ + ComponentNotFound, /**< @brief Unknown component specified. */ + ReadOnly, /**< @brief Parameter is read-only. */ + TypeUnsupported, /**< @brief Parameter data type is not supported by flight stack. */ + TypeMismatch, /**< @brief Parameter type does not match expected type. */ + ReadFail, /**< @brief Parameter exists but reading failed. */ }; /** diff --git a/src/mavsdk/plugins/param/param.cpp b/src/mavsdk/plugins/param/param.cpp index d08e0db93e..523fd0acb7 100644 --- a/src/mavsdk/plugins/param/param.cpp +++ b/src/mavsdk/plugins/param/param.cpp @@ -159,6 +159,22 @@ std::ostream& operator<<(std::ostream& str, Param::Result const& result) return str << "Param Value Too Long"; case Param::Result::Failed: return str << "Failed"; + case Param::Result::DoesNotExist: + return str << "Does Not Exist"; + case Param::Result::ValueOutOfRange: + return str << "Value Out Of Range"; + case Param::Result::PermissionDenied: + return str << "Permission Denied"; + case Param::Result::ComponentNotFound: + return str << "Component Not Found"; + case Param::Result::ReadOnly: + return str << "Read Only"; + case Param::Result::TypeUnsupported: + return str << "Type Unsupported"; + case Param::Result::TypeMismatch: + return str << "Type Mismatch"; + case Param::Result::ReadFail: + return str << "Read Fail"; default: return str << "Unknown"; } diff --git a/src/mavsdk/plugins/param/param_impl.cpp b/src/mavsdk/plugins/param/param_impl.cpp index 78969afcf9..c93a47e59c 100644 --- a/src/mavsdk/plugins/param/param_impl.cpp +++ b/src/mavsdk/plugins/param/param_impl.cpp @@ -137,24 +137,39 @@ ParamImpl::result_from_mavlink_parameter_client_result(MavlinkParameterClient::R return Param::Result::ParamNameTooLong; case MavlinkParameterClient::Result::NotFound: LogWarn() << "NotFound"; - return Param::Result::Unknown; // TODO fix + return Param::Result::Unknown; // TODO fix - no corresponding proto Result yet case MavlinkParameterClient::Result::ValueUnsupported: LogWarn() << "ValueUnsupported"; - return Param::Result::Unknown; // TODO fix + return Param::Result::Unknown; // TODO fix - no corresponding proto Result yet case MavlinkParameterClient::Result::Failed: - LogWarn() << "Failed"; - return Param::Result::Unknown; // TODO fix + return Param::Result::Failed; case MavlinkParameterClient::Result::ParamValueTooLong: return Param::Result::ParamValueTooLong; case MavlinkParameterClient::Result::StringTypeUnsupported: LogWarn() << "StringTypeUnsupported"; - return Param::Result::Unknown; // TODO fix + return Param::Result::Unknown; // TODO fix - no corresponding proto Result yet case MavlinkParameterClient::Result::InconsistentData: LogWarn() << "InconsistentData"; - return Param::Result::Unknown; // TODO fix + return Param::Result::Unknown; // TODO fix - no corresponding proto Result yet case MavlinkParameterClient::Result::UnknownError: - LogErr() << "Unknown 2 param error"; return Param::Result::Unknown; + // New PARAM_ERROR codes + case MavlinkParameterClient::Result::DoesNotExist: + return Param::Result::DoesNotExist; + case MavlinkParameterClient::Result::ValueOutOfRange: + return Param::Result::ValueOutOfRange; + case MavlinkParameterClient::Result::PermissionDenied: + return Param::Result::PermissionDenied; + case MavlinkParameterClient::Result::ComponentNotFound: + return Param::Result::ComponentNotFound; + case MavlinkParameterClient::Result::ReadOnly: + return Param::Result::ReadOnly; + case MavlinkParameterClient::Result::TypeUnsupported: + return Param::Result::TypeUnsupported; + case MavlinkParameterClient::Result::TypeMismatch: + return Param::Result::TypeMismatch; + case MavlinkParameterClient::Result::ReadFail: + return Param::Result::ReadFail; default: LogErr() << "Unknown param error" << (int)result; return Param::Result::Unknown; diff --git a/src/mavsdk_server/src/generated/param/param.pb.cc b/src/mavsdk_server/src/generated/param/param.pb.cc index c98f14e50b..bc669a04ce 100644 --- a/src/mavsdk_server/src/generated/param/param.pb.cc +++ b/src/mavsdk_server/src/generated/param/param.pb.cc @@ -891,39 +891,44 @@ const char descriptor_table_protodef_param_2fparam_2eproto[] ABSL_ATTRIBUTE_SECT " \003(\0132\032.mavsdk.rpc.param.IntParam\0222\n\014floa" "t_params\030\002 \003(\0132\034.mavsdk.rpc.param.FloatP" "aram\0224\n\rcustom_params\030\003 \003(\0132\035.mavsdk.rpc" - ".param.CustomParam\"\274\002\n\013ParamResult\0224\n\006re" + ".param.CustomParam\"\227\004\n\013ParamResult\0224\n\006re" "sult\030\001 \001(\0162$.mavsdk.rpc.param.ParamResul" - "t.Result\022\022\n\nresult_str\030\002 \001(\t\"\342\001\n\006Result\022" + "t.Result\022\022\n\nresult_str\030\002 \001(\t\"\275\003\n\006Result\022" "\022\n\016RESULT_UNKNOWN\020\000\022\022\n\016RESULT_SUCCESS\020\001\022" "\022\n\016RESULT_TIMEOUT\020\002\022\033\n\027RESULT_CONNECTION" "_ERROR\020\003\022\025\n\021RESULT_WRONG_TYPE\020\004\022\036\n\032RESUL" "T_PARAM_NAME_TOO_LONG\020\005\022\024\n\020RESULT_NO_SYS" "TEM\020\006\022\037\n\033RESULT_PARAM_VALUE_TOO_LONG\020\007\022\021" - "\n\rRESULT_FAILED\020\010*D\n\017ProtocolVersion\022\027\n\023" - "PROTOCOL_VERSION_V1\020\000\022\030\n\024PROTOCOL_VERSIO" - "N_EXT\020\0012\313\006\n\014ParamService\022`\n\013GetParamInt\022" - "$.mavsdk.rpc.param.GetParamIntRequest\032%." - "mavsdk.rpc.param.GetParamIntResponse\"\004\200\265" - "\030\001\022`\n\013SetParamInt\022$.mavsdk.rpc.param.Set" - "ParamIntRequest\032%.mavsdk.rpc.param.SetPa" - "ramIntResponse\"\004\200\265\030\001\022f\n\rGetParamFloat\022&." - "mavsdk.rpc.param.GetParamFloatRequest\032\'." - "mavsdk.rpc.param.GetParamFloatResponse\"\004" - "\200\265\030\001\022f\n\rSetParamFloat\022&.mavsdk.rpc.param" - ".SetParamFloatRequest\032\'.mavsdk.rpc.param" - ".SetParamFloatResponse\"\004\200\265\030\001\022i\n\016GetParam" - "Custom\022\'.mavsdk.rpc.param.GetParamCustom" - "Request\032(.mavsdk.rpc.param.GetParamCusto" - "mResponse\"\004\200\265\030\001\022i\n\016SetParamCustom\022\'.mavs" - "dk.rpc.param.SetParamCustomRequest\032(.mav" - "sdk.rpc.param.SetParamCustomResponse\"\004\200\265" - "\030\001\022c\n\014GetAllParams\022%.mavsdk.rpc.param.Ge" - "tAllParamsRequest\032&.mavsdk.rpc.param.Get" - "AllParamsResponse\"\004\200\265\030\001\022l\n\017SelectCompone" - "nt\022(.mavsdk.rpc.param.SelectComponentReq" - "uest\032).mavsdk.rpc.param.SelectComponentR" - "esponse\"\004\200\265\030\001B\035\n\017io.mavsdk.paramB\nParamP" - "rotob\006proto3" + "\n\rRESULT_FAILED\020\010\022\031\n\025RESULT_DOES_NOT_EXI" + "ST\020\t\022\035\n\031RESULT_VALUE_OUT_OF_RANGE\020\n\022\034\n\030R" + "ESULT_PERMISSION_DENIED\020\013\022\036\n\032RESULT_COMP" + "ONENT_NOT_FOUND\020\014\022\024\n\020RESULT_READ_ONLY\020\r\022" + "\033\n\027RESULT_TYPE_UNSUPPORTED\020\016\022\030\n\024RESULT_T" + "YPE_MISMATCH\020\017\022\024\n\020RESULT_READ_FAIL\020\020*D\n\017" + "ProtocolVersion\022\027\n\023PROTOCOL_VERSION_V1\020\000" + "\022\030\n\024PROTOCOL_VERSION_EXT\020\0012\313\006\n\014ParamServ" + "ice\022`\n\013GetParamInt\022$.mavsdk.rpc.param.Ge" + "tParamIntRequest\032%.mavsdk.rpc.param.GetP" + "aramIntResponse\"\004\200\265\030\001\022`\n\013SetParamInt\022$.m" + "avsdk.rpc.param.SetParamIntRequest\032%.mav" + "sdk.rpc.param.SetParamIntResponse\"\004\200\265\030\001\022" + "f\n\rGetParamFloat\022&.mavsdk.rpc.param.GetP" + "aramFloatRequest\032\'.mavsdk.rpc.param.GetP" + "aramFloatResponse\"\004\200\265\030\001\022f\n\rSetParamFloat" + "\022&.mavsdk.rpc.param.SetParamFloatRequest" + "\032\'.mavsdk.rpc.param.SetParamFloatRespons" + "e\"\004\200\265\030\001\022i\n\016GetParamCustom\022\'.mavsdk.rpc.p" + "aram.GetParamCustomRequest\032(.mavsdk.rpc." + "param.GetParamCustomResponse\"\004\200\265\030\001\022i\n\016Se" + "tParamCustom\022\'.mavsdk.rpc.param.SetParam" + "CustomRequest\032(.mavsdk.rpc.param.SetPara" + "mCustomResponse\"\004\200\265\030\001\022c\n\014GetAllParams\022%." + "mavsdk.rpc.param.GetAllParamsRequest\032&.m" + "avsdk.rpc.param.GetAllParamsResponse\"\004\200\265" + "\030\001\022l\n\017SelectComponent\022(.mavsdk.rpc.param" + ".SelectComponentRequest\032).mavsdk.rpc.par" + "am.SelectComponentResponse\"\004\200\265\030\001B\035\n\017io.m" + "avsdk.paramB\nParamProtob\006proto3" }; static const ::_pbi::DescriptorTable* const descriptor_table_param_2fparam_2eproto_deps[1] = { @@ -933,7 +938,7 @@ static ::absl::once_flag descriptor_table_param_2fparam_2eproto_once; PROTOBUF_CONSTINIT const ::_pbi::DescriptorTable descriptor_table_param_2fparam_2eproto = { false, false, - 2692, + 2911, descriptor_table_protodef_param_2fparam_2eproto, "param/param.proto", &descriptor_table_param_2fparam_2eproto_once, @@ -954,9 +959,9 @@ const ::google::protobuf::EnumDescriptor* ParamResult_Result_descriptor() { return file_level_enum_descriptors_param_2fparam_2eproto[0]; } PROTOBUF_CONSTINIT const uint32_t ParamResult_Result_internal_data_[] = { - 589824u, 0u, }; + 1114112u, 0u, }; bool ParamResult_Result_IsValid(int value) { - return 0 <= value && value <= 8; + return 0 <= value && value <= 16; } #if (__cplusplus < 201703) && \ (!defined(_MSC_VER) || (_MSC_VER >= 1900 && _MSC_VER < 1912)) @@ -970,6 +975,14 @@ constexpr ParamResult_Result ParamResult::RESULT_PARAM_NAME_TOO_LONG; constexpr ParamResult_Result ParamResult::RESULT_NO_SYSTEM; constexpr ParamResult_Result ParamResult::RESULT_PARAM_VALUE_TOO_LONG; constexpr ParamResult_Result ParamResult::RESULT_FAILED; +constexpr ParamResult_Result ParamResult::RESULT_DOES_NOT_EXIST; +constexpr ParamResult_Result ParamResult::RESULT_VALUE_OUT_OF_RANGE; +constexpr ParamResult_Result ParamResult::RESULT_PERMISSION_DENIED; +constexpr ParamResult_Result ParamResult::RESULT_COMPONENT_NOT_FOUND; +constexpr ParamResult_Result ParamResult::RESULT_READ_ONLY; +constexpr ParamResult_Result ParamResult::RESULT_TYPE_UNSUPPORTED; +constexpr ParamResult_Result ParamResult::RESULT_TYPE_MISMATCH; +constexpr ParamResult_Result ParamResult::RESULT_READ_FAIL; constexpr ParamResult_Result ParamResult::Result_MIN; constexpr ParamResult_Result ParamResult::Result_MAX; constexpr int ParamResult::Result_ARRAYSIZE; diff --git a/src/mavsdk_server/src/generated/param/param.pb.h b/src/mavsdk_server/src/generated/param/param.pb.h index eeaa421366..e3d645ea8b 100644 --- a/src/mavsdk_server/src/generated/param/param.pb.h +++ b/src/mavsdk_server/src/generated/param/param.pb.h @@ -141,6 +141,14 @@ enum ParamResult_Result : int { ParamResult_Result_RESULT_NO_SYSTEM = 6, ParamResult_Result_RESULT_PARAM_VALUE_TOO_LONG = 7, ParamResult_Result_RESULT_FAILED = 8, + ParamResult_Result_RESULT_DOES_NOT_EXIST = 9, + ParamResult_Result_RESULT_VALUE_OUT_OF_RANGE = 10, + ParamResult_Result_RESULT_PERMISSION_DENIED = 11, + ParamResult_Result_RESULT_COMPONENT_NOT_FOUND = 12, + ParamResult_Result_RESULT_READ_ONLY = 13, + ParamResult_Result_RESULT_TYPE_UNSUPPORTED = 14, + ParamResult_Result_RESULT_TYPE_MISMATCH = 15, + ParamResult_Result_RESULT_READ_FAIL = 16, ParamResult_Result_ParamResult_Result_INT_MIN_SENTINEL_DO_NOT_USE_ = std::numeric_limits<::int32_t>::min(), ParamResult_Result_ParamResult_Result_INT_MAX_SENTINEL_DO_NOT_USE_ = @@ -150,8 +158,8 @@ enum ParamResult_Result : int { bool ParamResult_Result_IsValid(int value); extern const uint32_t ParamResult_Result_internal_data_[]; constexpr ParamResult_Result ParamResult_Result_Result_MIN = static_cast(0); -constexpr ParamResult_Result ParamResult_Result_Result_MAX = static_cast(8); -constexpr int ParamResult_Result_Result_ARRAYSIZE = 8 + 1; +constexpr ParamResult_Result ParamResult_Result_Result_MAX = static_cast(16); +constexpr int ParamResult_Result_Result_ARRAYSIZE = 16 + 1; const ::google::protobuf::EnumDescriptor* ParamResult_Result_descriptor(); template @@ -164,7 +172,7 @@ const std::string& ParamResult_Result_Name(T value) { template <> inline const std::string& ParamResult_Result_Name(ParamResult_Result value) { return ::google::protobuf::internal::NameOfDenseEnum( + 0, 16>( static_cast(value)); } inline bool ParamResult_Result_Parse(absl::string_view name, ParamResult_Result* value) { @@ -1200,6 +1208,14 @@ class ParamResult final static constexpr Result RESULT_NO_SYSTEM = ParamResult_Result_RESULT_NO_SYSTEM; static constexpr Result RESULT_PARAM_VALUE_TOO_LONG = ParamResult_Result_RESULT_PARAM_VALUE_TOO_LONG; static constexpr Result RESULT_FAILED = ParamResult_Result_RESULT_FAILED; + static constexpr Result RESULT_DOES_NOT_EXIST = ParamResult_Result_RESULT_DOES_NOT_EXIST; + static constexpr Result RESULT_VALUE_OUT_OF_RANGE = ParamResult_Result_RESULT_VALUE_OUT_OF_RANGE; + static constexpr Result RESULT_PERMISSION_DENIED = ParamResult_Result_RESULT_PERMISSION_DENIED; + static constexpr Result RESULT_COMPONENT_NOT_FOUND = ParamResult_Result_RESULT_COMPONENT_NOT_FOUND; + static constexpr Result RESULT_READ_ONLY = ParamResult_Result_RESULT_READ_ONLY; + static constexpr Result RESULT_TYPE_UNSUPPORTED = ParamResult_Result_RESULT_TYPE_UNSUPPORTED; + static constexpr Result RESULT_TYPE_MISMATCH = ParamResult_Result_RESULT_TYPE_MISMATCH; + static constexpr Result RESULT_READ_FAIL = ParamResult_Result_RESULT_READ_FAIL; static inline bool Result_IsValid(int value) { return ParamResult_Result_IsValid(value); } diff --git a/src/mavsdk_server/src/plugins/param/param_service_impl.h b/src/mavsdk_server/src/plugins/param/param_service_impl.h index a3bc87e8aa..f57773fc02 100644 --- a/src/mavsdk_server/src/plugins/param/param_service_impl.h +++ b/src/mavsdk_server/src/plugins/param/param_service_impl.h @@ -213,6 +213,22 @@ class ParamServiceImpl final : public rpc::param::ParamService::Service { return rpc::param::ParamResult_Result_RESULT_PARAM_VALUE_TOO_LONG; case mavsdk::Param::Result::Failed: return rpc::param::ParamResult_Result_RESULT_FAILED; + case mavsdk::Param::Result::DoesNotExist: + return rpc::param::ParamResult_Result_RESULT_DOES_NOT_EXIST; + case mavsdk::Param::Result::ValueOutOfRange: + return rpc::param::ParamResult_Result_RESULT_VALUE_OUT_OF_RANGE; + case mavsdk::Param::Result::PermissionDenied: + return rpc::param::ParamResult_Result_RESULT_PERMISSION_DENIED; + case mavsdk::Param::Result::ComponentNotFound: + return rpc::param::ParamResult_Result_RESULT_COMPONENT_NOT_FOUND; + case mavsdk::Param::Result::ReadOnly: + return rpc::param::ParamResult_Result_RESULT_READ_ONLY; + case mavsdk::Param::Result::TypeUnsupported: + return rpc::param::ParamResult_Result_RESULT_TYPE_UNSUPPORTED; + case mavsdk::Param::Result::TypeMismatch: + return rpc::param::ParamResult_Result_RESULT_TYPE_MISMATCH; + case mavsdk::Param::Result::ReadFail: + return rpc::param::ParamResult_Result_RESULT_READ_FAIL; } } @@ -241,6 +257,22 @@ class ParamServiceImpl final : public rpc::param::ParamService::Service { return mavsdk::Param::Result::ParamValueTooLong; case rpc::param::ParamResult_Result_RESULT_FAILED: return mavsdk::Param::Result::Failed; + case rpc::param::ParamResult_Result_RESULT_DOES_NOT_EXIST: + return mavsdk::Param::Result::DoesNotExist; + case rpc::param::ParamResult_Result_RESULT_VALUE_OUT_OF_RANGE: + return mavsdk::Param::Result::ValueOutOfRange; + case rpc::param::ParamResult_Result_RESULT_PERMISSION_DENIED: + return mavsdk::Param::Result::PermissionDenied; + case rpc::param::ParamResult_Result_RESULT_COMPONENT_NOT_FOUND: + return mavsdk::Param::Result::ComponentNotFound; + case rpc::param::ParamResult_Result_RESULT_READ_ONLY: + return mavsdk::Param::Result::ReadOnly; + case rpc::param::ParamResult_Result_RESULT_TYPE_UNSUPPORTED: + return mavsdk::Param::Result::TypeUnsupported; + case rpc::param::ParamResult_Result_RESULT_TYPE_MISMATCH: + return mavsdk::Param::Result::TypeMismatch; + case rpc::param::ParamResult_Result_RESULT_READ_FAIL: + return mavsdk::Param::Result::ReadFail; } } diff --git a/src/system_tests/param_set_and_get.cpp b/src/system_tests/param_set_and_get.cpp index 80eb38b8df..cb7a3ebc81 100644 --- a/src/system_tests/param_set_and_get.cpp +++ b/src/system_tests/param_set_and_get.cpp @@ -43,9 +43,9 @@ TEST(SystemTest, ParamSetAndGet) // First we try to get a param before it is available. auto result_pair_float = param.get_param_float(param_name_float); - EXPECT_EQ(result_pair_float.first, Param::Result::Timeout); + EXPECT_EQ(result_pair_float.first, Param::Result::DoesNotExist); auto result_pair_int = param.get_param_int(param_name_int); - EXPECT_EQ(result_pair_int.first, Param::Result::Timeout); + EXPECT_EQ(result_pair_int.first, Param::Result::DoesNotExist); // Then we make it available. EXPECT_EQ( diff --git a/third_party/mavlink/CMakeLists.txt b/third_party/mavlink/CMakeLists.txt index 5d24a0f016..b2f63ebcdc 100644 --- a/third_party/mavlink/CMakeLists.txt +++ b/third_party/mavlink/CMakeLists.txt @@ -9,7 +9,7 @@ list(APPEND CMAKE_ARGS "-DMAVLINK_DIALECT=${MAVLINK_DIALECT}" ) -set(MAVLINK_GIT_HASH 5e3a42b8f3f53038f2779f9f69bd64767b913bb8) +set(MAVLINK_GIT_HASH 3ca42d6cdab73122d9c677f88e64882ef2b0dde8) if(IOS) ExternalProject_add(