diff --git a/src/lib/parameters/ParametersServer.cpp b/src/lib/parameters/ParametersServer.cpp index 04d8c4c12f..3814b7d801 100644 --- a/src/lib/parameters/ParametersServer.cpp +++ b/src/lib/parameters/ParametersServer.cpp @@ -106,4 +106,82 @@ void ParametersServer::Run() if (ret != 0) { PX4_ERR("param auto save failed (%i)", ret); } + + // Check for parameter requests (get/set/list) + if (_param_request_sub.updated()) { + parameter_request_s request{}; + param_request_sub.copy(&request); + + /* + * We know how many parameters are exposed by this node, so + * process the request. + */ + if (request.message_type == parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_READ) { + parameter_value_s parameter_value{}; + + if (request.param_index >= 0) { + param_for_index(request.param_index); + + } else { + param_find(&request.param_id); + } + + int call_res = _param_getset_client.call(request.node_id, req); + + if (call_res < 0) { + PX4_ERR("couldn't send GetSet: %d", call_res); + + } else { + _param_in_progress = true; + _param_index = request.param_index; + } + + } else if (request.message_type == parameter_request_s::MESSAGE_TYPE_PARAM_SET) { + uavcan::protocol::param::GetSet::Request req; + + if (request.param_index >= 0) { + req.index = request.param_index; + + } else { + req.name = (char *)request.param_id; + } + + if (request.param_type == parameter_request_s::TYPE_REAL32) { + req.value.to() = request.real_value; + + } else if (request.param_type == parameter_request_s::TYPE_UINT8) { + req.value.to() = request.int_value; + + } else { + req.value.to() = request.int_value; + } + + } else if (request.message_type == parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_LIST) { + // This triggers the _param_list_in_progress case below. + _param_index = 0; + _param_list_in_progress = true; + _param_list_node_id = request.node_id; + _param_list_all_nodes = false; + + PX4_DEBUG("starting component-specific param list"); + + } else if (request.node_id == parameter_request_s::NODE_ID_ALL) { + if (request.message_type == parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_LIST) { + /* + * This triggers the _param_list_in_progress case below, + * but additionally iterates over all active nodes. + */ + _param_index = 0; + _param_list_in_progress = true; + _param_list_node_id = get_next_active_node_id(0); + _param_list_all_nodes = true; + + PX4_DEBUG("starting global param list with node %hhu", _param_list_node_id); + + if (_param_counts[_param_list_node_id] == 0) { + param_count(_param_list_node_id); + } + } + } + } } diff --git a/src/lib/parameters/ParametersServer.hpp b/src/lib/parameters/ParametersServer.hpp index 1e23ac13fb..f8d1e99bce 100644 --- a/src/lib/parameters/ParametersServer.hpp +++ b/src/lib/parameters/ParametersServer.hpp @@ -35,6 +35,11 @@ #include +#include +#include +#include +#include + /** * Automatically save the parameters after a timeout and limited rate. * @@ -56,7 +61,11 @@ public: static void print_status(); private: + void Run() override; + hrt_abstime _last_autosave_timestamp{0}; - void Run() override; + uORB::Publication _param_response_pub{ORB_ID(parameter_value)}; + uORB::Subscription _param_request_sub{ORB_ID(uavcan_parameter_request)}; + };