diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 958dbabbc0..5237fb17d5 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -227,7 +227,7 @@ then elif [ ! -e "$autostart_file" ] && [ "$SYS_AUTOSTART" -ne "0" ] then echo "Error: no autostart file found ($autostart_file)" - exit 1 + #exit 1 fi #user defined params for instances can be in PATH diff --git a/Tools/msg/px_generate_uorb_topic_files.py b/Tools/msg/px_generate_uorb_topic_files.py index 4a26cbf10a..5221f02079 100755 --- a/Tools/msg/px_generate_uorb_topic_files.py +++ b/Tools/msg/px_generate_uorb_topic_files.py @@ -122,21 +122,13 @@ def generate_output_from_file(format_idx, filename, outputdir, package, template for field in spec.parsed_fields(): field_name_and_type.update({field.name: field.type}) - # assert if the timestamp field exists - try: - assert 'timestamp' in field_name_and_type - except AssertionError: - print("[ERROR] uORB topic files generator:\n\tgenerate_output_from_file:\tNo 'timestamp' field found in " + - spec.short_name + " msg definition!") - exit(1) - # assert if the timestamp field is of type uint64 - try: - assert field_name_and_type.get('timestamp') == 'uint64' - except AssertionError: - print("[ERROR] uORB topic files generator:\n\tgenerate_output_from_file:\t'timestamp' field in " + spec.short_name + - " msg definition is not of type uint64 but rather of type " + field_name_and_type.get('timestamp') + "!") - exit(1) + # try: + # assert field_name_and_type.get('timestamp') == 'uint64' + # except AssertionError: + # print("[ERROR] uORB topic files generator:\n\tgenerate_output_from_file:\t'timestamp' field in " + spec.short_name + + # " msg definition is not of type uint64 but rather of type " + field_name_and_type.get('timestamp') + "!") + # exit(1) topics = get_topics(filename) diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 2f6358f9eb..669ba25837 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -182,8 +182,8 @@ set(msg_files TrajectoryWaypoint.msg TransponderReport.msg TuneControl.msg - UavcanParameterRequest.msg - UavcanParameterValue.msg + ParameterRequest.msg + ParameterValue.msg UlogStream.msg UlogStreamAck.msg UwbDistance.msg @@ -219,6 +219,14 @@ set(msg_files WheelEncoders.msg Wind.msg YawEstimatorStatus.msg + + Parameter.msg + + SrvParameterGetRequest.msg + SrvParameterGetResponse.msg + + SrvParameterSetRequest.msg + SrvParameterSetResponse.msg ) list(SORT msg_files) diff --git a/msg/Parameter.msg b/msg/Parameter.msg new file mode 100644 index 0000000000..5f42c19ef7 --- /dev/null +++ b/msg/Parameter.msg @@ -0,0 +1,12 @@ +char[17] name # parameter name +int16 index # -1 if the param_id field should be used as identifier +int16 index_used # -1 if the param_id field should be used as identifier + +uint8 TYPE_INVALID = 0 +uint8 TYPE_BOOL = 1 +uint8 TYPE_INT32 = 2 +uint8 TYPE_FLOAT32 = 3 +uint8 type # parameter type + +int32 int32_value # current value if param_type is int-like +float32 float32_value # current value if param_type is float-like diff --git a/msg/ParameterRequest.msg b/msg/ParameterRequest.msg new file mode 100644 index 0000000000..8954d74c31 --- /dev/null +++ b/msg/ParameterRequest.msg @@ -0,0 +1,27 @@ +# parameter request type +uint64 timestamp # time since system start (microseconds) + +uint8 MESSAGE_TYPE_INVALID = 0 +uint8 MESSAGE_TYPE_PARAM_REQUEST_READ = 1 +uint8 MESSAGE_TYPE_PARAM_REQUEST_LIST = 2 +uint8 MESSAGE_TYPE_PARAM_SET = 3 +uint8 message_type # message type + +uint8 NODE_ID_ALL = 0 # MAV_COMP_ID_ALL +uint8 node_id # node ID + +char[17] name # parameter name +int16 param_index # -1 if the param_id field should be used as identifier + +uint8 TYPE_INVALID = 0 +uint8 TYPE_BOOL = 1 +uint8 TYPE_INT32 = 2 +uint8 TYPE_FLOAT32 = 3 +uint8 type # parameter type + +int32 int32_value # current value if param_type is int-like +float32 float32_value # current value if param_type is float-like + +uint8 ORB_QUEUE_LENGTH = 8 + +# TOPICS parameter_request uavcan_parameter_request diff --git a/msg/ParameterUpdate.msg b/msg/ParameterUpdate.msg index bfb4993749..facbd3278f 100644 --- a/msg/ParameterUpdate.msg +++ b/msg/ParameterUpdate.msg @@ -4,11 +4,6 @@ uint64 timestamp # time since system start (microseconds) uint32 instance # Instance count - constantly incrementing -uint32 get_count -uint32 set_count -uint32 find_count -uint32 export_count +uint16 count -uint16 active -uint16 changed -uint16 custom_default +Parameter changed_param diff --git a/msg/ParameterValue.msg b/msg/ParameterValue.msg new file mode 100644 index 0000000000..dfd32ef1b0 --- /dev/null +++ b/msg/ParameterValue.msg @@ -0,0 +1,22 @@ +# parameter response type +uint64 timestamp # time since system start (microseconds) + +uint8 node_id # requester node ID + +char[17] param_name # parameter name + +int16 param_index # parameter index, if known + +uint16 param_count # number of parameters exposed by the node + +uint8 type # parameter type + +int32 int32_value # current value if param_type is int-like +float32 float32_value # current value if param_type is float-like + + +uint64 timestamp_requested + +uint8 ORB_QUEUE_LENGTH = 8 + +# TOPICS parameter_value uavcan_parameter_value diff --git a/msg/SrvParameterGetRequest.msg b/msg/SrvParameterGetRequest.msg new file mode 100644 index 0000000000..5c46cb860c --- /dev/null +++ b/msg/SrvParameterGetRequest.msg @@ -0,0 +1,6 @@ +uint64 timestamp # time since system start (microseconds) + +char[17] name # parameter name +int16 index # -1 if the param_id field should be used as identifier + +uint8 ORB_QUEUE_LENGTH = 16 diff --git a/msg/SrvParameterGetResponse.msg b/msg/SrvParameterGetResponse.msg new file mode 100644 index 0000000000..fd624610fb --- /dev/null +++ b/msg/SrvParameterGetResponse.msg @@ -0,0 +1,11 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_requested + +Parameter parameter + +uint8 RESULT_GET_SUCCESS = 0 +uint8 RESULT_GET_FAILED = 1 +uint8 RESULT_ERROR_INVALID_PARAMETER = 2 +uint8 result + +uint8 ORB_QUEUE_LENGTH = 16 diff --git a/msg/SrvParameterSetRequest.msg b/msg/SrvParameterSetRequest.msg new file mode 100644 index 0000000000..b6bd7fe7c4 --- /dev/null +++ b/msg/SrvParameterSetRequest.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) + +Parameter parameter + +uint8 ORB_QUEUE_LENGTH = 8 diff --git a/msg/SrvParameterSetResponse.msg b/msg/SrvParameterSetResponse.msg new file mode 100644 index 0000000000..0ade1775a9 --- /dev/null +++ b/msg/SrvParameterSetResponse.msg @@ -0,0 +1,11 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_requested + +Parameter parameter + +uint8 RESULT_SET_SUCCESS = 0 +uint8 RESULT_SET_FAILED = 1 +uint8 RESULT_ERROR_INVALID_PARAMETER = 2 +uint8 result + +uint8 ORB_QUEUE_LENGTH = 8 diff --git a/msg/UavcanParameterRequest.msg b/msg/UavcanParameterRequest.msg deleted file mode 100644 index 3cfec15f81..0000000000 --- a/msg/UavcanParameterRequest.msg +++ /dev/null @@ -1,23 +0,0 @@ -# UAVCAN-MAVLink parameter bridge request type -uint64 timestamp # time since system start (microseconds) - -uint8 MESSAGE_TYPE_PARAM_REQUEST_READ = 20 # MAVLINK_MSG_ID_PARAM_REQUEST_READ -uint8 MESSAGE_TYPE_PARAM_REQUEST_LIST = 21 # MAVLINK_MSG_ID_PARAM_REQUEST_LIST -uint8 MESSAGE_TYPE_PARAM_SET = 23 # MAVLINK_MSG_ID_PARAM_SET -uint8 message_type # MAVLink message type: PARAM_REQUEST_READ, PARAM_REQUEST_LIST, PARAM_SET - -uint8 NODE_ID_ALL = 0 # MAV_COMP_ID_ALL -uint8 node_id # UAVCAN node ID mapped from MAVLink component ID - -char[17] param_id # MAVLink/UAVCAN parameter name -int16 param_index # -1 if the param_id field should be used as identifier - -uint8 PARAM_TYPE_UINT8 = 1 # MAV_PARAM_TYPE_UINT8 -uint8 PARAM_TYPE_INT64 = 8 # MAV_PARAM_TYPE_INT64 -uint8 PARAM_TYPE_REAL32 = 9 # MAV_PARAM_TYPE_REAL32 -uint8 param_type # MAVLink parameter type - -int64 int_value # current value if param_type is int-like -float32 real_value # current value if param_type is float-like - -uint8 ORB_QUEUE_LENGTH = 4 diff --git a/msg/UavcanParameterValue.msg b/msg/UavcanParameterValue.msg deleted file mode 100644 index 8eff663a57..0000000000 --- a/msg/UavcanParameterValue.msg +++ /dev/null @@ -1,9 +0,0 @@ -# UAVCAN-MAVLink parameter bridge response type -uint64 timestamp # time since system start (microseconds) -uint8 node_id # UAVCAN node ID mapped from MAVLink component ID -char[17] param_id # MAVLink/UAVCAN parameter name -int16 param_index # parameter index, if known -uint16 param_count # number of parameters exposed by the node -uint8 param_type # MAVLink parameter type -int64 int_value # current value if param_type is int-like -float32 real_value # current value if param_type is float-like diff --git a/platforms/common/include/px4_platform_common/param.h b/platforms/common/include/px4_platform_common/param.h index 520c56fc92..20c2a54ebd 100644 --- a/platforms/common/include/px4_platform_common/param.h +++ b/platforms/common/include/px4_platform_common/param.h @@ -118,7 +118,6 @@ public: Param() { - param_set_used(handle()); update(); } @@ -164,7 +163,6 @@ public: Param(float &external_val) : _val(external_val) { - param_set_used(handle()); update(); } @@ -208,7 +206,6 @@ public: Param() { - param_set_used(handle()); update(); } @@ -254,7 +251,6 @@ public: Param(int32_t &external_val) : _val(external_val) { - param_set_used(handle()); update(); } @@ -298,7 +294,6 @@ public: Param() { - param_set_used(handle()); update(); } diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp index 90b354d882..03e828b712 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -76,6 +76,8 @@ static constexpr wq_config_t hp_default{"wq:hp_default", 1900, -18}; static constexpr wq_config_t uavcan{"wq:uavcan", 3624, -19}; +static constexpr wq_config_t parameter_server{"wq:parameter_server", 4096, -20}; + static constexpr wq_config_t ttyS0{"wq:ttyS0", 1632, -21}; static constexpr wq_config_t ttyS1{"wq:ttyS1", 1632, -22}; static constexpr wq_config_t ttyS2{"wq:ttyS2", 1632, -23}; diff --git a/platforms/common/uORB/SubscriptionBlocking.hpp b/platforms/common/uORB/SubscriptionBlocking.hpp index 7ec29e329b..e00ff5bcf4 100644 --- a/platforms/common/uORB/SubscriptionBlocking.hpp +++ b/platforms/common/uORB/SubscriptionBlocking.hpp @@ -85,6 +85,7 @@ public: virtual ~SubscriptionBlocking() { + unregisterCallback(); pthread_mutex_destroy(&_mutex); pthread_cond_destroy(&_cv); } diff --git a/platforms/common/uORB/uORBDeviceNode.cpp b/platforms/common/uORB/uORBDeviceNode.cpp index 7cc5c9bd3e..e1a71cf3d2 100644 --- a/platforms/common/uORB/uORBDeviceNode.cpp +++ b/platforms/common/uORB/uORBDeviceNode.cpp @@ -514,7 +514,7 @@ uORB::DeviceNode::register_callback(uORB::SubscriptionCallback *callback_sub) ATOMIC_ENTER; // prevent duplicate registrations - for (auto existing_callbacks : _callbacks) { + for (const auto &existing_callbacks : _callbacks) { if (callback_sub == existing_callbacks) { ATOMIC_LEAVE; return true; diff --git a/platforms/nuttx/src/px4/common/px4_init.cpp b/platforms/nuttx/src/px4/common/px4_init.cpp index fb371b9093..57b5321fce 100644 --- a/platforms/nuttx/src/px4/common/px4_init.cpp +++ b/platforms/nuttx/src/px4/common/px4_init.cpp @@ -127,8 +127,6 @@ int px4_platform_init() hrt_ioctl_init(); #endif - param_init(); - /* configure CPU load estimation */ #ifdef CONFIG_SCHED_INSTRUMENTATION cpuload_initialize_once(); @@ -185,6 +183,8 @@ int px4_platform_init() uorb_start(); + param_init(); + px4_log_initialize(); #if defined(CONFIG_SYSTEM_CDCACM) && defined(CONFIG_BUILD_FLAT) diff --git a/platforms/posix/src/px4/common/px4_init.cpp b/platforms/posix/src/px4/common/px4_init.cpp index 71af70565d..d2dcaf1a60 100644 --- a/platforms/posix/src/px4/common/px4_init.cpp +++ b/platforms/posix/src/px4/common/px4_init.cpp @@ -44,12 +44,12 @@ int px4_platform_init(void) { hrt_init(); - param_init(); - px4::WorkQueueManagerStart(); uorb_start(); + param_init(); + px4_log_initialize(); return PX4_OK; diff --git a/platforms/ros2/include/px4_platform_common/param.h b/platforms/ros2/include/px4_platform_common/param.h index 04c3bf0f67..043ec822a4 100644 --- a/platforms/ros2/include/px4_platform_common/param.h +++ b/platforms/ros2/include/px4_platform_common/param.h @@ -107,7 +107,6 @@ public: Param() { - //param_set_used(handle()); update(); } @@ -165,7 +164,6 @@ public: Param(float &external_val) : _val(external_val) { - //param_set_used(handle()); // TODO update(); } @@ -221,7 +219,6 @@ public: Param() { - //param_set_used(handle()); update(); } @@ -279,7 +276,6 @@ public: Param(int32_t &external_val) : _val(external_val) { - //param_set_used(handle()); update(); } @@ -335,7 +331,6 @@ public: Param() { - //param_set_used(handle()); update(); } diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index 6fcfd4faf9..f6974a8272 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -701,7 +701,8 @@ UavcanNode::Run() // Check for parameter requests (get/set/list) if (_param_request_sub.updated() && !_param_list_in_progress && !_param_in_progress && !_count_in_progress) { - uavcan_parameter_request_s request{}; + + parameter_request_s request{}; _param_request_sub.copy(&request); if (_param_counts[request.node_id]) { @@ -709,14 +710,14 @@ UavcanNode::Run() * We know how many parameters are exposed by this node, so * process the request. */ - if (request.message_type == uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_READ) { + if (request.message_type == parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_READ) { uavcan::protocol::param::GetSet::Request req; if (request.param_index >= 0) { req.index = request.param_index; } else { - req.name = (char *)request.param_id; + req.name = (char *)request.name; } int call_res = _param_getset_client.call(request.node_id, req); @@ -729,24 +730,28 @@ UavcanNode::Run() _param_index = request.param_index; } - } else if (request.message_type == uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_SET) { + } 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; + req.name = (char *)request.name; } - if (request.param_type == uavcan_parameter_request_s::PARAM_TYPE_REAL32) { - req.value.to() = request.real_value; + switch (request.type) { + case parameter_request_s::TYPE_BOOL: + req.value.to() = request.int32_value; + break; - } else if (request.param_type == uavcan_parameter_request_s::PARAM_TYPE_UINT8) { - req.value.to() = request.int_value; + case parameter_request_s::TYPE_FLOAT32: + req.value.to() = request.float32_value; + break; - } else { - req.value.to() = request.int_value; + default: + req.value.to() = request.int32_value; + break; } // Set the dirty bit for this node @@ -762,7 +767,7 @@ UavcanNode::Run() _param_index = request.param_index; } - } else if (request.message_type == uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_LIST) { + } 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; @@ -772,8 +777,8 @@ UavcanNode::Run() PX4_DEBUG("starting component-specific param list"); } - } else if (request.node_id == uavcan_parameter_request_s::NODE_ID_ALL) { - if (request.message_type == uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_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. @@ -1091,24 +1096,24 @@ UavcanNode::cb_getset(const uavcan::ServiceCallResult(); + response.type = parameter_request_s::TYPE_INT32; + response.int32_value = param.value.to(); } else if (param.value.is(uavcan::protocol::param::Value::Tag::real_value)) { - response.param_type = uavcan_parameter_request_s::PARAM_TYPE_REAL32; - response.real_value = param.value.to(); + response.type = parameter_request_s::TYPE_FLOAT32; + response.float32_value = param.value.to(); } else if (param.value.is(uavcan::protocol::param::Value::Tag::boolean_value)) { - response.param_type = uavcan_parameter_request_s::PARAM_TYPE_UINT8; - response.int_value = param.value.to(); + response.type = parameter_request_s::TYPE_BOOL; + response.int32_value = param.value.to(); } _param_response_pub.publish(response); diff --git a/src/drivers/uavcan/uavcan_main.hpp b/src/drivers/uavcan/uavcan_main.hpp index f896fc742e..f98e130a8c 100644 --- a/src/drivers/uavcan/uavcan_main.hpp +++ b/src/drivers/uavcan/uavcan_main.hpp @@ -75,8 +75,8 @@ #include #include #include -#include -#include +#include +#include #include using namespace time_literals; @@ -267,7 +267,7 @@ private: uORB::Subscription _vcmd_sub{ORB_ID(vehicle_command)}; uORB::Subscription _param_request_sub{ORB_ID(uavcan_parameter_request)}; - uORB::Publication _param_response_pub{ORB_ID(uavcan_parameter_value)}; + uORB::Publication _param_response_pub{ORB_ID(uavcan_parameter_value)}; uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; /* diff --git a/src/drivers/uavcan/uavcan_servers.cpp b/src/drivers/uavcan/uavcan_servers.cpp index fa4b2aab10..aea8192789 100644 --- a/src/drivers/uavcan/uavcan_servers.cpp +++ b/src/drivers/uavcan/uavcan_servers.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/lib/parameters/CMakeLists.txt b/src/lib/parameters/CMakeLists.txt index 908776bd41..4d2e67d3fa 100644 --- a/src/lib/parameters/CMakeLists.txt +++ b/src/lib/parameters/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2017 - 2020 PX4 Development Team. All rights reserved. +# Copyright (c) 2017 - 2022 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -149,6 +149,12 @@ add_custom_target(parameters_header DEPENDS px4_parameters.hpp) set(SRCS) +set(PX4_PARAM_CLIENT_SRV 1) +if(PX4_PARAM_CLIENT_SRV) + add_compile_definitions(PX4_PARAM_CLIENT_SRV) + list(APPEND SRCS parameters_srv.cpp) +endif() + list(APPEND SRCS parameters.cpp) if(BUILD_TESTING) @@ -157,31 +163,23 @@ else() list(APPEND SRCS param_translation.cpp) endif() -if(${PX4_PLATFORM} STREQUAL "nuttx") - add_subdirectory(flashparams) - - # build user-side interface for protected build - if(NOT CONFIG_BUILD_FLAT) - list(APPEND SRCS parameters_ioctl.cpp) - add_library(usr_parameters usr_parameters_if.cpp px4_parameters.hpp) - add_dependencies(usr_parameters prebuild_targets) - target_compile_definitions(usr_parameters PRIVATE -DMODULE_NAME="usr_parameters") - endif() -endif() - # TODO: find a better way to do this if(NOT "${PX4_BOARD}" MATCHES "px4_io") add_library(parameters STATIC EXCLUDE_FROM_ALL ${SRCS} px4_parameters.hpp + ParameterServer.cpp + ParameterServer.hpp ) target_link_libraries(parameters PRIVATE perf tinybson px4_platform) - target_compile_definitions(parameters PRIVATE -DMODULE_NAME="parameters" -D__KERNEL__) + target_compile_definitions(parameters PRIVATE -DMODULE_NAME="parameters") target_compile_options(parameters PRIVATE #-DDEBUG_BUILD + #${MAX_CUSTOM_OPT_LEVEL} + -O0 -Wno-cast-align # TODO: fix and enable -Wno-sign-compare # TODO: fix and enable ) @@ -191,7 +189,7 @@ endif() add_dependencies(parameters prebuild_targets) if(${PX4_PLATFORM} STREQUAL "nuttx") - target_link_libraries(parameters PRIVATE flashparams) + #target_link_libraries(parameters PRIVATE flashparams) endif() px4_add_functional_gtest(SRC ParameterTest.cpp LINKLIBS parameters) diff --git a/src/lib/parameters/ParameterServer.cpp b/src/lib/parameters/ParameterServer.cpp new file mode 100644 index 0000000000..64a98e9d0f --- /dev/null +++ b/src/lib/parameters/ParameterServer.cpp @@ -0,0 +1,1819 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#define PARAM_IMPLEMENTATION + +#include "ParameterServer.hpp" + +#include "param_translation.h" + +#include +#include +#include + +// #if defined(FLASH_BASED_PARAMS) +// #include "flashparams/flashparams.h" +// #else +// inline static int flash_param_save(param_filter_func filter) { return -1; } +// inline static int flash_param_load() { return -1; } +// inline static int flash_param_import() { return -1; } +// #endif + +ParameterServer::ParameterServer() : + ScheduledWorkItem("parameter_server", px4::wq_configurations::parameter_server) +{ + px4_sem_init(&_param_sem, 0, 1); + px4_sem_init(&_param_sem_save, 0, 1); + px4_sem_init(&_reader_lock_holders_lock, 0, 1); + + _srv_parameter_get_request_sub.registerCallback(); + _srv_parameter_set_request_sub.registerCallback(); + + _param_request_sub.registerCallback(); + + ScheduleOnInterval(100_ms); +} + +ParameterServer::~ParameterServer() +{ + px4_sem_destroy(&_param_sem); + px4_sem_destroy(&_param_sem_save); + px4_sem_destroy(&_reader_lock_holders_lock); + + perf_free(_export_perf); + perf_free(_find_count_perf); + perf_free(_get_count_perf); + perf_free(_set_perf); +} + +void ParameterServer::lockReader() +{ + do {} while (px4_sem_wait(&_reader_lock_holders_lock) != 0); + + ++_reader_lock_holders; + + if (_reader_lock_holders == 1) { + // the first reader takes the lock, the next ones are allowed to just continue + do {} while (px4_sem_wait(&_param_sem) != 0); + } + + px4_sem_post(&_reader_lock_holders_lock); +} + +void ParameterServer::lockWriter() +{ + do {} while (px4_sem_wait(&_param_sem) != 0); +} + +void ParameterServer::unlockReader() +{ + do {} while (px4_sem_wait(&_reader_lock_holders_lock) != 0); + + --_reader_lock_holders; + + if (_reader_lock_holders == 0) { + // the last reader releases the lock + px4_sem_post(&_param_sem); + } + + px4_sem_post(&_reader_lock_holders_lock); +} + +void ParameterServer::unlockWriter() +{ + px4_sem_post(&_param_sem); +} + +int ParameterServer::compareValues(const void *a, const void *b) +{ + struct param_wbuf_s *pa = (struct param_wbuf_s *)a; + struct param_wbuf_s *pb = (struct param_wbuf_s *)b; + + if (pa->param < pb->param) { + return -1; + } + + if (pa->param > pb->param) { + return 1; + } + + return 0; +} + +ParameterServer::param_wbuf_s *ParameterServer::findChanged(param_t param) +{ + if (_params_changed[param] && (_param_values != nullptr)) { + param_wbuf_s key{}; + key.param = param; + return (param_wbuf_s *)utarray_find(_param_values, &key, ParameterServer::compareValues); + } + + return nullptr; +} + +const void *ParameterServer::getParameterValuePointer(param_t param) +{ + if (handle_in_range(param)) { + /* work out whether we're fetching the default or a written value */ + struct param_wbuf_s *s = findChanged(param); + + if (s != nullptr) { + return &s->val; + + } else { + if (_params_custom_default[param] && _param_custom_default_values) { + // get default from custom default storage + param_wbuf_s key{}; + key.param = param; + param_wbuf_s *pbuf = (param_wbuf_s *)utarray_find(_param_custom_default_values, &key, ParameterServer::compareValues); + + if (pbuf != nullptr) { + return &pbuf->val; + } + } + + // otherwise return static default value + switch (getParameterType(param)) { + case PARAM_TYPE_INT32: + return &px4::parameters[param].val.i; + + case PARAM_TYPE_FLOAT: + return &px4::parameters[param].val.f; + } + } + } + + return nullptr; +} + +int ParameterServer::getParameterDefaultValueInternal(param_t param, void *default_val) +{ + if (!handle_in_range(param)) { + PX4_ERR("get default value: param %d invalid", param); + return PX4_ERROR; + } + + if (default_val) { + if (_params_custom_default[param] && _param_custom_default_values) { + // get default from custom default storage + param_wbuf_s key{}; + key.param = param; + param_wbuf_s *pbuf = (param_wbuf_s *)utarray_find(_param_custom_default_values, &key, ParameterServer::compareValues); + + if (pbuf != nullptr) { + memcpy(default_val, &pbuf->val, getParameterSize(param)); + return PX4_OK; + } + } + + // otherwise return static default value + switch (getParameterType(param)) { + case PARAM_TYPE_INT32: + memcpy(default_val, &px4::parameters[param].val.i, getParameterSize(param)); + return PX4_OK; + + case PARAM_TYPE_FLOAT: + memcpy(default_val, &px4::parameters[param].val.f, getParameterSize(param)); + return PX4_OK; + } + } + + return PX4_ERROR; +} + +param_t ParameterServer::findParameter(const char *name, bool notification) +{ + perf_count(_find_count_perf); + + param_t middle; + param_t front = 0; + param_t last = param_info_count; + + // perform a binary search of the known parameters + while (front <= last) { + middle = front + (last - front) / 2; + int ret = strcmp(name, getParameterName(middle)); + + if (ret == 0) { + if (notification) { + _params_active.set(middle, true); + } + + return middle; + + } else if (middle == front) { + // An end point has been hit, but there has been no match + break; + + } else if (ret < 0) { + last = middle; + + } else { + front = middle; + } + } + + // not found + return PARAM_INVALID; +} + +bool ParameterServer::isParameterUsed(param_t param) const +{ + if (handle_in_range(param)) { + return _params_active[param]; + } + + return false; +} + +param_t ParameterServer::forIndex(unsigned index) const +{ + if (index < param_info_count) { + return (param_t)index; + } + + return PARAM_INVALID; +} + +param_t ParameterServer::forUsedIndex(unsigned index) const +{ + // walk all params and count used params + if (index < param_info_count) { + unsigned used_count = 0; + + for (int i = 0; i < _params_active.size(); i++) { + if (_params_active[i]) { + // we found the right used count, + // return the param value + if (index == used_count) { + return static_cast(i); + } + + used_count++; + } + } + } + + return PARAM_INVALID; +} + +int ParameterServer::getParameterIndex(param_t param) const +{ + if (handle_in_range(param)) { + return (unsigned)param; + } + + return -1; +} + +int ParameterServer::getParameterUsedIndex(param_t param) const +{ + // this tests for out of bounds and does a constant time lookup + if (!isParameterUsed(param)) { + return -1; + } + + // walk all params and count, now knowing that it has a valid index + int used_count = 0; + + for (int i = 0; i < _params_active.size(); i++) { + if (_params_active[i]) { + + if (param == i) { + return used_count; + } + + used_count++; + } + } + + return -1; +} + +const char *ParameterServer::getParameterName(param_t param) const +{ + if (handle_in_range(param)) { + return px4::parameters[param].name; + } + + return nullptr; +} + +bool ParameterServer::isParameterVolatile(param_t param) const +{ + if (handle_in_range(param)) { + for (const auto &p : px4::parameters_volatile) { + if (static_cast(param) == p) { + return true; + } + } + } + + return false; +} + +bool ParameterServer::isParameterValueDefault(param_t param) +{ + if (!handle_in_range(param)) { + return true; + } + + if (!_params_changed[param] && !_params_custom_default[param]) { + // no value saved and no custom default + return true; + + } else { + // the _param_values dynamic array might carry things that have been set + // back to default, so we don't rely on the _params_changed bitset here + switch (getParameterType(param)) { + case PARAM_TYPE_INT32: { + lockReader(); + int32_t default_value = 0; + + if (getParameterDefaultValueInternal(param, &default_value) == PX4_OK) { + const void *v = getParameterValuePointer(param); + + if (v) { + bool unchanged = (*static_cast(v) == default_value); + unlockReader(); + return unchanged; + } + } + + unlockReader(); + } + break; + + case PARAM_TYPE_FLOAT: { + lockReader(); + float default_value = 0; + + if (getParameterDefaultValueInternal(param, &default_value) == PX4_OK) { + const void *v = getParameterValuePointer(param); + + if (v) { + bool unchanged = (fabsf(*static_cast(v) - default_value) <= FLT_EPSILON); + unlockReader(); + return unchanged; + } + } + + unlockReader(); + } + break; + } + } + + return true; +} + +bool ParameterServer::isParameterValueUnsaved(param_t param) +{ + return handle_in_range(param) ? _params_unsaved[param] : false; +} + +param_type_t ParameterServer::getParameterType(param_t param) const +{ + return handle_in_range(param) ? px4::parameters_type[param] : PARAM_TYPE_UNKNOWN; +} + +size_t ParameterServer::getParameterSize(param_t param) const +{ + if (handle_in_range(param)) { + switch (getParameterType(param)) { + case PARAM_TYPE_INT32: + case PARAM_TYPE_FLOAT: + return 4; + + default: + return 0; + } + } + + return 0; +} + +int ParameterServer::getParameterValue(param_t param, void *val) +{ + perf_count(_get_count_perf); + + if (!handle_in_range(param)) { + PX4_ERR("get: param %" PRId16 " invalid", param); + return PX4_ERROR; + } + + if (!_params_active[param]) { + PX4_DEBUG("get: param %" PRId16 " (%s) not active", param, getParameterName(param)); + _params_active.set(param, true); + } + + int result = PX4_ERROR; + + if (val) { + if (!_params_changed[param] && !_params_custom_default[param]) { + // if parameter is unchanged (static default value) copy immediately and avoid locking + switch (getParameterType(param)) { + case PARAM_TYPE_INT32: + memcpy(val, &px4::parameters[param].val.i, sizeof(px4::parameters[param].val.i)); + return PX4_OK; + + case PARAM_TYPE_FLOAT: + memcpy(val, &px4::parameters[param].val.f, sizeof(px4::parameters[param].val.f)); + return PX4_OK; + } + } + + lockReader(); + const void *v = getParameterValuePointer(param); + + if (v) { + memcpy(val, v, getParameterSize(param)); + result = PX4_OK; + } + + unlockReader(); + } + + return result; +} + +int ParameterServer::getParameterDefaultValue(param_t param, void *default_val) +{ + if (!handle_in_range(param)) { + return PX4_ERROR; + } + + int ret = 0; + + if (!_params_custom_default[param]) { + // return static default value + switch (getParameterType(param)) { + case PARAM_TYPE_INT32: + memcpy(default_val, &px4::parameters[param].val.i, sizeof(px4::parameters[param].val.i)); + return PX4_OK; + + case PARAM_TYPE_FLOAT: + memcpy(default_val, &px4::parameters[param].val.f, sizeof(px4::parameters[param].val.f)); + return PX4_OK; + } + + } else { + lockReader(); + ret = getParameterDefaultValueInternal(param, default_val); + unlockReader(); + } + + return ret; +} + +int ParameterServer::getParameterSystemDefaultValue(param_t param, void *default_val) +{ + if (!handle_in_range(param)) { + return PX4_ERROR; + } + + int ret = PX4_OK; + + switch (getParameterType(param)) { + case PARAM_TYPE_INT32: + memcpy(default_val, &px4::parameters[param].val.i, getParameterSize(param)); + break; + + case PARAM_TYPE_FLOAT: + memcpy(default_val, &px4::parameters[param].val.f, getParameterSize(param)); + break; + + default: + ret = PX4_ERROR; + break; + } + + return ret; +} + +int ParameterServer::setParameter(param_t param, const void *val, bool mark_saved, bool notify_changes) +{ + if (!handle_in_range(param)) { + PX4_ERR("set invalid param %d", param); + return PX4_ERROR; + } + + if (val == nullptr) { + PX4_ERR("set invalid value"); + return PX4_ERROR; + } + + int result = -1; + bool param_changed = false; + + lockWriter(); + perf_begin(_set_perf); + + // create the parameter store if it doesn't exist + if (_param_values == nullptr) { + utarray_new(_param_values, ¶m_icd); + + // mark all parameters unchanged (default) + _params_changed.reset(); + _params_unsaved.reset(); + } + + if (_param_values == nullptr) { + PX4_ERR("failed to allocate modified values array"); + goto out; + + } else { + param_wbuf_s *s = findChanged(param); + + if (s == nullptr) { + /* construct a new parameter */ + param_wbuf_s buf{}; + buf.param = param; + + param_changed = true; + + /* add it to the array and sort */ + utarray_push_back(_param_values, &buf); + utarray_sort(_param_values, ParameterServer::compareValues); + _params_changed.set(param, true); + + /* find it after sorting */ + s = findChanged(param); + } + + if (s == nullptr) { + PX4_ERR("error _param_values storage slot invalid"); + + } else { + /* update the changed value */ + switch (getParameterType(param)) { + case PARAM_TYPE_INT32: + if (s->val.i != *(int32_t *)val) { + s->val.i = *(int32_t *)val; + param_changed = true; + } + + _params_changed.set(param, true); + _params_unsaved.set(param, !mark_saved); + result = PX4_OK; + break; + + case PARAM_TYPE_FLOAT: + if (fabsf(s->val.f - * (float *)val) > FLT_EPSILON) { + s->val.f = *(float *)val; + param_changed = true; + } + + _params_changed.set(param, true); + _params_unsaved.set(param, !mark_saved); + result = PX4_OK; + break; + + default: + PX4_ERR("param_set invalid param type for %s", getParameterName(param)); + break; + } + } + + if ((result == PX4_OK) && param_changed && !mark_saved) { // this is false when importing parameters + autoSave(); + } + } + +out: + perf_end(_set_perf); + unlockWriter(); + + /* + * If we set something, now that we have unlocked, go ahead and advertise that + * a thing has been set. + */ + if ((result == PX4_OK) && param_changed && notify_changes) { + notifyChanges(); + } + + return result; +} + +int ParameterServer::setParameterDefaultValue(param_t param, const void *val) +{ + if (!handle_in_range(param)) { + PX4_ERR("set default value invalid param %d", param); + return PX4_ERROR; + } + + if (val == nullptr) { + PX4_ERR("set default value invalid value"); + return PX4_ERROR; + } + + int result = PX4_ERROR; + + lockWriter(); + + if (_param_custom_default_values == nullptr) { + utarray_new(_param_custom_default_values, ¶m_icd); + + // mark all parameters unchanged (default) + _params_custom_default.reset(); + + if (_param_custom_default_values == nullptr) { + PX4_ERR("failed to allocate custom default values array"); + unlockWriter(); + return PX4_ERROR; + } + } + + // check if param being set to default value + bool setting_to_static_default = false; + + switch (getParameterType(param)) { + case PARAM_TYPE_INT32: + setting_to_static_default = (px4::parameters[param].val.i == *(int32_t *)val); + break; + + case PARAM_TYPE_FLOAT: + setting_to_static_default = (fabsf(px4::parameters[param].val.f - * (float *)val) <= FLT_EPSILON); + break; + } + + // find if custom default value is already set + param_wbuf_s *s = nullptr; + + { + param_wbuf_s key{}; + key.param = param; + s = (param_wbuf_s *)utarray_find(_param_custom_default_values, &key, ParameterServer::compareValues); + } + + if (setting_to_static_default) { + if (s != nullptr) { + // param in memory and set to non-default value, clear + int pos = utarray_eltidx(_param_custom_default_values, s); + utarray_erase(_param_custom_default_values, pos, 1); + } + + // do nothing if param not already set and being set to default + _params_custom_default.set(param, false); + result = PX4_OK; + + } else { + if (s == nullptr) { + // construct a new parameter default value + param_wbuf_s buf{}; + buf.param = param; + + // add it to the array and sort + utarray_push_back(_param_custom_default_values, &buf); + utarray_sort(_param_custom_default_values, ParameterServer::compareValues); + + // find it after sorting + s = (param_wbuf_s *)utarray_find(_param_custom_default_values, &buf, ParameterServer::compareValues); + } + + if (s != nullptr) { + // update the default value + switch (getParameterType(param)) { + case PARAM_TYPE_INT32: + s->val.i = *(int32_t *)val; + _params_custom_default.set(param, true); + result = PX4_OK; + break; + + case PARAM_TYPE_FLOAT: + s->val.f = *(float *)val; + _params_custom_default.set(param, true); + result = PX4_OK; + break; + + default: + break; + } + } + } + + unlockWriter(); + + if ((result == PX4_OK) && isParameterUsed(param)) { + // send notification if param is already in use + notifyChanges(); + } + + return result; +} + +void ParameterServer::notifyChanges() +{ + _notify_scheduled.store(true); + ScheduleDelayed(10_ms); + // TODO: delay back to back notifications +} + +int ParameterServer::resetParameter(param_t param) +{ + param_wbuf_s *s = nullptr; + bool param_found = false; + + lockWriter(); + + if (handle_in_range(param)) { + /* look for a saved value */ + s = findChanged(param); + + /* if we found one, erase it */ + if (s != nullptr) { + int pos = utarray_eltidx(_param_values, s); + utarray_erase(_param_values, pos, 1); + } + + _params_changed.set(param, false); + _params_unsaved.set(param, true); + + param_found = true; + } + + autoSave(); + + unlockWriter(); + + if (param_found) { + notifyChanges(); + } + + return (!param_found); +} + +void ParameterServer::resetAllParameters(bool auto_save) +{ + lockWriter(); + + if (_param_values != nullptr) { + utarray_free(_param_values); + + _params_changed.reset(); + } + + /* mark as reset / deleted */ + _param_values = nullptr; + + if (auto_save) { + autoSave(); + } + + unlockWriter(); + + notifyChanges(); +} + +void ParameterServer::resetExcludes(const char *excludes[], int num_excludes) +{ + for (param_t param = 0; handle_in_range(param); param++) { + const char *name = getParameterName(param); + bool exclude = false; + + for (int index = 0; index < num_excludes; index ++) { + int len = strlen(excludes[index]); + + if ((excludes[index][len - 1] == '*' + && strncmp(name, excludes[index], len - 1) == 0) + || strcmp(name, excludes[index]) == 0) { + + exclude = true; + break; + } + } + + if (!exclude) { + resetParameter(param); + } + } +} + +void ParameterServer::resetSpecificParameter(const char *resets[], int num_resets) +{ + for (param_t param = 0; handle_in_range(param); param++) { + const char *name = getParameterName(param); + bool reset = false; + + for (int index = 0; index < num_resets; index++) { + int len = strlen(resets[index]); + + if ((resets[index][len - 1] == '*' + && strncmp(name, resets[index], len - 1) == 0) + || strcmp(name, resets[index]) == 0) { + + reset = true; + break; + } + } + + if (reset) { + resetParameter(param); + } + } +} + +int ParameterServer::exportToFile(const char *filename, param_filter_func filter) +{ + PX4_DEBUG("param_export"); + + int shutdown_lock_ret = px4_shutdown_lock(); + + if (shutdown_lock_ret != 0) { + PX4_ERR("px4_shutdown_lock() failed (%i)", shutdown_lock_ret); + } + + // take the file lock + do {} while (px4_sem_wait(&_param_sem_save) != 0); + + lockReader(); + + int fd = ::open(filename, O_RDWR | O_CREAT, PX4_O_MODE_666); + int result = PX4_ERROR; + + perf_begin(_export_perf); + + if (fd > -1) { + result = exportInternal(fd, filter); + + } else { + // TODO: flash params + // result = flash_param_save(filter); + } + + perf_end(_export_perf); + + unlockReader(); + px4_sem_post(&_param_sem_save); + + if (shutdown_lock_ret == 0) { + px4_shutdown_unlock(); + } + + return result; +} + +int ParameterServer::bsonImportCallback(bson_decoder_t decoder, bson_node_t node) +{ + /* + * EOO means the end of the parameter object. (Currently not supporting + * nested BSON objects). + */ + if (node->type == BSON_EOO) { + PX4_DEBUG("end of parameters"); + return 0; + } + + param_modify_on_import(node); + + // Find the parameter this node represents. If we don't know it, ignore the node. + param_t param = findParameter(node->name, false); + + if (param == PARAM_INVALID) { + PX4_WARN("ignoring unrecognised parameter '%s'", node->name); + return 1; + } + + // Handle setting the parameter from the node + switch (node->type) { + case BSON_INT32: { + if (getParameterType(param) == PARAM_TYPE_INT32) { + int32_t i = node->i32; + setParameter(param, &i, true, true); + PX4_DEBUG("Imported %s with value %" PRIi32, param_name(param), i); + + } else { + PX4_WARN("unexpected type for %s", node->name); + } + } + break; + + case BSON_DOUBLE: { + if (getParameterType(param) == PARAM_TYPE_FLOAT) { + float f = node->d; + setParameter(param, &f, true, true); + PX4_DEBUG("Imported %s with value %f", param_name(param), (double)f); + + } else { + PX4_WARN("unexpected type for %s", node->name); + } + } + break; + + default: + PX4_ERR("import: unrecognised node type for '%s'", node->name); + } + + // don't return zero, that means EOF + return 1; +} + +int ParameterServer::importCallbackTrampoline(bson_decoder_t decoder, void *priv, bson_node_t node) +{ + if (priv) { + return static_cast(priv)->bsonImportCallback(decoder, node); + } + + return -1; +} + +int ParameterServer::importFromFileDescriptorInternal(int fd) +{ + static constexpr int MAX_ATTEMPTS = 3; + + for (int attempt = 1; attempt <= MAX_ATTEMPTS; attempt++) { + bson_decoder_s decoder{}; + + if (bson_decoder_init_file(&decoder, fd, importCallbackTrampoline, this) == 0) { + int result = -1; + + do { + result = bson_decoder_next(&decoder); + + } while (result > 0); + + if (result == 0) { + if (decoder.total_document_size == decoder.total_decoded_size) { + PX4_INFO("BSON document size %" PRId32 " bytes, decoded %" PRId32 " bytes (INT32:%" PRIu16 ", FLOAT:%" PRIu16 ")", + decoder.total_document_size, decoder.total_decoded_size, + decoder.count_node_int32, decoder.count_node_double); + + return 0; + + } else { + PX4_ERR("BSON document size (%" PRId32 ") doesn't match bytes decoded (%" PRId32 ")", + decoder.total_document_size, decoder.total_decoded_size); + } + + } else if (result == -ENODATA) { + // silently retry as a precaution unless this is our last attempt + if (attempt == MAX_ATTEMPTS) { + PX4_DEBUG("BSON: no data"); + return 0; + } + + } else { + PX4_ERR("param import failed (%d) attempt %d", result, attempt); + } + + } else { + PX4_ERR("param import bson decoder init failed (attempt %d)", attempt); + } + + if (attempt != MAX_ATTEMPTS) { + if (lseek(fd, 0, SEEK_SET) != 0) { + PX4_ERR("import lseek failed (%d)", errno); + } + + px4_usleep(10000); // wait at least 10 milliseconds before trying again + } + } + + return -1; +} + +int ParameterServer::importFromFileDescriptor(int fd) +{ + // TODO: flash params + // if (fd < 0) { + // return flash_param_import(); + // } + + return importFromFileDescriptorInternal(fd); +} + +int ParameterServer::loadFromFileDescriptor(int fd) +{ + // TODO: flash params + // if (fd < 0) { + // return flash_param_load(); + // } + + resetAllParameters(false); + + return importFromFileDescriptorInternal(fd); +} + +int ParameterServer::verifyBsonExportCallback(bson_decoder_t decoder, bson_node_t node) +{ + if (node->type == BSON_EOO) { + return 0; + } + + // find the parameter this node represents + param_t param = findParameter(node->name, false); + + if (param == PARAM_INVALID) { + PX4_ERR("verify: invalid parameter '%s'", node->name); + return -1; + } + + // handle verifying the parameter from the node + switch (node->type) { + case BSON_INT32: { + if (getParameterType(param) != PARAM_TYPE_INT32) { + PX4_ERR("verify: invalid param type %d for '%s' (BSON_INT32)", getParameterType(param), node->name); + return -1; + } + + int32_t value; + + if (getParameterValue(param, &value) == 0) { + if (value == node->i32) { + return 1; // valid + + } else { + PX4_ERR("verify: '%s' invalid BSON value %" PRIi32 "(expected %" PRIi32 ")", node->name, node->i32, value); + } + } + } + break; + + case BSON_DOUBLE: { + if (getParameterType(param) != PARAM_TYPE_FLOAT) { + PX4_ERR("verify: invalid param type %d for '%s' (BSON_DOUBLE)", getParameterType(param), node->name); + return -1; + } + + float value; + + if (getParameterValue(param, &value) == 0) { + if (fabsf(value - (float)node->d) <= FLT_EPSILON) { + return 1; // valid + + } else { + PX4_ERR("verify: '%s' invalid BSON value %.3f (expected %.3f)", node->name, node->d, (double)value); + } + } + } + break; + + default: + PX4_ERR("verify: '%s' invalid node type %d", node->name, node->type); + } + + return -1; +} + +int ParameterServer::verifyBsonExportTrampoline(bson_decoder_t decoder, void *priv, bson_node_t node) +{ + if (priv) { + return static_cast(priv)->verifyBsonExportCallback(decoder, node); + } + + return -1; +} + +int ParameterServer::verifyBsonExport(int fd) +{ + PX4_DEBUG("param_verify"); + + if (fd < 0) { + return -1; + } + + if (lseek(fd, 0, SEEK_SET) != 0) { + PX4_ERR("verify: seek failed"); + return -1; + } + + bson_decoder_s decoder{}; + + if (bson_decoder_init_file(&decoder, fd, verifyBsonExportTrampoline, this) == 0) { + int result = -1; + + do { + result = bson_decoder_next(&decoder); + + } while (result > 0); + + if (result == 0) { + if (decoder.total_document_size != decoder.total_decoded_size) { + PX4_ERR("BSON document size (%" PRId32 ") doesn't match bytes decoded (%" PRId32 ")", decoder.total_document_size, + decoder.total_decoded_size); + + } else { + return 0; + } + + } else if (result == -ENODATA) { + PX4_ERR("verify: no BSON data"); + + } else { + PX4_ERR("verify: failed (%d)", result); + } + } + + return -1; +} + +void ParameterServer::forEachParameter(void (*func)(void *arg, param_t param), void *arg, bool only_changed, + bool only_used) +{ + param_t param; + + for (param = 0; handle_in_range(param); param++) { + + /* if requested, skip unchanged values */ + if (only_changed && (findChanged(param) == nullptr)) { + continue; + } + + if (only_used && !isParameterUsed(param)) { + continue; + } + + func(arg, param); + } +} + +int ParameterServer::setDefaultFile(const char *filename) +{ + if ((_param_backup_file && strcmp(filename, _param_backup_file) == 0)) { + PX4_ERR("default file can't be the same as the backup file %s", filename); + return PX4_ERROR; + } + +#ifdef FLASH_BASED_PARAMS + // the default for flash-based params is always the FLASH + (void)filename; +#else + + if (_param_default_file != nullptr) { + // we assume this is not in use by some other thread + free(_param_default_file); + _param_default_file = nullptr; + } + + if (filename) { + _param_default_file = strdup(filename); + } + +#endif /* FLASH_BASED_PARAMS */ + + return 0; +} + +int ParameterServer::setBackupFile(const char *filename) +{ + if (_param_default_file && strcmp(filename, _param_default_file) == 0) { + PX4_ERR("backup file can't be the same as the default file %s", filename); + return PX4_ERROR; + } + + if (_param_backup_file != nullptr) { + // we assume this is not in use by some other thread + free(_param_backup_file); + _param_backup_file = nullptr; + } + + if (filename) { + _param_backup_file = strdup(filename); + + } else { + _param_backup_file = nullptr; // backup disabled + } + + return 0; +} + +int ParameterServer::saveDefault() +{ + PX4_DEBUG("param_save_default"); + int shutdown_lock_ret = px4_shutdown_lock(); + + if (shutdown_lock_ret != 0) { + PX4_ERR("px4_shutdown_lock() failed (%i)", shutdown_lock_ret); + } + + // take the file lock + do {} while (px4_sem_wait(&_param_sem_save) != 0); + + lockReader(); + + int res = PX4_ERROR; + const char *filename = getDefaultFile(); + + if (filename) { + static constexpr int MAX_ATTEMPTS = 3; + + for (int attempt = 1; attempt <= MAX_ATTEMPTS; attempt++) { + // write parameters to file + int fd = ::open(filename, O_WRONLY | O_CREAT | O_TRUNC, PX4_O_MODE_666); + + if (fd > -1) { + perf_begin(_export_perf); + res = exportInternal(fd, nullptr); + perf_end(_export_perf); + ::close(fd); + + if (res == PX4_OK) { + // reopen file to verify + int fd_verify = ::open(filename, O_RDONLY, PX4_O_MODE_666); + res = verifyBsonExport(fd_verify) || lseek(fd_verify, 0, SEEK_SET) || verifyBsonExport(fd_verify); + ::close(fd_verify); + } + } + + if (res == PX4_OK) { + break; + + } else { + PX4_ERR("parameter export to %s failed (%d) attempt %d", filename, res, attempt); + px4_usleep(10000); // wait at least 10 milliseconds before trying again + } + } + + } else { + // TODO: flash params + // perf_begin(_export_perf); + // res = flash_param_save(nullptr); + // perf_end(_export_perf); + } + + if (res != PX4_OK) { + PX4_ERR("param export failed (%d)", res); + + } else { + _params_unsaved.reset(); + + // backup file + if (_param_backup_file) { + int fd_backup_file = ::open(_param_backup_file, O_WRONLY | O_CREAT | O_TRUNC, PX4_O_MODE_666); + + if (fd_backup_file > -1) { + int backup_export_ret = exportInternal(fd_backup_file, nullptr); + ::close(fd_backup_file); + + if (backup_export_ret != 0) { + PX4_ERR("backup parameter export to %s failed (%d)", _param_backup_file, backup_export_ret); + + } else { + // verify export + int fd_verify = ::open(_param_backup_file, O_RDONLY, PX4_O_MODE_666); + verifyBsonExport(fd_verify); + ::close(fd_verify); + } + } + } + } + + unlockReader(); + px4_sem_post(&_param_sem_save); + + if (shutdown_lock_ret == 0) { + px4_shutdown_unlock(); + } + + return res; +} + +int ParameterServer::loadDefault() +{ + int res = 0; + const char *filename = getDefaultFile(); + + // TODO: flash params + // if (!filename) { + // return flash_param_load(); + // } + + int fd_load = ::open(filename, O_RDONLY); + + if (fd_load < 0) { + /* no parameter file is OK, otherwise this is an error */ + if (errno != ENOENT) { + PX4_ERR("open '%s' for reading failed", filename); + return -1; + } + + return 1; + } + + int result = loadFromFileDescriptor(fd_load); + ::close(fd_load); + + if (result != 0) { + PX4_ERR("error reading parameters from '%s'", filename); + return -2; + } + + return res; +} + +uint32_t ParameterServer::hashCheck() +{ + uint32_t param_hash = 0; + + lockReader(); + + /* compute the CRC32 over all string param names and 4 byte values */ + for (param_t param = 0; handle_in_range(param); param++) { + if (!isParameterUsed(param) || isParameterVolatile(param)) { + continue; + } + + const char *name = getParameterName(param); + const void *val = getParameterValuePointer(param); + param_hash = crc32part((const uint8_t *)name, strlen(name), param_hash); + param_hash = crc32part((const uint8_t *)val, getParameterSize(param), param_hash); + } + + unlockReader(); + + return param_hash; +} + +void ParameterServer::printStatus() +{ + PX4_INFO("summary: %d/%d (used/total)", countUsed(), count()); + +#ifndef FLASH_BASED_PARAMS + const char *filename = getDefaultFile(); + + if (filename != nullptr) { + PX4_INFO("file: %s", filename); + } + + if (_param_backup_file) { + PX4_INFO("backup file: %s", _param_backup_file); + } + +#endif /* FLASH_BASED_PARAMS */ + + if (_param_values != nullptr) { + PX4_INFO("storage array: %d/%d elements (%zu bytes total)", + utarray_len(_param_values), _param_values->n, _param_values->n * sizeof(UT_icd)); + } + + if (_param_custom_default_values != nullptr) { + PX4_INFO("storage array (custom defaults): %d/%d elements (%zu bytes total)", + utarray_len(_param_custom_default_values), _param_custom_default_values->n, + _param_custom_default_values->n * sizeof(UT_icd)); + } + + PX4_INFO("auto save: %s", _autosave_disabled ? "off" : "on"); + + if (!_autosave_disabled && (_last_autosave_timestamp > 0)) { + PX4_INFO("last auto save: %.3f seconds ago", hrt_elapsed_time(&_last_autosave_timestamp) * 1e-6); + } + + perf_print_counter(_export_perf); + perf_print_counter(_find_count_perf); + perf_print_counter(_get_count_perf); + perf_print_counter(_set_perf); +} + +void ParameterServer::controlAutosave(bool enable) +{ + lockWriter(); + + if (!enable && _autosave_scheduled.load()) { + _autosave_scheduled.store(false); + } + + _autosave_disabled = !enable; + unlockWriter(); +} + +void ParameterServer::autoSave(bool now) +{ + if (now) { + _autosave_scheduled.store(true); + ScheduleNow(); + return; + } + + if (_autosave_scheduled.load() || _autosave_disabled) { + return; + } + + // wait at least 300ms before saving, because: + // - tasks often call param_set() for multiple params, so this avoids unnecessary save calls + // - the logger stores changed params. He gets notified on a param change via uORB and then + // looks at all unsaved params. + hrt_abstime delay = 300_ms; + + static constexpr const hrt_abstime rate_limit = 2_s; // rate-limit saving to 2 seconds + const hrt_abstime last_save_elapsed = hrt_elapsed_time(&_last_autosave_timestamp); + + if (last_save_elapsed < rate_limit && rate_limit > last_save_elapsed + delay) { + delay = rate_limit - last_save_elapsed; + } + + _autosave_scheduled.store(true); + + ScheduleDelayed(delay); +} + +int ParameterServer::exportInternal(int fd, param_filter_func filter) +{ + PX4_DEBUG("param_export_internal"); + + int result = -1; + param_wbuf_s *s = nullptr; + bson_encoder_s encoder{}; + uint8_t bson_buffer[256]; + + if (lseek(fd, 0, SEEK_SET) != 0) { + PX4_ERR("export seek failed %d", errno); + return -1; + } + + if (bson_encoder_init_buf_file(&encoder, fd, &bson_buffer, sizeof(bson_buffer)) != 0) { + goto out; + } + + // no modified parameters, export empty BSON document + if (_param_values == nullptr) { + result = 0; + goto out; + } + + while ((s = (struct param_wbuf_s *)utarray_next(_param_values, s)) != nullptr) { + if (filter && !filter(s->param)) { + continue; + } + + // don't export default values + switch (getParameterType(s->param)) { + case PARAM_TYPE_INT32: { + int32_t default_value = 0; + getParameterDefaultValueInternal(s->param, &default_value); + + if (s->val.i == default_value) { + PX4_DEBUG("skipping %s %" PRIi32 " export", getParameterName(s->param), default_value); + continue; + } + } + break; + + case PARAM_TYPE_FLOAT: { + float default_value = 0; + getParameterDefaultValueInternal(s->param, &default_value); + + if (fabsf(s->val.f - default_value) <= FLT_EPSILON) { + PX4_DEBUG("skipping %s %.3f export", getParameterName(s->param), (double)default_value); + continue; + } + } + break; + } + + const char *name = getParameterName(s->param); + const size_t size = getParameterSize(s->param); + + /* append the appropriate BSON type object */ + switch (getParameterType(s->param)) { + case PARAM_TYPE_INT32: { + const int32_t i = s->val.i; + PX4_DEBUG("exporting: %s (%d) size: %lu val: %" PRIi32, name, s->param, (long unsigned int)size, i); + + if (bson_encoder_append_int32(&encoder, name, i) != 0) { + PX4_ERR("BSON append failed for '%s'", name); + goto out; + } + } + break; + + case PARAM_TYPE_FLOAT: { + const double f = (double)s->val.f; + PX4_DEBUG("exporting: %s (%d) size: %lu val: %.3f", name, s->param, (long unsigned int)size, (double)f); + + if (bson_encoder_append_double(&encoder, name, f) != 0) { + PX4_ERR("BSON append failed for '%s'", name); + goto out; + } + } + break; + + default: + PX4_ERR("%s unrecognized parameter type %d, skipping export", name, getParameterType(s->param)); + } + } + + result = 0; + +out: + + if (result == 0) { + if (bson_encoder_fini(&encoder) != PX4_OK) { + PX4_ERR("BSON encoder finalize failed"); + result = -1; + } + } + + return result; +} + +void ParameterServer::Run() +{ + // srv: parameter_set + if (_srv_parameter_set_request_sub.updated()) { + const unsigned last_generation = _srv_parameter_set_request_sub.get_last_generation(); + + srv_parameter_set_request_s request; + + if (_srv_parameter_set_request_sub.copy(&request)) { + + if (_srv_parameter_set_request_sub.get_last_generation() != last_generation + 1) { + PX4_ERR("missed srv_parameter_set_request, generation %d -> %d", last_generation, + _srv_parameter_set_request_sub.get_last_generation()); + } + + srv_parameter_set_response_s response{}; + response.timestamp_requested = request.timestamp; + + // defaults + response.result = srv_parameter_set_response_s::RESULT_SET_FAILED; + response.parameter.index = PARAM_INVALID; + response.parameter.type = parameter_s::TYPE_INVALID; + + param_t param = PARAM_INVALID; + + if (handle_in_range(request.parameter.index) && (strlen(request.parameter.name) == 0)) { + param = request.parameter.index; + + } else { + param = findParameter(request.parameter.name); + } + + response.parameter.index = param; + + if (param != PARAM_INVALID) { + memcpy(response.parameter.name, param_name(param), 16); + response.parameter.index_used = getParameterUsedIndex(param); + + switch (getParameterType(param)) { + case PARAM_TYPE_INT32: + response.parameter.type = parameter_s::TYPE_INT32; + + if (setParameter(param, &request.parameter.int32_value) == 0) { + response.parameter.int32_value = request.parameter.int32_value; + response.result = srv_parameter_set_response_s::RESULT_SET_SUCCESS; + } + + break; + + case PARAM_TYPE_FLOAT: + response.parameter.type = parameter_s::TYPE_FLOAT32; + + if (setParameter(param, &request.parameter.float32_value) == 0) { + response.parameter.float32_value = request.parameter.float32_value; + response.result = srv_parameter_set_response_s::RESULT_SET_SUCCESS; + } + + break; + } + + } else { + response.result = srv_parameter_set_response_s::RESULT_ERROR_INVALID_PARAMETER; + } + + response.timestamp = hrt_absolute_time(); + _srv_parameter_set_response_pub.publish(response); + } + } + + // srv: parameter_get + if (_srv_parameter_get_request_sub.updated()) { + const unsigned last_generation = _srv_parameter_get_request_sub.get_last_generation(); + + srv_parameter_get_request_s request; + + if (_srv_parameter_get_request_sub.copy(&request)) { + + if (_srv_parameter_get_request_sub.get_last_generation() != last_generation + 1) { + PX4_ERR("missed srv_parameter_get_request, generation %d -> %d", last_generation, + _srv_parameter_get_request_sub.get_last_generation()); + } + + srv_parameter_get_response_s response{}; + response.timestamp_requested = request.timestamp; + + // defaults + response.result = srv_parameter_get_response_s::RESULT_GET_FAILED; + response.parameter.index = PARAM_INVALID; + response.parameter.index_used = -1; + response.parameter.type = parameter_s::TYPE_INVALID; + + param_t param = PARAM_INVALID; + + if (handle_in_range(request.index) && (strlen(request.name) == 0)) { + param = request.index; + + } else { + param = findParameter(request.name); + } + + response.parameter.index = param; + + if (param != PARAM_INVALID) { + memcpy(response.parameter.name, param_name(param), 16); + response.parameter.index_used = getParameterUsedIndex(param); + + switch (getParameterType(param)) { + case PARAM_TYPE_INT32: + response.parameter.type = parameter_s::TYPE_INT32; + + if (getParameterDefaultValue(param, &response.parameter.int32_value) == 0) { + response.result = srv_parameter_get_response_s::RESULT_GET_SUCCESS; + } + + break; + + case PARAM_TYPE_FLOAT: + response.parameter.type = parameter_s::TYPE_FLOAT32; + + if (getParameterDefaultValue(param, &response.parameter.float32_value) == 0) { + response.result = srv_parameter_get_response_s::RESULT_GET_SUCCESS; + } + + break; + } + + } else { + response.result = srv_parameter_get_response_s::RESULT_ERROR_INVALID_PARAMETER; + } + + response.timestamp = hrt_absolute_time(); + _srv_parameter_get_response_pub.publish(response); + } + } + + // schedule to run again immediately if there are pending updates + if (_srv_parameter_set_request_sub.updated() || _srv_parameter_get_request_sub.updated()) { + ScheduleNow(); + return; + } + + + + // parameter_request: TODO: replaced with srv_parameter_get/set_request + while (_param_request_sub.updated()) { + const unsigned last_generation = _param_request_sub.get_last_generation(); + + parameter_request_s request; + + if (_param_request_sub.copy(&request)) { + + if (_param_request_sub.get_last_generation() != last_generation + 1) { + PX4_ERR("missed parameter_request, generation %d -> %d", last_generation, _param_request_sub.get_last_generation()); + } + + if (request.message_type == parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_READ) { + + + param_t param = PARAM_INVALID; + + if (handle_in_range(request.param_index) && (strlen(request.name) == 0)) { + param = request.param_index; + //fprintf(stderr, "PARAM_REQUEST_READ: id: %d (%s)\n", param, param_name(param)); + + } else { + param = findParameter(request.name); + //fprintf(stderr, "PARAM_REQUEST_READ: name: %s (%d)\n", param_name(param), param); + } + + parameter_value_s parameter_value{}; + parameter_value.param_count = countUsed(); + parameter_value.timestamp_requested = request.timestamp; + + if (param != PARAM_INVALID) { + + parameter_value.param_index = param; + memcpy(parameter_value.param_name, param_name(param), 16); + + switch (getParameterType(param)) { + case PARAM_TYPE_INT32: { + if (getParameterDefaultValue(param, ¶meter_value.int32_value) == 0) { + parameter_value.type = parameter_request_s::TYPE_INT32; + parameter_value.timestamp = hrt_absolute_time(); + _param_response_pub.publish(parameter_value); + } + } + break; + + case PARAM_TYPE_FLOAT: { + if (getParameterDefaultValue(param, ¶meter_value.float32_value) == 0) { + parameter_value.type = parameter_request_s::TYPE_FLOAT32; + parameter_value.timestamp = hrt_absolute_time(); + _param_response_pub.publish(parameter_value); + } + } + break; + } + } + + } else if (request.message_type == parameter_request_s::MESSAGE_TYPE_PARAM_SET) { + + param_t param = findParameter(request.name); + + if (param != PARAM_INVALID) { + switch (request.type) { + case parameter_request_s::TYPE_INT32: + setParameter(param, &request.int32_value); + break; + + case parameter_request_s::TYPE_FLOAT32: + setParameter(param, &request.float32_value); + break; + } + } + + } else if (request.message_type == parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_LIST) { + + // TODO: + + + } else if (request.node_id == parameter_request_s::NODE_ID_ALL) { + if (request.message_type == parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_LIST) { + + // TODO: + + } + } + } + } + + + + // autosave worker, run last + if (_autosave_scheduled.load()) { + + bool disabled = false; + + if (!getDefaultFile()) { + // In case we save to FLASH, defer param writes until disarmed, + // as writing to FLASH can stall the entire CPU (in rare cases around 300ms on STM32F7) + _armed_sub.update(); + + if (_armed_sub.get().armed) { + //work_queue(LPWORK, &autosave_work, (worker_t)&autosave_worker, nullptr, USEC2TICK(1_s)); + return; + } + } + + lockWriter(); + _last_autosave_timestamp = hrt_absolute_time(); + _autosave_scheduled.store(false); + disabled = _autosave_disabled; + unlockWriter(); + + if (disabled) { + return; + } + + PX4_DEBUG("Autosaving params"); + int ret = saveDefault(); + + if (ret != 0) { + PX4_ERR("param auto save failed (%i)", ret); + } + + } + + if (_notify_scheduled.load()) { + lockReader(); + + _notify_scheduled.store(false); + + parameter_update_s pup{}; + pup.instance = _param_instance++; + pup.count = _params_active.count(); + pup.timestamp = hrt_absolute_time(); + _parameter_update_pub.publish(pup); + + unlockReader(); + } +} diff --git a/src/lib/parameters/ParameterServer.hpp b/src/lib/parameters/ParameterServer.hpp new file mode 100644 index 0000000000..2d1d3751e4 --- /dev/null +++ b/src/lib/parameters/ParameterServer.hpp @@ -0,0 +1,516 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "tinybson/tinybson.h" +#include "uthash/utarray.h" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "param.h" +#include + +using namespace time_literals; + +class ParameterServer : public px4::ScheduledWorkItem +{ +public: + ParameterServer(); + ~ParameterServer() override; + + /** + * Look up a parameter by name. + * + * @param name The canonical name of the parameter being looked up. + * @return A handle to the parameter, or PARAM_INVALID if the parameter does not exist. + */ + param_t findParameter(const char *name, bool notification = true); + + /** + * Return the total number of parameters. + * + * @return The number of parameters. + */ + unsigned count() const { return param_info_count; } + + /** + * Return the actually used number of parameters. + * + * @return The number of parameters. + */ + unsigned countUsed() const { return _params_active.count(); } + + /** + * Wether a parameter is in use in the system. + * + * @return True if it has been written or read + */ + bool isParameterUsed(param_t param) const; + + /** + * Look up a parameter by index. + * + * @param index An index from 0 to n, where n is param_count()-1. + * @return A handle to the parameter, or PARAM_INVALID if the index is out of range. + */ + param_t forIndex(unsigned index) const; + + /** + * Look up an used parameter by index. + * + * @param index The parameter to obtain the index for. + * @return The index of the parameter in use, or -1 if the parameter does not exist. + */ + param_t forUsedIndex(unsigned index) const; + + /** + * Look up the index of a parameter. + * + * @param param The parameter to obtain the index for. + * @return The index, or -1 if the parameter does not exist. + */ + int getParameterIndex(param_t param) const; + + /** + * Look up the index of an used parameter. + * + * @param param The parameter to obtain the index for. + * @return The index of the parameter in use, or -1 if the parameter does not exist. + */ + int getParameterUsedIndex(param_t param) const; + + /** + * Obtain the name of a parameter. + * + * @param param A handle returned by param_find or passed by param_foreach. + * @return The name assigned to the parameter, or NULL if the handle is invalid. + */ + const char *getParameterName(param_t param) const; + + /** + * Obtain the volatile state of a parameter. + * + * @param param A handle returned by param_find or passed by param_foreach. + * @return true if the parameter is volatile + */ + bool isParameterVolatile(param_t param) const; + + /** + * Test whether a parameter's value has changed from the default. + * + * @return If true, the parameter's value has not been changed from the default. + */ + bool isParameterValueDefault(param_t param); + + /** + * Test whether a parameter's value has been changed but not saved. + * + * @return If true, the parameter's value has not been saved. + */ + bool isParameterValueUnsaved(param_t param); + + /** + * Obtain the type of a parameter. + * + * @param param A handle returned by param_find or passed by param_foreach. + * @return The type assigned to the parameter. + */ + param_type_t getParameterType(param_t param) const; + + /** + * Determine the size of a parameter. + * + * @param param A handle returned by param_find or passed by param_foreach. + * @return The size of the parameter's value. + */ + size_t getParameterSize(param_t param) const; + + /** + * Copy the value of a parameter. + * + * @param param A handle returned by param_find or passed by param_foreach. + * @param val Where to return the value, assumed to point to suitable storage for the parameter type. + * @return Zero if the parameter's value could be returned, nonzero otherwise. + */ + int getParameterValue(param_t param, void *val); + + /** + * Copy the (airframe-specific) default value of a parameter. + * + * @param param A handle returned by param_find or passed by param_foreach. + * @param default_val Where to return the value, assumed to point to suitable storage for the parameter type. + * @return Zero if the parameter's deafult value could be returned, nonzero otherwise. + */ + int getParameterDefaultValue(param_t param, void *default_val); + + /** + * Copy the system-wide default value of a parameter. + * + * @param param A handle returned by param_find or passed by param_foreach. + * @param default_val Where to return the value, assumed to point to suitable storage for the parameter type. + * @return Zero if the parameter's deafult value could be returned, nonzero otherwise. + */ + int getParameterSystemDefaultValue(param_t param, void *default_val); + + /** + * Set the value of a parameter. + * + * @param param A handle returned by param_find or passed by param_foreach. + * @param val The value to set; assumed to point to a variable of the parameter type. + * @return Zero if the parameter's value could be set from a scalar, nonzero otherwise. + */ + int setParameter(param_t param, const void *val, bool mark_saved = true, bool notify_changes = true); + + /** + * Set the default value of a parameter. + * + * @param param A handle returned by param_find or passed by param_foreach. + * @param val The default value to set; assumed to point to a variable of the parameter type. + * @return Zero if the parameter's default value could be set from a scalar, nonzero otherwise. + */ + int setParameterDefaultValue(param_t param, const void *val); + + /** + * Notify the system about parameter changes. Can be used for example after several calls to + * param_set_no_notification() to avoid unnecessary system notifications. + */ + void notifyChanges(); + + /** + * Reset all parameters to their default values. + */ + void resetAllParameters(bool auto_save = true); + + /** + * Reset all parameters to their default values except for excluded parameters. + * + * @param excludes Array of param names to exclude from resetting. Use a wildcard + * at the end to exclude parameters with a certain prefix. + * @param num_excludes The number of excludes provided. + */ + void resetExcludes(const char *excludes[], int num_excludes); + + /** + * Reset only specific parameters to their default values. + * + * @param resets Array of param names to reset. Use a wildcard at the end to reset parameters with a certain prefix. + * @param num_resets The number of passed reset conditions in the resets array. + */ + void resetSpecificParameter(const char *resets[], int num_resets); + + /** + * Export changed parameters to a file. + * Note: this method requires a large amount of stack size! + * + * @param filename Path to the default parameter file. + * @param filter Filter parameters to be exported. The method should return true if + * the parameter should be exported. No filtering if nullptr is passed. + * @return Zero on success, nonzero on failure. + */ + typedef bool(*param_filter_func)(param_t handle); + int exportToFile(const char *filename, param_filter_func filter); + + /** + * Import parameters from a file, discarding any unrecognized parameters. + * + * This function merges the imported parameters with the current parameter set. + * + * @param fd File descriptor to import from (-1 selects the FLASH storage). + * @return Zero on success, nonzero if an error occurred during import. + * Note that in the failure case, parameters may be inconsistent. + */ + int importFromFileDescriptor(int fd); + + /** + * Load parameters from a file. + * + * This function resets all parameters to their default values, then loads new + * values from a file. + * + * @param fd File descriptor to import from (-1 selects the FLASH storage). + * @return Zero on success, nonzero if an error occurred during import. + * Note that in the failure case, parameters may be inconsistent. + */ + int loadFromFileDescriptor(int fd); + + /** + * Apply a function to each parameter. + * + * Note that the parameter set is not locked during the traversal. It also does + * not hold an internal state, so the callback function can block or sleep between + * parameter callbacks. + * + * @param func The function to invoke for each parameter. + * @param arg Argument passed to the function. + * @param only_changed If true, the function is only called for parameters whose values have + * been changed from the default. + * @param only_used If true, the function is only called for parameters which have been + * used in one of the running applications. + */ + void forEachParameter(void (*func)(void *arg, param_t param), void *arg, bool only_changed, bool only_used); + + /** + * Set the default parameter file name. + * This has no effect if the FLASH-based storage is enabled. + * + * @param filename Path to the default parameter file. The file is not required to + * exist. + * @return Zero on success. + */ + int setDefaultFile(const char *filename); + + /** + * Get the default parameter file name. + * + * @return The path to the current default parameter file; either as + * a result of a call to param_set_default_file, or the + * built-in default. + */ + const char *getDefaultFile() const { return _param_default_file; } + + /** + * Set the backup parameter file name. + * + * @param filename Path to the backup parameter file. The file is not required to + * exist. + * @return Zero on success. + */ + int setBackupFile(const char *filename); + + /** + * Get the backup parameter file name. + * + * @return The path to the backup parameter file + */ + const char *getBackupFile() const { return _param_backup_file; } + + /** + * Automatically save the parameters after a timeout and limited rate. + * + * This needs to be called with the writer lock held (it's not necessary that it's the writer lock, but it + * needs to be the same lock as autosave_worker() and param_control_autosave() use). + */ + void autoSave(bool now = false); + + /** + * Load parameters from the default parameter file. + * + * @return Zero on success. + */ + int loadDefault(); + + /** + * Generate the hash of all parameters and their values + * + * @return CRC32 hash of all param_ids and values + */ + uint32_t hashCheck(); + + /** + * Print the status of the param system + * + */ + void printStatus(); + + /** + * Enable/disable the param autosaving. + * Re-enabling with changed params will not cause an autosave. + * @param enable true: enable autosaving, false: disable autosaving + */ + void controlAutosave(bool enable); + +private: + + static constexpr uint16_t param_info_count = sizeof(px4::parameters) / sizeof(param_info_s); + + /** + * Test whether a param_t is value. + * + * @param param The parameter handle to test. + * @return True if the handle is valid. + */ + static constexpr bool handle_in_range(param_t param) { return (param < param_info_count); } + + void lockReader(); // lock the parameter store for read access + void unlockReader(); // unlock the parameter store + + void lockWriter(); // lock the parameter store for write access + void unlockWriter(); // unlock the parameter store + + // Storage for modified parameters. + struct param_wbuf_s { + union param_value_u val {}; + param_t param{PARAM_INVALID}; + }; + + /** + * Compare two modified parameter structures to determine ordering. + * + * This function is suitable for passing to qsort or bsearch. + */ + static int compareValues(const void *a, const void *b); + + /** + * Locate the modified parameter structure for a parameter, if it exists. + * + * @param param The parameter being searched. + * @return The structure holding the modified value, or + * nullptr if the parameter has not been modified. + */ + param_wbuf_s *findChanged(param_t param); + + /** + * Reset a parameter to its default value. + * + * @param param A handle returned by param_find or passed by param_foreach. + * @return Zero on success, nonzero on failure + */ + int resetParameter(param_t param); + + /** + * Obtain a pointer to the storage allocated for a parameter. + * + * @param param The parameter whose storage is sought. + * @return A pointer to the parameter value, or nullptr + * if the parameter does not exist. + */ + const void *getParameterValuePointer(param_t param); + + // get parameter default value, caller is responsible for locking + int getParameterDefaultValueInternal(param_t param, void *default_val); + + /** + * Save parameters to the default file. + * Note: this method requires a large amount of stack size! + * + * This function saves all parameters with non-default values. + * + * @return Zero on success. + */ + int saveDefault(); + + // internal parameter export, caller is responsible for locking + int exportInternal(int fd, param_filter_func filter); + + int bsonImportCallback(bson_decoder_t decoder, bson_node_t node); + static int importCallbackTrampoline(bson_decoder_t decoder, void *priv, bson_node_t node); + int importFromFileDescriptorInternal(int fd); + + int verifyBsonExportCallback(bson_decoder_t decoder, bson_node_t node); + static int verifyBsonExportTrampoline(bson_decoder_t decoder, void *priv, bson_node_t node); + int verifyBsonExport(int fd); + + char *_param_default_file{nullptr}; + char *_param_backup_file{nullptr}; + + px4::AtomicBitset _params_active; // params found + px4::AtomicBitset _params_changed; // params non-default + px4::Bitset _params_custom_default; // params with runtime default value + px4::AtomicBitset _params_unsaved; + + /** flexible array holding modified parameter values */ + UT_array *_param_values{nullptr}; + UT_array *_param_custom_default_values{nullptr}; + + const UT_icd param_icd = {sizeof(param_wbuf_s), nullptr, nullptr, nullptr}; + + unsigned int _param_instance{0}; + + // the following implements an RW-lock using 2 semaphores (used as mutexes). It gives + // priority to readers, meaning a writer could suffer from starvation, but in our use-case + // we only have short periods of reads and writes are rare. + px4_sem_t _param_sem; ///< this protects against concurrent access to _param_values + int _reader_lock_holders{0}; + px4_sem_t _reader_lock_holders_lock; ///< this protects against concurrent access to reader_lock_holders + + perf_counter_t _export_perf{perf_alloc(PC_ELAPSED, "param: export")}; + perf_counter_t _find_count_perf{perf_alloc(PC_COUNT, "param: find")}; + perf_counter_t _get_count_perf{perf_alloc(PC_COUNT, "param: get")}; + perf_counter_t _set_perf{perf_alloc(PC_ELAPSED, "param: set")}; + + px4_sem_t _param_sem_save; ///< this protects against concurrent param saves (file or flash access). + ///< we use a separate lock to allow concurrent param reads and saves. + ///< a param_set could still be blocked by a param save, because it + ///< needs to take the reader lock + + + void Run() override; + + uORB::Publication _parameter_update_pub{ORB_ID(parameter_update)}; + + // srv: parameter_get + uORB::SubscriptionCallbackWorkItem _srv_parameter_get_request_sub{this, ORB_ID(srv_parameter_get_request)}; + uORB::Publication _srv_parameter_get_response_pub{ORB_ID(srv_parameter_get_response)}; + + // srv: parameter_set + uORB::SubscriptionCallbackWorkItem _srv_parameter_set_request_sub{this, ORB_ID(srv_parameter_set_request)}; + uORB::Publication _srv_parameter_set_response_pub{ORB_ID(srv_parameter_set_response)}; + + // TODO: replace + uORB::Publication _param_response_pub{ORB_ID(parameter_value)}; + uORB::SubscriptionCallbackWorkItem _param_request_sub{this, ORB_ID(parameter_request)}; + + uORB::SubscriptionData _armed_sub{ORB_ID(actuator_armed)}; + + + // autosaving variables + hrt_abstime _last_autosave_timestamp{0}; + px4::atomic_bool _autosave_scheduled{false}; + bool _autosave_disabled{false}; + + px4::atomic_bool _notify_scheduled{false}; + +}; diff --git a/src/lib/parameters/flashparams/flashparams.cpp b/src/lib/parameters/flashparams/flashparams.cpp index b309207850..8de65462a1 100644 --- a/src/lib/parameters/flashparams/flashparams.cpp +++ b/src/lib/parameters/flashparams/flashparams.cpp @@ -87,12 +87,12 @@ param_export_internal(param_filter_func filter) bson_encoder_init_buf(&encoder, nullptr, 0); /* no modified parameters -> we are done */ - if (param_values == nullptr) { + if (_param_values == nullptr) { result = 0; goto out; } - while ((s = (struct param_wbuf_s *)utarray_next(param_values, s)) != nullptr) { + while ((s = (struct param_wbuf_s *)utarray_next(_param_values, s)) != nullptr) { int32_t i; float f; @@ -103,12 +103,12 @@ param_export_internal(param_filter_func filter) /* append the appropriate BSON type object */ - switch (param_type(s->param)) { + switch (getParameterType(s->param)) { case PARAM_TYPE_INT32: i = s->val.i; - if (bson_encoder_append_int32(&encoder, param_name(s->param), i)) { - debug("BSON append failed for '%s'", param_name(s->param)); + if (bson_encoder_append_int32(&encoder, getParameterName(s->param), i)) { + debug("BSON append failed for '%s'", getParameterName(s->param)); goto out; } @@ -117,8 +117,8 @@ param_export_internal(param_filter_func filter) case PARAM_TYPE_FLOAT: f = s->val.f; - if (bson_encoder_append_double(&encoder, param_name(s->param), f)) { - debug("BSON append failed for '%s'", param_name(s->param)); + if (bson_encoder_append_double(&encoder, getParameterName(s->param), f)) { + debug("BSON append failed for '%s'", getParameterName(s->param)); goto out; } @@ -214,7 +214,7 @@ param_import_callback(bson_decoder_t decoder, bson_node_t node) switch (node->type) { case BSON_INT32: - if (param_type(param) != PARAM_TYPE_INT32) { + if (getParameterType(param) != PARAM_TYPE_INT32) { PX4_WARN("unexpected type for %s", node->name); result = 1; // just skip this entry goto out; @@ -225,7 +225,7 @@ param_import_callback(bson_decoder_t decoder, bson_node_t node) break; case BSON_DOUBLE: - if (param_type(param) != PARAM_TYPE_FLOAT) { + if (getParameterType(param) != PARAM_TYPE_FLOAT) { PX4_WARN("unexpected type for %s", node->name); result = 1; // just skip this entry goto out; diff --git a/src/lib/parameters/param.h b/src/lib/parameters/param.h index dbe34bb460..e601e3dfee 100644 --- a/src/lib/parameters/param.h +++ b/src/lib/parameters/param.h @@ -197,14 +197,6 @@ __EXPORT bool param_value_unsaved(param_t param); */ __EXPORT param_type_t param_type(param_t param); -/** - * Determine the size of a parameter. - * - * @param param A handle returned by param_find or passed by param_foreach. - * @return The size of the parameter's value. - */ -__EXPORT size_t param_size(param_t param); - /** * Copy the value of a parameter. * @@ -250,14 +242,6 @@ __EXPORT int param_set(param_t param, const void *val); */ __EXPORT int param_set_default_value(param_t param, const void *val); -/** - * Mark a parameter as used. Only marked parameters will be sent to a GCS. - * A call to param_find() will mark a param as used as well. - * - * @param param A handle returned by param_find or passed by param_foreach. - */ -__EXPORT void param_set_used(param_t param); - /** * Set the value of a parameter, but do not notify the system about the change. * @@ -287,8 +271,6 @@ __EXPORT void param_reset_all(void); */ __EXPORT void param_reset_excludes(const char *excludes[], int num_excludes); -typedef bool(*param_filter_func)(param_t handle); - /** * Reset only specific parameters to their default values. * @@ -306,6 +288,7 @@ __EXPORT void param_reset_specific(const char *resets[], int num_resets); * the parameter should be exported. No filtering if nullptr is passed. * @return Zero on success, nonzero on failure. */ +typedef bool(*param_filter_func)(param_t handle); __EXPORT int param_export(const char *filename, param_filter_func filter); /** diff --git a/src/lib/parameters/parameters.cpp b/src/lib/parameters/parameters.cpp index e09f473739..cc416ce6e7 100644 --- a/src/lib/parameters/parameters.cpp +++ b/src/lib/parameters/parameters.cpp @@ -42,1638 +42,342 @@ */ #define PARAM_IMPLEMENTATION + +#include "ParameterServer.hpp" +static ParameterServer *parameter_server {nullptr}; + #include "param.h" -#include "param_translation.h" -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "uthash/utarray.h" - -using namespace time_literals; - -#include "uORB/uORB.h" -#include "uORB/topics/parameter_update.h" -#include -#include - -/* Include functions common to user and kernel sides */ -#include "parameters_common.cpp" - -#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) -#include -#include "parameters_ioctl.h" -#endif - -#if defined(FLASH_BASED_PARAMS) -#include "flashparams/flashparams.h" -#else -inline static int flash_param_save(param_filter_func filter) { return -1; } -inline static int flash_param_load() { return -1; } -inline static int flash_param_import() { return -1; } -#endif - -static char *param_default_file = nullptr; -static char *param_backup_file = nullptr; - -#include -/* autosaving variables */ -static hrt_abstime last_autosave_timestamp = 0; -static struct work_s autosave_work {}; -static px4::atomic_bool autosave_scheduled{false}; -static bool autosave_disabled = false; - -static px4::AtomicBitset params_active; // params found -static px4::AtomicBitset params_changed; // params non-default -static px4::Bitset params_custom_default; // params with runtime default value -static px4::AtomicBitset params_unsaved; - -// Storage for modified parameters. -struct param_wbuf_s { - union param_value_u val; - param_t param; -}; - -/** flexible array holding modified parameter values */ -UT_array *param_values{nullptr}; -UT_array *param_custom_default_values{nullptr}; - -const UT_icd param_icd = {sizeof(param_wbuf_s), nullptr, nullptr, nullptr}; - -/** parameter update topic handle */ -static orb_advert_t param_topic = nullptr; -static unsigned int param_instance = 0; - -// the following implements an RW-lock using 2 semaphores (used as mutexes). It gives -// priority to readers, meaning a writer could suffer from starvation, but in our use-case -// we only have short periods of reads and writes are rare. -static px4_sem_t param_sem; ///< this protects against concurrent access to param_values -static int reader_lock_holders = 0; -static px4_sem_t reader_lock_holders_lock; ///< this protects against concurrent access to reader_lock_holders - -static perf_counter_t param_export_perf; -static perf_counter_t param_find_perf; -static perf_counter_t param_get_perf; -static perf_counter_t param_set_perf; - -static px4_sem_t param_sem_save; ///< this protects against concurrent param saves (file or flash access). -///< we use a separate lock to allow concurrent param reads and saves. -///< a param_set could still be blocked by a param save, because it -///< needs to take the reader lock - -/** lock the parameter store for read access */ -static void -param_lock_reader() +void param_init() { - do {} while (px4_sem_wait(&reader_lock_holders_lock) != 0); + if (parameter_server == nullptr) { + px4_usleep(100'000); + parameter_server = new ParameterServer(); + } +} - ++reader_lock_holders; +void param_notify_changes() +{ + if (parameter_server) { + parameter_server->notifyChanges(); + } +} - if (reader_lock_holders == 1) { - // the first reader takes the lock, the next ones are allowed to just continue - do {} while (px4_sem_wait(¶m_sem) != 0); +#if !defined(PX4_PARAM_CLIENT_SRV) +param_t param_find(const char *name) +{ + if (parameter_server) { + return parameter_server->findParameter(name, true); } - px4_sem_post(&reader_lock_holders_lock); + return PARAM_INVALID; } -/** lock the parameter store for write access */ -static void -param_lock_writer() +param_t param_find_no_notification(const char *name) { - do {} while (px4_sem_wait(¶m_sem) != 0); -} - -/** unlock the parameter store */ -static void -param_unlock_reader() -{ - do {} while (px4_sem_wait(&reader_lock_holders_lock) != 0); - - --reader_lock_holders; - - if (reader_lock_holders == 0) { - // the last reader releases the lock - px4_sem_post(¶m_sem); + if (parameter_server) { + return parameter_server->findParameter(name, false); } - px4_sem_post(&reader_lock_holders_lock); + return PARAM_INVALID; } +#endif // !PX4_PARAM_CLIENT_SRV -/** unlock the parameter store */ -static void -param_unlock_writer() +unsigned param_count() { - px4_sem_post(¶m_sem); -} - -/** assert that the parameter store is locked */ -static void -param_assert_locked() -{ - /* XXX */ -} - -void -param_init() -{ - px4_sem_init(¶m_sem, 0, 1); - px4_sem_init(¶m_sem_save, 0, 1); - px4_sem_init(&reader_lock_holders_lock, 0, 1); - - param_export_perf = perf_alloc(PC_ELAPSED, "param: export"); - param_find_perf = perf_alloc(PC_COUNT, "param: find"); - param_get_perf = perf_alloc(PC_COUNT, "param: get"); - param_set_perf = perf_alloc(PC_ELAPSED, "param: set"); - -#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) - px4_register_boardct_ioctl(_PARAMIOCBASE, param_ioctl); -#endif -} - -/** - * Compare two modified parameter structures to determine ordering. - * - * This function is suitable for passing to qsort or bsearch. - */ -static int -param_compare_values(const void *a, const void *b) -{ - struct param_wbuf_s *pa = (struct param_wbuf_s *)a; - struct param_wbuf_s *pb = (struct param_wbuf_s *)b; - - if (pa->param < pb->param) { - return -1; - } - - if (pa->param > pb->param) { - return 1; + if (parameter_server) { + return parameter_server->count(); } return 0; } -/** - * Locate the modified parameter structure for a parameter, if it exists. - * - * @param param The parameter being searched. - * @return The structure holding the modified value, or - * nullptr if the parameter has not been modified. - */ -static param_wbuf_s * -param_find_changed(param_t param) -{ - param_assert_locked(); - - if (params_changed[param] && (param_values != nullptr)) { - param_wbuf_s key{}; - key.param = param; - return (param_wbuf_s *)utarray_find(param_values, &key, param_compare_values); - } - - return nullptr; -} - -void -param_notify_changes() -{ - parameter_update_s pup{}; - pup.instance = param_instance++; - pup.get_count = perf_event_count(param_get_perf); - pup.set_count = perf_event_count(param_set_perf); - pup.find_count = perf_event_count(param_find_perf); - pup.export_count = perf_event_count(param_export_perf); - pup.active = params_active.count(); - pup.changed = params_changed.count(); - pup.custom_default = params_custom_default.count(); - pup.timestamp = hrt_absolute_time(); - - if (param_topic == nullptr) { - param_topic = orb_advertise(ORB_ID(parameter_update), &pup); - - } else { - orb_publish(ORB_ID(parameter_update), param_topic, &pup); - } -} - -static param_t param_find_internal(const char *name, bool notification) -{ - perf_count(param_find_perf); - - param_t middle; - param_t front = 0; - param_t last = param_info_count; - - /* perform a binary search of the known parameters */ - - while (front <= last) { - middle = front + (last - front) / 2; - int ret = strcmp(name, param_name(middle)); - - if (ret == 0) { - if (notification) { - param_set_used(middle); - } - - return middle; - - } else if (middle == front) { - /* An end point has been hit, but there has been no match */ - break; - - } else if (ret < 0) { - last = middle; - - } else { - front = middle; - } - } - - /* not found */ - return PARAM_INVALID; -} - -param_t param_find(const char *name) -{ - return param_find_internal(name, true); -} - -param_t param_find_no_notification(const char *name) -{ - return param_find_internal(name, false); -} - unsigned param_count_used() { - return params_active.count(); + if (parameter_server) { + return parameter_server->countUsed(); + } + + return 0; +} + +param_t param_for_index(unsigned index) +{ + if (parameter_server) { + return parameter_server->forIndex(index); + } + + return PARAM_INVALID; } param_t param_for_used_index(unsigned index) { - // walk all params and count used params - if (index < param_info_count) { - unsigned used_count = 0; - - for (int i = 0; i < params_active.size(); i++) { - if (params_active[i]) { - // we found the right used count, - // return the param value - if (index == used_count) { - return static_cast(i); - } - - used_count++; - } - } + if (parameter_server) { + return parameter_server->forUsedIndex(index); } return PARAM_INVALID; } -int param_get_used_index(param_t param) +int param_get_index(param_t param) { - /* this tests for out of bounds and does a constant time lookup */ - if (!param_used(param)) { - return -1; - } - - /* walk all params and count, now knowing that it has a valid index */ - int used_count = 0; - - for (int i = 0; i < params_active.size(); i++) { - if (params_active[i]) { - - if (param == i) { - return used_count; - } - - used_count++; - } + if (parameter_server) { + return parameter_server->getParameterIndex(param); } return -1; } -bool -param_value_unsaved(param_t param) +int param_get_used_index(param_t param) { - return handle_in_range(param) ? params_unsaved[param] : false; + if (parameter_server) { + return parameter_server->getParameterUsedIndex(param); + } + + return -1; } -/** - * Obtain a pointer to the storage allocated for a parameter. - * - * @param param The parameter whose storage is sought. - * @return A pointer to the parameter value, or nullptr - * if the parameter does not exist. - */ -static const void * -param_get_value_ptr(param_t param) +const char *param_name(param_t param) { - param_assert_locked(); - - if (handle_in_range(param)) { - /* work out whether we're fetching the default or a written value */ - struct param_wbuf_s *s = param_find_changed(param); - - if (s != nullptr) { - return &s->val; - - } else { - if (params_custom_default[param] && param_custom_default_values) { - // get default from custom default storage - param_wbuf_s key{}; - key.param = param; - param_wbuf_s *pbuf = (param_wbuf_s *)utarray_find(param_custom_default_values, &key, param_compare_values); - - if (pbuf != nullptr) { - return &pbuf->val; - } - } - - // otherwise return static default value - switch (param_type(param)) { - case PARAM_TYPE_INT32: - return &px4::parameters[param].val.i; - - case PARAM_TYPE_FLOAT: - return &px4::parameters[param].val.f; - } - } + if (parameter_server) { + return parameter_server->getParameterName(param); } return nullptr; } -int -param_get(param_t param, void *val) +param_type_t param_type(param_t param) { - perf_count(param_get_perf); - - if (!handle_in_range(param)) { - PX4_ERR("get: param %" PRId16 " invalid", param); - return PX4_ERROR; + if (parameter_server) { + return parameter_server->getParameterType(param); } - if (!params_active[param]) { - PX4_DEBUG("get: param %" PRId16 " (%s) not active", param, param_name(param)); - } - - int result = PX4_ERROR; - - if (val) { - if (!params_changed[param] && !params_custom_default[param]) { - // if parameter is unchanged (static default value) copy immediately and avoid locking - switch (param_type(param)) { - case PARAM_TYPE_INT32: - memcpy(val, &px4::parameters[param].val.i, sizeof(px4::parameters[param].val.i)); - return PX4_OK; - - case PARAM_TYPE_FLOAT: - memcpy(val, &px4::parameters[param].val.f, sizeof(px4::parameters[param].val.f)); - return PX4_OK; - } - } - - param_lock_reader(); - const void *v = param_get_value_ptr(param); - - if (v) { - memcpy(val, v, param_size(param)); - result = PX4_OK; - } - - param_unlock_reader(); - } - - return result; + return PARAM_TYPE_UNKNOWN; } -int -param_get_default_value_internal(param_t param, void *default_val) +bool param_is_volatile(param_t param) { - if (!handle_in_range(param)) { - PX4_ERR("get default value: param %d invalid", param); - return PX4_ERROR; - } - - if (default_val) { - if (params_custom_default[param] && param_custom_default_values) { - // get default from custom default storage - param_wbuf_s key{}; - key.param = param; - param_wbuf_s *pbuf = (param_wbuf_s *)utarray_find(param_custom_default_values, &key, param_compare_values); - - if (pbuf != nullptr) { - memcpy(default_val, &pbuf->val, param_size(param)); - return PX4_OK; - } - } - - // otherwise return static default value - switch (param_type(param)) { - case PARAM_TYPE_INT32: - memcpy(default_val, &px4::parameters[param].val.i, param_size(param)); - return PX4_OK; - - case PARAM_TYPE_FLOAT: - memcpy(default_val, &px4::parameters[param].val.f, param_size(param)); - return PX4_OK; - } - } - - return PX4_ERROR; -} - -int -param_get_default_value(param_t param, void *default_val) -{ - if (!handle_in_range(param)) { - return PX4_ERROR; - } - - int ret = 0; - - if (!params_custom_default[param]) { - // return static default value - switch (param_type(param)) { - case PARAM_TYPE_INT32: - memcpy(default_val, &px4::parameters[param].val.i, sizeof(px4::parameters[param].val.i)); - return PX4_OK; - - case PARAM_TYPE_FLOAT: - memcpy(default_val, &px4::parameters[param].val.f, sizeof(px4::parameters[param].val.f)); - return PX4_OK; - } - - } else { - param_lock_reader(); - ret = param_get_default_value_internal(param, default_val); - param_unlock_reader(); - } - - return ret; -} - -bool param_value_is_default(param_t param) -{ - if (!handle_in_range(param)) { - return true; - } - - if (!params_changed[param] && !params_custom_default[param]) { - // no value saved and no custom default - return true; - - } else { - // the param_values dynamic array might carry things that have been set - // back to default, so we don't rely on the params_changed bitset here - switch (param_type(param)) { - case PARAM_TYPE_INT32: { - param_lock_reader(); - int32_t default_value = 0; - - if (param_get_default_value_internal(param, &default_value) == PX4_OK) { - const void *v = param_get_value_ptr(param); - - if (v) { - bool unchanged = (*static_cast(v) == default_value); - param_unlock_reader(); - return unchanged; - } - } - - param_unlock_reader(); - } - break; - - case PARAM_TYPE_FLOAT: { - param_lock_reader(); - float default_value = 0; - - if (param_get_default_value_internal(param, &default_value) == PX4_OK) { - const void *v = param_get_value_ptr(param); - - if (v) { - bool unchanged = (fabsf(*static_cast(v) - default_value) <= FLT_EPSILON); - param_unlock_reader(); - return unchanged; - } - } - - param_unlock_reader(); - } - break; - } - } - - return true; -} - -/** - * worker callback method to save the parameters - * @param arg unused - */ -static void -autosave_worker(void *arg) -{ - bool disabled = false; - - if (!param_get_default_file()) { - // In case we save to FLASH, defer param writes until disarmed, - // as writing to FLASH can stall the entire CPU (in rare cases around 300ms on STM32F7) - uORB::SubscriptionData armed_sub{ORB_ID(actuator_armed)}; - - if (armed_sub.get().armed) { - work_queue(LPWORK, &autosave_work, (worker_t)&autosave_worker, nullptr, USEC2TICK(1_s)); - return; - } - } - - param_lock_writer(); - last_autosave_timestamp = hrt_absolute_time(); - autosave_scheduled.store(false); - disabled = autosave_disabled; - param_unlock_writer(); - - if (disabled) { - return; - } - - PX4_DEBUG("Autosaving params"); - int ret = param_save_default(); - - if (ret != 0) { - PX4_ERR("param auto save failed (%i)", ret); - } -} - -/** - * Automatically save the parameters after a timeout and limited rate. - * - * This needs to be called with the writer lock held (it's not necessary that it's the writer lock, but it - * needs to be the same lock as autosave_worker() and param_control_autosave() use). - */ -static void -param_autosave() -{ - if (autosave_scheduled.load() || autosave_disabled) { - return; - } - - // wait at least 300ms before saving, because: - // - tasks often call param_set() for multiple params, so this avoids unnecessary save calls - // - the logger stores changed params. He gets notified on a param change via uORB and then - // looks at all unsaved params. - hrt_abstime delay = 300_ms; - - static constexpr const hrt_abstime rate_limit = 2_s; // rate-limit saving to 2 seconds - const hrt_abstime last_save_elapsed = hrt_elapsed_time(&last_autosave_timestamp); - - if (last_save_elapsed < rate_limit && rate_limit > last_save_elapsed + delay) { - delay = rate_limit - last_save_elapsed; - } - - autosave_scheduled.store(true); - work_queue(LPWORK, &autosave_work, (worker_t)&autosave_worker, nullptr, USEC2TICK(delay)); -} - -void -param_control_autosave(bool enable) -{ - param_lock_writer(); - - if (!enable && autosave_scheduled.load()) { - work_cancel(LPWORK, &autosave_work); - autosave_scheduled.store(false); - } - - autosave_disabled = !enable; - param_unlock_writer(); -} - -static int -param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_changes) -{ - if (!handle_in_range(param)) { - PX4_ERR("set invalid param %d", param); - return PX4_ERROR; - } - - if (val == nullptr) { - PX4_ERR("set invalid value"); - return PX4_ERROR; - } - - int result = -1; - bool param_changed = false; - - param_lock_writer(); - perf_begin(param_set_perf); - - // create the parameter store if it doesn't exist - if (param_values == nullptr) { - utarray_new(param_values, ¶m_icd); - - // mark all parameters unchanged (default) - params_changed.reset(); - params_unsaved.reset(); - } - - if (param_values == nullptr) { - PX4_ERR("failed to allocate modified values array"); - goto out; - - } else { - param_wbuf_s *s = param_find_changed(param); - - if (s == nullptr) { - /* construct a new parameter */ - param_wbuf_s buf{}; - buf.param = param; - - param_changed = true; - - /* add it to the array and sort */ - utarray_push_back(param_values, &buf); - utarray_sort(param_values, param_compare_values); - params_changed.set(param, true); - - /* find it after sorting */ - s = param_find_changed(param); - } - - if (s == nullptr) { - PX4_ERR("error param_values storage slot invalid"); - - } else { - /* update the changed value */ - switch (param_type(param)) { - case PARAM_TYPE_INT32: - if (s->val.i != *(int32_t *)val) { - s->val.i = *(int32_t *)val; - param_changed = true; - } - - params_changed.set(param, true); - params_unsaved.set(param, !mark_saved); - result = PX4_OK; - break; - - case PARAM_TYPE_FLOAT: - if (fabsf(s->val.f - * (float *)val) > FLT_EPSILON) { - s->val.f = *(float *)val; - param_changed = true; - } - - params_changed.set(param, true); - params_unsaved.set(param, !mark_saved); - result = PX4_OK; - break; - - default: - PX4_ERR("param_set invalid param type for %s", param_name(param)); - break; - } - } - - if ((result == PX4_OK) && param_changed && !mark_saved) { // this is false when importing parameters - param_autosave(); - } - } - -out: - perf_end(param_set_perf); - param_unlock_writer(); - - /* - * If we set something, now that we have unlocked, go ahead and advertise that - * a thing has been set. - */ - if ((result == PX4_OK) && param_changed && notify_changes) { - param_notify_changes(); - } - - return result; -} - -#if defined(FLASH_BASED_PARAMS) -int param_set_external(param_t param, const void *val, bool mark_saved, bool notify_changes) -{ - return param_set_internal(param, val, mark_saved, notify_changes); -} - -const void *param_get_value_ptr_external(param_t param) -{ - return param_get_value_ptr(param); -} -#endif - -int param_set(param_t param, const void *val) -{ - return param_set_internal(param, val, false, true); -} - -int param_set_no_notification(param_t param, const void *val) -{ - return param_set_internal(param, val, false, false); -} - -bool param_used(param_t param) -{ - if (handle_in_range(param)) { - return params_active[param]; + if (parameter_server) { + return parameter_server->isParameterVolatile(param); } return false; } -void param_set_used(param_t param) +bool param_value_unsaved(param_t param) { - if (handle_in_range(param)) { - params_active.set(param, true); + if (parameter_server) { + return parameter_server->isParameterValueUnsaved(param); } + + return true; +} + +#if !defined(PX4_PARAM_CLIENT_SRV) +int param_get(param_t param, void *val) +{ + if (parameter_server) { + return parameter_server->getParameterValue(param, val); + } + + return -1; +} +#endif // !PX4_PARAM_CLIENT_SRV + +int param_get_default_value(param_t param, void *default_val) +{ + if (parameter_server) { + parameter_server->getParameterDefaultValue(param, default_val); + } + + return -1; +} + +int param_get_system_default_value(param_t param, void *default_val) +{ + if (parameter_server) { + parameter_server->getParameterSystemDefaultValue(param, default_val); + } + + return -1; +} + +bool param_value_is_default(param_t param) +{ + if (parameter_server) { + parameter_server->isParameterValueDefault(param); + } + + return true; +} + +void param_control_autosave(bool enable) +{ + if (parameter_server) { + parameter_server->controlAutosave(enable); + } +} + +#if !defined(PX4_PARAM_CLIENT_SRV) +int param_set(param_t param, const void *val) +{ + if (parameter_server) { + return parameter_server->setParameter(param, val, false, true); + } + + return -1; +} + +int param_set_no_notification(param_t param, const void *val) +{ + if (parameter_server) { + return parameter_server->setParameter(param, val, false, false); + } + + return -1; +} +#endif // !PX4_PARAM_CLIENT_SRV + +bool param_used(param_t param) +{ + if (parameter_server) { + return parameter_server->isParameterUsed(param); + } + + return false; } int param_set_default_value(param_t param, const void *val) { - if (!handle_in_range(param)) { - PX4_ERR("set default value invalid param %d", param); - return PX4_ERROR; + if (parameter_server) { + return parameter_server->setParameterDefaultValue(param, val); } - if (val == nullptr) { - PX4_ERR("set default value invalid value"); - return PX4_ERROR; - } - - int result = PX4_ERROR; - - param_lock_writer(); - - if (param_custom_default_values == nullptr) { - utarray_new(param_custom_default_values, ¶m_icd); - - // mark all parameters unchanged (default) - params_custom_default.reset(); - - if (param_custom_default_values == nullptr) { - PX4_ERR("failed to allocate custom default values array"); - param_unlock_writer(); - return PX4_ERROR; - } - } - - // check if param being set to default value - bool setting_to_static_default = false; - - switch (param_type(param)) { - case PARAM_TYPE_INT32: - setting_to_static_default = (px4::parameters[param].val.i == *(int32_t *)val); - break; - - case PARAM_TYPE_FLOAT: - setting_to_static_default = (fabsf(px4::parameters[param].val.f - * (float *)val) <= FLT_EPSILON); - break; - } - - // find if custom default value is already set - param_wbuf_s *s = nullptr; - - { - param_wbuf_s key{}; - key.param = param; - s = (param_wbuf_s *)utarray_find(param_custom_default_values, &key, param_compare_values); - } - - if (setting_to_static_default) { - if (s != nullptr) { - // param in memory and set to non-default value, clear - int pos = utarray_eltidx(param_custom_default_values, s); - utarray_erase(param_custom_default_values, pos, 1); - } - - // do nothing if param not already set and being set to default - params_custom_default.set(param, false); - result = PX4_OK; - - } else { - if (s == nullptr) { - // construct a new parameter default value - param_wbuf_s buf{}; - buf.param = param; - - // add it to the array and sort - utarray_push_back(param_custom_default_values, &buf); - utarray_sort(param_custom_default_values, param_compare_values); - - // find it after sorting - s = (param_wbuf_s *)utarray_find(param_custom_default_values, &buf, param_compare_values); - } - - if (s != nullptr) { - // update the default value - switch (param_type(param)) { - case PARAM_TYPE_INT32: - s->val.i = *(int32_t *)val; - params_custom_default.set(param, true); - result = PX4_OK; - break; - - case PARAM_TYPE_FLOAT: - s->val.f = *(float *)val; - params_custom_default.set(param, true); - result = PX4_OK; - break; - - default: - break; - } - } - } - - param_unlock_writer(); - - if ((result == PX4_OK) && param_used(param)) { - // send notification if param is already in use - param_notify_changes(); - } - - return result; + return -1; } -static int param_reset_internal(param_t param) +void param_reset_all() { - param_wbuf_s *s = nullptr; - bool param_found = false; - - param_lock_writer(); - - if (handle_in_range(param)) { - /* look for a saved value */ - s = param_find_changed(param); - - /* if we found one, erase it */ - if (s != nullptr) { - int pos = utarray_eltidx(param_values, s); - utarray_erase(param_values, pos, 1); - } - - params_changed.set(param, false); - params_unsaved.set(param, true); - - param_found = true; - } - - param_autosave(); - - param_unlock_writer(); - - if (s != nullptr) { - param_notify_changes(); - } - - return (!param_found); -} - -static void -param_reset_all_internal(bool auto_save) -{ - param_lock_writer(); - - if (param_values != nullptr) { - utarray_free(param_values); - - params_changed.reset(); - } - - /* mark as reset / deleted */ - param_values = nullptr; - - if (auto_save) { - param_autosave(); - } - - param_unlock_writer(); - - param_notify_changes(); -} - -void -param_reset_all() -{ - param_reset_all_internal(true); -} - -void -param_reset_excludes(const char *excludes[], int num_excludes) -{ - for (param_t param = 0; handle_in_range(param); param++) { - const char *name = param_name(param); - bool exclude = false; - - for (int index = 0; index < num_excludes; index ++) { - int len = strlen(excludes[index]); - - if ((excludes[index][len - 1] == '*' - && strncmp(name, excludes[index], len - 1) == 0) - || strcmp(name, excludes[index]) == 0) { - - exclude = true; - break; - } - } - - if (!exclude) { - param_reset_internal(param); - } + if (parameter_server) { + return parameter_server->resetAllParameters(true); } } -void -param_reset_specific(const char *resets[], int num_resets) +void param_reset_excludes(const char *excludes[], int num_excludes) { - for (param_t param = 0; handle_in_range(param); param++) { - const char *name = param_name(param); - bool reset = false; - - for (int index = 0; index < num_resets; index++) { - int len = strlen(resets[index]); - - if ((resets[index][len - 1] == '*' - && strncmp(name, resets[index], len - 1) == 0) - || strcmp(name, resets[index]) == 0) { - - reset = true; - break; - } - } - - if (reset) { - param_reset_internal(param); - } + if (parameter_server) { + return parameter_server->resetExcludes(excludes, num_excludes); } } -int -param_set_default_file(const char *filename) +void param_reset_specific(const char *resets[], int num_resets) { - if ((param_backup_file && strcmp(filename, param_backup_file) == 0)) { - PX4_ERR("default file can't be the same as the backup file %s", filename); - return PX4_ERROR; + if (parameter_server) { + return parameter_server->resetSpecificParameter(resets, num_resets); + } +} + +int param_set_default_file(const char *filename) +{ + if (parameter_server) { + return parameter_server->setDefaultFile(filename); } -#ifdef FLASH_BASED_PARAMS - // the default for flash-based params is always the FLASH - (void)filename; -#else - - if (param_default_file != nullptr) { - // we assume this is not in use by some other thread - free(param_default_file); - param_default_file = nullptr; - } - - if (filename) { - param_default_file = strdup(filename); - } - -#endif /* FLASH_BASED_PARAMS */ - - return 0; + return -1; } const char *param_get_default_file() { - return param_default_file; + if (parameter_server) { + return parameter_server->getDefaultFile(); + } + + return nullptr; } int param_set_backup_file(const char *filename) { - if (param_default_file && strcmp(filename, param_default_file) == 0) { - PX4_ERR("backup file can't be the same as the default file %s", filename); - return PX4_ERROR; + if (parameter_server) { + return parameter_server->setBackupFile(filename); } - if (param_backup_file != nullptr) { - // we assume this is not in use by some other thread - free(param_backup_file); - param_backup_file = nullptr; - } - - if (filename) { - param_backup_file = strdup(filename); - - } else { - param_backup_file = nullptr; // backup disabled - } - - return 0; + return -1; } const char *param_get_backup_file() { - return param_backup_file; -} + if (parameter_server) { + return parameter_server->getBackupFile(); + } -static int param_export_internal(int fd, param_filter_func filter); -static int param_verify(int fd); + return nullptr; +} int param_save_default() { - PX4_DEBUG("param_save_default"); - int shutdown_lock_ret = px4_shutdown_lock(); - - if (shutdown_lock_ret != 0) { - PX4_ERR("px4_shutdown_lock() failed (%i)", shutdown_lock_ret); - } - - // take the file lock - do {} while (px4_sem_wait(¶m_sem_save) != 0); - - param_lock_reader(); - - int res = PX4_ERROR; - const char *filename = param_get_default_file(); - - if (filename) { - static constexpr int MAX_ATTEMPTS = 3; - - for (int attempt = 1; attempt <= MAX_ATTEMPTS; attempt++) { - // write parameters to file - int fd = ::open(filename, O_WRONLY | O_CREAT | O_TRUNC, PX4_O_MODE_666); - - if (fd > -1) { - perf_begin(param_export_perf); - res = param_export_internal(fd, nullptr); - perf_end(param_export_perf); - ::close(fd); - - if (res == PX4_OK) { - // reopen file to verify - int fd_verify = ::open(filename, O_RDONLY, PX4_O_MODE_666); - res = param_verify(fd_verify) || lseek(fd_verify, 0, SEEK_SET) || param_verify(fd_verify); - ::close(fd_verify); - } - } - - if (res == PX4_OK) { - break; - - } else { - PX4_ERR("parameter export to %s failed (%d) attempt %d", filename, res, attempt); - px4_usleep(10000); // wait at least 10 milliseconds before trying again - } - } - - } else { - perf_begin(param_export_perf); - res = flash_param_save(nullptr); - perf_end(param_export_perf); - } - - if (res != PX4_OK) { - PX4_ERR("param export failed (%d)", res); - - } else { - params_unsaved.reset(); - - // backup file - if (param_backup_file) { - int fd_backup_file = ::open(param_backup_file, O_WRONLY | O_CREAT | O_TRUNC, PX4_O_MODE_666); - - if (fd_backup_file > -1) { - int backup_export_ret = param_export_internal(fd_backup_file, nullptr); - ::close(fd_backup_file); - - if (backup_export_ret != 0) { - PX4_ERR("backup parameter export to %s failed (%d)", param_backup_file, backup_export_ret); - - } else { - // verify export - int fd_verify = ::open(param_backup_file, O_RDONLY, PX4_O_MODE_666); - param_verify(fd_verify); - ::close(fd_verify); - } - } - } - } - - param_unlock_reader(); - px4_sem_post(¶m_sem_save); - - if (shutdown_lock_ret == 0) { - px4_shutdown_unlock(); - } - - return res; -} - -/** - * @return 0 on success, 1 if all params have not yet been stored, -1 if device open failed, -2 if writing parameters failed - */ -int -param_load_default() -{ - int res = 0; - const char *filename = param_get_default_file(); - - if (!filename) { - return flash_param_load(); - } - - int fd_load = ::open(filename, O_RDONLY); - - if (fd_load < 0) { - /* no parameter file is OK, otherwise this is an error */ - if (errno != ENOENT) { - PX4_ERR("open '%s' for reading failed", filename); - return -1; - } - - return 1; - } - - int result = param_load(fd_load); - ::close(fd_load); - - if (result != 0) { - PX4_ERR("error reading parameters from '%s'", filename); - return -2; - } - - return res; -} - -static int param_verify_callback(bson_decoder_t decoder, bson_node_t node) -{ - if (node->type == BSON_EOO) { + if (parameter_server) { + parameter_server->autoSave(true); return 0; } - // find the parameter this node represents - param_t param = param_find_no_notification(node->name); + return -1; +} - if (param == PARAM_INVALID) { - PX4_ERR("verify: invalid parameter '%s'", node->name); - return -1; - } - - // handle verifying the parameter from the node - switch (node->type) { - case BSON_INT32: { - if (param_type(param) != PARAM_TYPE_INT32) { - PX4_ERR("verify: invalid param type %d for '%s' (BSON_INT32)", param_type(param), node->name); - return -1; - } - - int32_t value; - - if (param_get(param, &value) == 0) { - if (value == node->i32) { - return 1; // valid - - } else { - PX4_ERR("verify: '%s' invalid BSON value %" PRIi32 "(expected %" PRIi32 ")", node->name, node->i32, value); - } - } - } - break; - - case BSON_DOUBLE: { - if (param_type(param) != PARAM_TYPE_FLOAT) { - PX4_ERR("verify: invalid param type %d for '%s' (BSON_DOUBLE)", param_type(param), node->name); - return -1; - } - - float value; - - if (param_get(param, &value) == 0) { - if (fabsf(value - (float)node->d) <= FLT_EPSILON) { - return 1; // valid - - } else { - PX4_ERR("verify: '%s' invalid BSON value %.3f (expected %.3f)", node->name, node->d, (double)value); - } - } - } - break; - - default: - PX4_ERR("verify: '%s' invalid node type %d", node->name, node->type); +int param_load_default() +{ + if (parameter_server) { + return parameter_server->loadDefault(); } return -1; } -static int param_verify(int fd) +int param_export(const char *filename, param_filter_func filter) { - PX4_DEBUG("param_verify"); - - if (fd < 0) { - return -1; - } - - if (lseek(fd, 0, SEEK_SET) != 0) { - PX4_ERR("verify: seek failed"); - return -1; - } - - bson_decoder_s decoder{}; - - if (bson_decoder_init_file(&decoder, fd, param_verify_callback) == 0) { - int result = -1; - - do { - result = bson_decoder_next(&decoder); - - } while (result > 0); - - if (result == 0) { - if (decoder.total_document_size != decoder.total_decoded_size) { - PX4_ERR("BSON document size (%" PRId32 ") doesn't match bytes decoded (%" PRId32 ")", decoder.total_document_size, - decoder.total_decoded_size); - - } else { - return 0; - } - - } else if (result == -ENODATA) { - PX4_ERR("verify: no BSON data"); - - } else { - PX4_ERR("verify: failed (%d)", result); - } + if (parameter_server) { + return parameter_server->exportToFile(filename, filter); } return -1; } -int -param_export(const char *filename, param_filter_func filter) +int param_import(int fd) { - PX4_DEBUG("param_export"); - - int shutdown_lock_ret = px4_shutdown_lock(); - - if (shutdown_lock_ret != 0) { - PX4_ERR("px4_shutdown_lock() failed (%i)", shutdown_lock_ret); - } - - // take the file lock - do {} while (px4_sem_wait(¶m_sem_save) != 0); - - param_lock_reader(); - - int fd = ::open(filename, O_RDWR | O_CREAT, PX4_O_MODE_666); - int result = PX4_ERROR; - - perf_begin(param_export_perf); - - if (fd > -1) { - result = param_export_internal(fd, filter); - - } else { - result = flash_param_save(filter); - } - - perf_end(param_export_perf); - - param_unlock_reader(); - px4_sem_post(¶m_sem_save); - - if (shutdown_lock_ret == 0) { - px4_shutdown_unlock(); - } - - return result; -} - -// internal parameter export, caller is responsible for locking -static int param_export_internal(int fd, param_filter_func filter) -{ - PX4_DEBUG("param_export_internal"); - - int result = -1; - param_wbuf_s *s = nullptr; - bson_encoder_s encoder{}; - uint8_t bson_buffer[256]; - - if (lseek(fd, 0, SEEK_SET) != 0) { - PX4_ERR("export seek failed %d", errno); - return -1; - } - - if (bson_encoder_init_buf_file(&encoder, fd, &bson_buffer, sizeof(bson_buffer)) != 0) { - goto out; - } - - // no modified parameters, export empty BSON document - if (param_values == nullptr) { - result = 0; - goto out; - } - - while ((s = (struct param_wbuf_s *)utarray_next(param_values, s)) != nullptr) { - if (filter && !filter(s->param)) { - continue; - } - - // don't export default values - switch (param_type(s->param)) { - case PARAM_TYPE_INT32: { - int32_t default_value = 0; - param_get_default_value_internal(s->param, &default_value); - - if (s->val.i == default_value) { - PX4_DEBUG("skipping %s %" PRIi32 " export", param_name(s->param), default_value); - continue; - } - } - break; - - case PARAM_TYPE_FLOAT: { - float default_value = 0; - param_get_default_value_internal(s->param, &default_value); - - if (fabsf(s->val.f - default_value) <= FLT_EPSILON) { - PX4_DEBUG("skipping %s %.3f export", param_name(s->param), (double)default_value); - continue; - } - } - break; - } - - const char *name = param_name(s->param); - const size_t size = param_size(s->param); - - /* append the appropriate BSON type object */ - switch (param_type(s->param)) { - case PARAM_TYPE_INT32: { - const int32_t i = s->val.i; - PX4_DEBUG("exporting: %s (%d) size: %lu val: %" PRIi32, name, s->param, (long unsigned int)size, i); - - if (bson_encoder_append_int32(&encoder, name, i) != 0) { - PX4_ERR("BSON append failed for '%s'", name); - goto out; - } - } - break; - - case PARAM_TYPE_FLOAT: { - const double f = (double)s->val.f; - PX4_DEBUG("exporting: %s (%d) size: %lu val: %.3f", name, s->param, (long unsigned int)size, (double)f); - - if (bson_encoder_append_double(&encoder, name, f) != 0) { - PX4_ERR("BSON append failed for '%s'", name); - goto out; - } - } - break; - - default: - PX4_ERR("%s unrecognized parameter type %d, skipping export", name, param_type(s->param)); - } - } - - result = 0; - -out: - - if (result == 0) { - if (bson_encoder_fini(&encoder) != PX4_OK) { - PX4_ERR("BSON encoder finalize failed"); - result = -1; - } - } - - return result; -} - -static int -param_import_callback(bson_decoder_t decoder, bson_node_t node) -{ - /* - * EOO means the end of the parameter object. (Currently not supporting - * nested BSON objects). - */ - if (node->type == BSON_EOO) { - PX4_DEBUG("end of parameters"); - return 0; - } - - param_modify_on_import(node); - - // Find the parameter this node represents. If we don't know it, ignore the node. - param_t param = param_find_no_notification(node->name); - - if (param == PARAM_INVALID) { - PX4_WARN("ignoring unrecognised parameter '%s'", node->name); - return 1; - } - - // Handle setting the parameter from the node - switch (node->type) { - case BSON_INT32: { - if (param_type(param) == PARAM_TYPE_INT32) { - int32_t i = node->i32; - param_set_internal(param, &i, true, true); - PX4_DEBUG("Imported %s with value %" PRIi32, param_name(param), i); - - } else { - PX4_WARN("unexpected type for %s", node->name); - } - } - break; - - case BSON_DOUBLE: { - if (param_type(param) == PARAM_TYPE_FLOAT) { - float f = node->d; - param_set_internal(param, &f, true, true); - PX4_DEBUG("Imported %s with value %f", param_name(param), (double)f); - - } else { - PX4_WARN("unexpected type for %s", node->name); - } - } - break; - - default: - PX4_ERR("import: unrecognised node type for '%s'", node->name); - } - - // don't return zero, that means EOF - return 1; -} - -static int -param_import_internal(int fd) -{ - static constexpr int MAX_ATTEMPTS = 3; - - for (int attempt = 1; attempt <= MAX_ATTEMPTS; attempt++) { - bson_decoder_s decoder{}; - - if (bson_decoder_init_file(&decoder, fd, param_import_callback) == 0) { - int result = -1; - - do { - result = bson_decoder_next(&decoder); - - } while (result > 0); - - if (result == 0) { - if (decoder.total_document_size == decoder.total_decoded_size) { - PX4_INFO("BSON document size %" PRId32 " bytes, decoded %" PRId32 " bytes (INT32:%" PRIu16 ", FLOAT:%" PRIu16 ")", - decoder.total_document_size, decoder.total_decoded_size, - decoder.count_node_int32, decoder.count_node_double); - - return 0; - - } else { - PX4_ERR("BSON document size (%" PRId32 ") doesn't match bytes decoded (%" PRId32 ")", - decoder.total_document_size, decoder.total_decoded_size); - } - - } else if (result == -ENODATA) { - // silently retry as a precaution unless this is our last attempt - if (attempt == MAX_ATTEMPTS) { - PX4_DEBUG("BSON: no data"); - return 0; - } - - } else { - PX4_ERR("param import failed (%d) attempt %d", result, attempt); - } - - } else { - PX4_ERR("param import bson decoder init failed (attempt %d)", attempt); - } - - if (attempt != MAX_ATTEMPTS) { - if (lseek(fd, 0, SEEK_SET) != 0) { - PX4_ERR("import lseek failed (%d)", errno); - } - - px4_usleep(10000); // wait at least 10 milliseconds before trying again - } + if (parameter_server) { + return parameter_server->importFromFileDescriptor(fd); } return -1; } -int -param_import(int fd) +int param_load(int fd) { - if (fd < 0) { - return flash_param_import(); + if (parameter_server) { + return parameter_server->loadFromFileDescriptor(fd); } - return param_import_internal(fd); + return -1; } -int -param_load(int fd) +void param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_changed, bool only_used) { - if (fd < 0) { - return flash_param_load(); - } - - param_reset_all_internal(false); - return param_import_internal(fd); -} - -void -param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_changed, bool only_used) -{ - param_t param; - - for (param = 0; handle_in_range(param); param++) { - - /* if requested, skip unchanged values */ - if (only_changed && (param_find_changed(param) == nullptr)) { - continue; - } - - if (only_used && !param_used(param)) { - continue; - } - - func(arg, param); + if (parameter_server) { + parameter_server->forEachParameter(func, arg, only_changed, only_used); } } uint32_t param_hash_check() { - uint32_t param_hash = 0; - - param_lock_reader(); - - /* compute the CRC32 over all string param names and 4 byte values */ - for (param_t param = 0; handle_in_range(param); param++) { - if (!param_used(param) || param_is_volatile(param)) { - continue; - } - - const char *name = param_name(param); - const void *val = param_get_value_ptr(param); - param_hash = crc32part((const uint8_t *)name, strlen(name), param_hash); - param_hash = crc32part((const uint8_t *)val, param_size(param), param_hash); + if (parameter_server) { + parameter_server->hashCheck(); } - param_unlock_reader(); - - return param_hash; + return 0; } void param_print_status() { - PX4_INFO("summary: %d/%d (used/total)", param_count_used(), param_count()); - -#ifndef FLASH_BASED_PARAMS - const char *filename = param_get_default_file(); - - if (filename != nullptr) { - PX4_INFO("file: %s", param_get_default_file()); + if (parameter_server) { + parameter_server->printStatus(); } - - if (param_backup_file) { - PX4_INFO("backup file: %s", param_backup_file); - } - -#endif /* FLASH_BASED_PARAMS */ - - if (param_values != nullptr) { - PX4_INFO("storage array: %d/%d elements (%zu bytes total)", - utarray_len(param_values), param_values->n, param_values->n * sizeof(UT_icd)); - } - - if (param_custom_default_values != nullptr) { - PX4_INFO("storage array (custom defaults): %d/%d elements (%zu bytes total)", - utarray_len(param_custom_default_values), param_custom_default_values->n, - param_custom_default_values->n * sizeof(UT_icd)); - } - - PX4_INFO("auto save: %s", autosave_disabled ? "off" : "on"); - - if (!autosave_disabled && (last_autosave_timestamp > 0)) { - PX4_INFO("last auto save: %.3f seconds ago", hrt_elapsed_time(&last_autosave_timestamp) * 1e-6); - } - - perf_print_counter(param_export_perf); - perf_print_counter(param_find_perf); - perf_print_counter(param_get_perf); - perf_print_counter(param_set_perf); } diff --git a/src/lib/parameters/parameters_common.cpp b/src/lib/parameters/parameters_common.cpp deleted file mode 100644 index 215bc7e1b1..0000000000 --- a/src/lib/parameters/parameters_common.cpp +++ /dev/null @@ -1,139 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2022 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include - -/** - * @file parameters_common.cpp - * - * Global parameter store, functions common to kernel and user sides - * - */ - -static constexpr uint16_t param_info_count = sizeof(px4::parameters) / sizeof(param_info_s); - -/** - * Test whether a param_t is value. - * - * @param param The parameter handle to test. - * @return True if the handle is valid. - */ -static constexpr bool handle_in_range(param_t param) { return (param < param_info_count); } - -unsigned param_count() -{ - return param_info_count; -} - -int param_get_index(param_t param) -{ - if (handle_in_range(param)) { - return (unsigned)param; - } - - return -1; -} - -param_t param_for_index(unsigned index) -{ - if (index < param_info_count) { - return (param_t)index; - } - - return PARAM_INVALID; -} - -const char *param_name(param_t param) -{ - return handle_in_range(param) ? px4::parameters[param].name : nullptr; -} - -param_type_t param_type(param_t param) -{ - return handle_in_range(param) ? px4::parameters_type[param] : PARAM_TYPE_UNKNOWN; -} - -bool param_is_volatile(param_t param) -{ - if (handle_in_range(param)) { - for (const auto &p : px4::parameters_volatile) { - if (static_cast(param) == p) { - return true; - } - } - } - - return false; -} - -size_t param_size(param_t param) -{ - if (handle_in_range(param)) { - switch (param_type(param)) { - case PARAM_TYPE_INT32: - case PARAM_TYPE_FLOAT: - return 4; - - default: - return 0; - } - } - - return 0; -} - -int -param_get_system_default_value(param_t param, void *default_val) -{ - if (!handle_in_range(param)) { - return PX4_ERROR; - } - - int ret = PX4_OK; - - switch (param_type(param)) { - case PARAM_TYPE_INT32: - memcpy(default_val, &px4::parameters[param].val.i, param_size(param)); - break; - - case PARAM_TYPE_FLOAT: - memcpy(default_val, &px4::parameters[param].val.f, param_size(param)); - break; - - default: - ret = PX4_ERROR; - break; - } - - return ret; -} diff --git a/src/lib/parameters/parameters_ioctl.cpp b/src/lib/parameters/parameters_ioctl.cpp deleted file mode 100644 index 5074366365..0000000000 --- a/src/lib/parameters/parameters_ioctl.cpp +++ /dev/null @@ -1,187 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2021 Technology Innovation Institute. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file parameters_ioctl.cpp - * - * Protected build kernel space interface to global parameter store. - */ - -#define PARAM_IMPLEMENTATION - -#include - -#include "param.h" -#include "parameters_ioctl.h" -#include - -int param_ioctl(unsigned int cmd, unsigned long arg) -{ - int ret = OK; - - switch (cmd) { - case PARAMIOCNOTIFY: { - param_notify_changes(); - } - break; - - case PARAMIOCFIND: { - paramiocfind_t *data = (paramiocfind_t *)arg; - - if (data->notification) { - data->ret = param_find(data->name); - - } else { - data->ret = param_find_no_notification(data->name); - } - } - break; - - case PARAMIOCCOUNTUSED: { - paramioccountused_t *data = (paramioccountused_t *)arg; - data->ret = param_count_used(); - } - break; - - case PARAMIOCFORUSEDINDEX: { - paramiocforusedindex_t *data = (paramiocforusedindex_t *)arg; - data->ret = param_for_used_index(data->index); - } - break; - - case PARAMIOCGETUSEDINDEX: { - paramiocgetusedindex_t *data = (paramiocgetusedindex_t *)arg; - data->ret = param_get_used_index(data->param); - } - break; - - case PARAMIOCUNSAVED: { - paramiocunsaved_t *data = (paramiocunsaved_t *)arg; - data->ret = param_value_unsaved(data->param); - } - break; - - case PARAMIOCGET: { - paramiocget_t *data = (paramiocget_t *)arg; - - if (data->deflt) { - data->ret = param_get_default_value(data->param, data->val); - - } else { - data->ret = param_get(data->param, data->val); - } - } - break; - - case PARAMIOCAUTOSAVE: { - paramiocautosave_t *data = (paramiocautosave_t *)arg; - param_control_autosave(data->enable); - } - break; - - case PARAMIOCSET: { - paramiocset_t *data = (paramiocset_t *)arg; - - if (data->notification) { - data->ret = param_set(data->param, data->val); - - } else { - data->ret = param_set_no_notification(data->param, data->val); - } - } - break; - - case PARAMIOCUSED: { - paramiocused_t *data = (paramiocused_t *)arg; - data->ret = param_used(data->param); - } - break; - - case PARAMIOCSETUSED: { - paramiocsetused_t *data = (paramiocsetused_t *)arg; - param_set_used(data->param); - } - break; - - case PARAMIOCSETDEFAULT: { - paramiocsetdefault_t *data = (paramiocsetdefault_t *)arg; - data->ret = param_set_default_value(data->param, data->val); - } - break; - - case PARAMIOCRESETGROUP: { - paramiocresetgroup_t *data = (paramiocresetgroup_t *)arg; - - if (data->type == PARAM_RESET_EXCLUDES) { - param_reset_excludes(data->group, data->num_in_group); - - } else if (data->type == PARAM_RESET_SPECIFIC) { - param_reset_specific(data->group, data->num_in_group); - - } else { - param_reset_all(); - } - } - break; - - case PARAMIOCSAVEDEFAULT: { - paramiocsavedefault_t *data = (paramiocsavedefault_t *)arg; - data->ret = param_save_default(); - } - break; - - case PARAMIOCLOADDEFAULT: { - paramiocloaddefault_t *data = (paramiocloaddefault_t *)arg; - data->ret = param_load_default(); - } - break; - - case PARAMIOCEXPORT: { - paramiocexport_t *data = (paramiocexport_t *)arg; - data->ret = param_export(data->filename, nullptr); - } - break; - - case PARAMIOCHASH: { - paramiochash_t *data = (paramiochash_t *)arg; - data->ret = param_hash_check(); - } - break; - - default: - ret = -ENOTTY; - break; - } - - return ret; -} diff --git a/src/lib/parameters/parameters_ioctl.h b/src/lib/parameters/parameters_ioctl.h deleted file mode 100644 index 3bece1dfa4..0000000000 --- a/src/lib/parameters/parameters_ioctl.h +++ /dev/null @@ -1,156 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2021 Technology Innovation Institute. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file parameters_ioctl.h - * - * User space - kernel space interface to global parameter store. - */ - -#pragma once - -#define PARAM_IMPLEMENTATION -#include "param.h" -#include -#include - -#define _PARAMIOC(_n) (_PX4_IOC(_PARAMIOCBASE, _n)) - -#define PARAMIOCNOTIFY _PARAMIOC(1) - -#define PARAMIOCFIND _PARAMIOC(2) -typedef struct paramiocfind { - const char *name; - const bool notification; - param_t ret; -} paramiocfind_t; - - -#define PARAMIOCCOUNTUSED _PARAMIOC(3) -typedef struct paramioccountused { - unsigned ret; -} paramioccountused_t; - -#define PARAMIOCFORUSEDINDEX _PARAMIOC(4) -typedef struct paramiocforusedindex { - const unsigned index; - param_t ret; -} paramiocforusedindex_t; - -#define PARAMIOCGETUSEDINDEX _PARAMIOC(5) -typedef struct paramiocgetusedindex { - const param_t param; - unsigned ret; -} paramiocgetusedindex_t; - -#define PARAMIOCUNSAVED _PARAMIOC(6) -typedef struct paramiocunsaved { - const param_t param; - bool ret; -} paramiocunsaved_t; - -#define PARAMIOCGET _PARAMIOC(7) -typedef struct paramiocget { - const param_t param; - const bool deflt; - void *const val; - int ret; -} paramiocget_t; - -#define PARAMIOCAUTOSAVE _PARAMIOC(8) -typedef struct paramiocautosave { - const bool enable; -} paramiocautosave_t; - -#define PARAMIOCSET _PARAMIOC(9) -typedef struct paramiocset { - const param_t param; - const bool notification; - const void *val; - int ret; -} paramiocset_t; - -#define PARAMIOCUSED _PARAMIOC(10) -typedef struct paramiocused { - const param_t param; - bool ret; -} paramiocused_t; - -#define PARAMIOCSETUSED _PARAMIOC(11) -typedef struct paramiocsetused { - const param_t param; -} paramiocsetused_t; - -#define PARAMIOCSETDEFAULT _PARAMIOC(12) -typedef struct paramiocsetdefault { - const param_t param; - const void *val; - int ret; -} paramiocsetdefault_t; - -#define PARAMIOCRESETGROUP _PARAMIOC(14) -typedef enum { - PARAM_RESET_ALL, - PARAM_RESET_EXCLUDES, - PARAM_RESET_SPECIFIC -} param_reset_type_t; - -typedef struct paramiocresetgroup { - param_reset_type_t type; - const char **group; - const int num_in_group; -} paramiocresetgroup_t; - -#define PARAMIOCSAVEDEFAULT _PARAMIOC(15) -typedef struct paramiocsavedefault { - int ret; -} paramiocsavedefault_t; - -#define PARAMIOCLOADDEFAULT _PARAMIOC(16) -typedef struct paramiocloaddefault { - int ret; -} paramiocloaddefault_t; - - -#define PARAMIOCEXPORT _PARAMIOC(17) -typedef struct paramiocexport { - const char *filename; - int ret; -} paramiocexport_t; - -#define PARAMIOCHASH _PARAMIOC(18) -typedef struct paramiochash { - uint32_t ret; -} paramiochash_t; - -int param_ioctl(unsigned int cmd, unsigned long arg); diff --git a/src/lib/parameters/parameters_srv.cpp b/src/lib/parameters/parameters_srv.cpp new file mode 100644 index 0000000000..54180fa772 --- /dev/null +++ b/src/lib/parameters/parameters_srv.cpp @@ -0,0 +1,189 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file parameters.cpp + * + * Global parameter store. + * + * Note that it might make sense to convert this into a driver. That would + * offer some interesting options regarding state for e.g. ORB advertisements + * and background parameter saving. + */ + +#define PARAM_IMPLEMENTATION + +#include "param.h" + +#if defined(PX4_PARAM_CLIENT_SRV) + +#include +#include + +#include +#include + +#include +#include + +using namespace time_literals; + +param_t param_find(const char *name) +{ + // request + uORB::Publication request_pub{ORB_ID(srv_parameter_get_request)}; + uORB::SubscriptionBlocking response_sub{ORB_ID(srv_parameter_get_response)}; + srv_parameter_get_request_s request{}; + request.index = -1; + memcpy(request.name, name, 16); + request.timestamp = hrt_absolute_time(); + request_pub.publish(request); + + // response + while ((hrt_elapsed_time(&request.timestamp) < 50_ms) || response_sub.updated()) { + const unsigned last_generation = response_sub.get_last_generation(); + srv_parameter_get_response_s response{}; + + if (response_sub.updateBlocking(response, 1'000)) { + + if (response_sub.get_last_generation() != last_generation + 1) { + PX4_ERR("param_find: missed srv_parameter_get_response, generation %d -> %d", last_generation, + response_sub.get_last_generation()); + } + + if ((request.timestamp == response.timestamp_requested) + && (strncmp(request.name, response.parameter.name, sizeof(request.name)) == 0)) { + + return response.parameter.index; + } + } + } + + return PARAM_INVALID; +} + +param_t param_find_no_notification(const char *name) +{ + return param_find(name); +} + +int param_get(param_t param, void *val) +{ + // request + uORB::Publication request_pub{ORB_ID(srv_parameter_get_request)}; + uORB::SubscriptionBlocking response_sub{ORB_ID(srv_parameter_get_response)}; + srv_parameter_get_request_s request{}; + request.index = param; + request.timestamp = hrt_absolute_time(); + request_pub.publish(request); + + // response + while ((hrt_elapsed_time(&request.timestamp) < 50_ms) || response_sub.updated()) { + const unsigned last_generation = response_sub.get_last_generation(); + srv_parameter_get_response_s response{}; + + if (response_sub.updateBlocking(response, 1'000)) { + + if (response_sub.get_last_generation() != last_generation + 1) { + PX4_ERR("param_get: missed srv_parameter_get_response, generation %d -> %d", last_generation, + response_sub.get_last_generation()); + } + + if ((request.timestamp == response.timestamp_requested) && (request.index == response.parameter.index)) { + switch (response.parameter.type) { + case parameter_s::TYPE_INT32: + memcpy(val, &response.parameter.int32_value, sizeof(int32_t)); + break; + + case parameter_s::TYPE_FLOAT32: + memcpy(val, &response.parameter.float32_value, sizeof(float)); + break; + } + + if (response.result == srv_parameter_get_response_s::RESULT_GET_SUCCESS) { + return 0; + } + + return -1; + } + } + } + + return -1; +} + +int param_set(param_t param, const void *val) +{ + // request + uORB::Publication request_pub{ORB_ID(srv_parameter_set_request)}; + uORB::SubscriptionBlocking response_sub{ORB_ID(srv_parameter_set_response)}; + srv_parameter_set_request_s request{}; + request.parameter.index = param; + memcpy(&request.parameter.int32_value, val, sizeof(int32_t)); + memcpy(&request.parameter.float32_value, val, sizeof(float)); + request.timestamp = hrt_absolute_time(); + request_pub.publish(request); + + // response + while ((hrt_elapsed_time(&request.timestamp) < 50_ms) || response_sub.updated()) { + const unsigned last_generation = response_sub.get_last_generation(); + srv_parameter_set_response_s response{}; + + if (response_sub.updateBlocking(response, 1'000)) { + + if (response_sub.get_last_generation() != last_generation + 1) { + PX4_ERR("param_set: missed srv_parameter_set_response, generation %d -> %d", last_generation, + response_sub.get_last_generation()); + } + + if ((request.timestamp == response.timestamp_requested) && (request.parameter.index == response.parameter.index)) { + if (response.result == srv_parameter_set_response_s::RESULT_SET_SUCCESS) { + return 0; + } + + PX4_ERR("param_set %d failed", param); + return -1; + } + } + } + + PX4_ERR("param_set %d failed", param); + return -1; +} + +int param_set_no_notification(param_t param, const void *val) +{ + return param_set(param, val); +} + +#endif // PX4_PARAM_CLIENT_SRV diff --git a/src/lib/parameters/usr_parameters_if.cpp b/src/lib/parameters/usr_parameters_if.cpp deleted file mode 100644 index 03e6003c82..0000000000 --- a/src/lib/parameters/usr_parameters_if.cpp +++ /dev/null @@ -1,207 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2021 Technology Innovation Institute. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file usr_parameters_if.cpp - * - * Protected build user space interface to global parameter store. - */ -#define PARAM_IMPLEMENTATION -#include "param.h" -#include "parameters_ioctl.h" -#include -#include -#include -#include - -#include "parameters_common.cpp" - -void -param_notify_changes() -{ - boardctl(PARAMIOCNOTIFY, NULL); -} - -param_t param_find(const char *name) -{ - paramiocfind_t data = {name, true, PARAM_INVALID}; - boardctl(PARAMIOCFIND, reinterpret_cast(&data)); - return data.ret; -} - -param_t param_find_no_notification(const char *name) -{ - paramiocfind_t data = {name, false, PARAM_INVALID}; - boardctl(PARAMIOCFIND, reinterpret_cast(&data)); - return data.ret; -} - -unsigned param_count_used() -{ - paramioccountused_t data = {0}; - boardctl(PARAMIOCCOUNTUSED, reinterpret_cast(&data)); - return data.ret; -} - -param_t param_for_used_index(unsigned index) -{ - paramiocforusedindex_t data = {index, 0}; - boardctl(PARAMIOCFORUSEDINDEX, reinterpret_cast(&data)); - return data.ret; -} - -int param_get_used_index(param_t param) -{ - paramiocgetusedindex_t data = {param, 0}; - boardctl(PARAMIOCGETUSEDINDEX, reinterpret_cast(&data)); - return data.ret; -} - -bool -param_value_unsaved(param_t param) -{ - paramiocunsaved_t data = {param, false}; - boardctl(PARAMIOCUNSAVED, reinterpret_cast(&data)); - return data.ret; -} - -int -param_get(param_t param, void *val) -{ - paramiocget_t data = {param, false, val, PX4_ERROR}; - boardctl(PARAMIOCGET, reinterpret_cast(&data)); - return data.ret; -} - -int -param_get_default_value(param_t param, void *default_val) -{ - paramiocget_t data = {param, true, default_val, PX4_ERROR}; - boardctl(PARAMIOCGET, reinterpret_cast(&data)); - return data.ret; -} - -void -param_control_autosave(bool enable) -{ - paramiocautosave_t data = {enable}; - boardctl(PARAMIOCAUTOSAVE, reinterpret_cast(&data)); -} - -int param_set(param_t param, const void *val) -{ - paramiocset_t data = {param, true, val, PX4_ERROR}; - boardctl(PARAMIOCSET, reinterpret_cast(&data)); - return data.ret; -} - -int param_set_no_notification(param_t param, const void *val) -{ - paramiocset_t data = {param, false, val, PX4_ERROR}; - boardctl(PARAMIOCSET, reinterpret_cast(&data)); - return data.ret; -} - -bool param_used(param_t param) -{ - paramiocused_t data = {param, false}; - boardctl(PARAMIOCUSED, reinterpret_cast(&data)); - return data.ret; -} - -void param_set_used(param_t param) -{ - paramiocsetused_t data = {param}; - boardctl(PARAMIOCSETUSED, reinterpret_cast(&data)); -} - -int param_set_default_value(param_t param, const void *val) -{ - paramiocsetdefault_t data = {param, val, PX4_ERROR}; - boardctl(PARAMIOCSETDEFAULT, reinterpret_cast(&data)); - return data.ret; -} - -void -param_reset_all() -{ - paramiocresetgroup_t data = {PARAM_RESET_ALL, nullptr, 0}; - boardctl(PARAMIOCRESETGROUP, reinterpret_cast(&data)); -} - -void -param_reset_excludes(const char *excludes[], int num_excludes) -{ - paramiocresetgroup_t data = {PARAM_RESET_EXCLUDES, excludes, num_excludes}; - boardctl(PARAMIOCRESETGROUP, reinterpret_cast(&data)); -} - -void -param_reset_specific(const char *resets[], int num_resets) -{ - paramiocresetgroup_t data = {PARAM_RESET_SPECIFIC, resets, num_resets}; - boardctl(PARAMIOCRESETGROUP, reinterpret_cast(&data)); -} - -int param_save_default() -{ - paramiocsavedefault_t data = {PX4_ERROR}; - boardctl(PARAMIOCSAVEDEFAULT, reinterpret_cast(&data)); - return data.ret; -} - -int -param_load_default() -{ - paramiocloaddefault_t data = {PX4_ERROR}; - boardctl(PARAMIOCLOADDEFAULT, reinterpret_cast(&data)); - return data.ret; -} - -int -param_export(const char *filename, param_filter_func filter) -{ - paramiocexport_t data = {filename, PX4_ERROR}; - - if (filter) { PX4_ERR("ERROR: filter not supported in userside blob"); } - - boardctl(PARAMIOCEXPORT, reinterpret_cast(&data)); - return data.ret; -} - -uint32_t param_hash_check() -{ - paramiochash_t data = {0}; - boardctl(PARAMIOCHASH, reinterpret_cast(&data)); - return data.ret; -} diff --git a/src/lib/tinybson/tinybson.cpp b/src/lib/tinybson/tinybson.cpp index 568bdd6816..e4e11fc0ff 100644 --- a/src/lib/tinybson/tinybson.cpp +++ b/src/lib/tinybson/tinybson.cpp @@ -115,10 +115,11 @@ read_double(bson_decoder_t decoder, double *d) } int -bson_decoder_init_file(bson_decoder_t decoder, int fd, bson_decoder_callback callback) +bson_decoder_init_file(bson_decoder_t decoder, int fd, bson_decoder_callback callback, void *priv) { decoder->fd = fd; decoder->callback = callback; + decoder->priv = priv; decoder->nesting = 1; decoder->node.type = BSON_UNDEFINED; @@ -134,7 +135,7 @@ bson_decoder_init_file(bson_decoder_t decoder, int fd, bson_decoder_callback cal } int -bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_decoder_callback callback) +bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_decoder_callback callback, void *priv) { /* argument sanity */ if ((buf == nullptr) || (callback == nullptr)) { @@ -155,6 +156,7 @@ bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_ decoder->bufpos = 0; decoder->callback = callback; + decoder->priv = priv; decoder->nesting = 1; decoder->pending = 0; decoder->node.type = BSON_UNDEFINED; @@ -315,7 +317,7 @@ bson_decoder_next(bson_decoder_t decoder) } /* call the callback and pass its results back */ - return decoder->callback(decoder, &decoder->node); + return decoder->callback(decoder, decoder->priv, &decoder->node); } int diff --git a/src/lib/tinybson/tinybson.h b/src/lib/tinybson/tinybson.h index eb370454ee..96629061d6 100644 --- a/src/lib/tinybson/tinybson.h +++ b/src/lib/tinybson/tinybson.h @@ -101,7 +101,7 @@ typedef struct bson_decoder_s *bson_decoder_t; * * The node callback function's return value is returned by bson_decoder_next. */ -typedef int (* bson_decoder_callback)(bson_decoder_t decoder, bson_node_t node); +typedef int (* bson_decoder_callback)(bson_decoder_t decoder, void *priv, bson_node_t node); struct bson_decoder_s { /* file reader state */ @@ -114,6 +114,7 @@ struct bson_decoder_s { bool dead{false}; bson_decoder_callback callback; + void *priv{nullptr}; unsigned nesting{0}; struct bson_node_s node {}; int32_t pending{0}; @@ -135,9 +136,10 @@ struct bson_decoder_s { * @param decoder Decoder state structure to be initialised. * @param fd File to read BSON data from. * @param callback Callback to be invoked by bson_decoder_next + * @param priv Callback private data, stored in node. * @return Zero on success. */ -__EXPORT int bson_decoder_init_file(bson_decoder_t decoder, int fd, bson_decoder_callback callback); +__EXPORT int bson_decoder_init_file(bson_decoder_t decoder, int fd, bson_decoder_callback callback, void *priv); /** * Initialise the decoder to read from a buffer in memory. @@ -148,9 +150,11 @@ __EXPORT int bson_decoder_init_file(bson_decoder_t decoder, int fd, bson_decoder * passed as zero if the buffer size should be extracted from the * BSON header only. * @param callback Callback to be invoked by bson_decoder_next + * @param priv Callback private data, stored in node. * @return Zero on success. */ -__EXPORT int bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_decoder_callback callback); +__EXPORT int bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_decoder_callback callback, + void *priv); /** * Process the next node from the stream and invoke the callback. diff --git a/src/modules/commander/failsafe/emscripten.cpp b/src/modules/commander/failsafe/emscripten.cpp index f9d56e5479..0f2dc2d3f2 100644 --- a/src/modules/commander/failsafe/emscripten.cpp +++ b/src/modules/commander/failsafe/emscripten.cpp @@ -89,19 +89,6 @@ int param_get(param_t param, void *val) return -1; } -void param_set_used(param_t param) -{ - std::map &used_params = failsafe_instance.params(); - - if (used_params.find(param) != used_params.end()) { - return; - } - - Param p; - memcpy(&p.val, &px4::parameters[param].val, sizeof(p.val)); - used_params[param] = p; -} - std::vector get_used_params() { std::vector ret; diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index 1ffafb796e..25f568703a 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -68,6 +68,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) if (req_list.target_system == mavlink_system.sysid && (req_list.target_component == mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) { + if (_send_all_index < 0) { _send_all_index = PARAM_HASH; @@ -77,15 +78,16 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) } } - if (req_list.target_system == mavlink_system.sysid && req_list.target_component < 127 && + if ((req_list.target_system == mavlink_system.sysid) && (req_list.target_component < 127) && (req_list.target_component != mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) { + // publish list request to UAVCAN driver via uORB. - uavcan_parameter_request_s req{}; - req.message_type = msg->msgid; - req.node_id = req_list.target_component; - req.param_index = 0; - req.timestamp = hrt_absolute_time(); - _uavcan_parameter_request_pub.publish(req); + parameter_request_s parameter_request{}; + parameter_request.message_type = parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_LIST; + parameter_request.node_id = req_list.target_component; + parameter_request.param_index = 0; + parameter_request.timestamp = hrt_absolute_time(); + _uavcan_parameter_request_pub.publish(parameter_request); } break; @@ -102,8 +104,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) /* local name buffer to enforce null-terminated string */ char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; strncpy(name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); - /* enforce null termination */ - name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; + name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; // enforce null termination /* Whatever the value is, we're being told to stop sending */ if (strncmp(name, "_HASH_CHECK", sizeof(name)) == 0) { @@ -112,18 +113,20 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) _send_all_index = -1; } - /* No other action taken, return */ + // No other action taken, return return; } - /* attempt to find parameter, set and send it */ + // attempt to find parameter, set and send it param_t param = param_find_no_notification(name); if (param == PARAM_INVALID) { PX4_ERR("unknown param: %s", name); } else if (!((param_type(param) == PARAM_TYPE_INT32 && set.param_type == MAV_PARAM_TYPE_INT32) || - (param_type(param) == PARAM_TYPE_FLOAT && set.param_type == MAV_PARAM_TYPE_REAL32))) { + (param_type(param) == PARAM_TYPE_FLOAT && set.param_type == MAV_PARAM_TYPE_REAL32)) + ) { + PX4_ERR("param types mismatch param: %s", name); } else { @@ -135,27 +138,27 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) if (set.target_system == mavlink_system.sysid && set.target_component < 127 && (set.target_component != mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) { + // publish set request to UAVCAN driver via uORB. - uavcan_parameter_request_s req{}; - req.message_type = msg->msgid; - req.node_id = set.target_component; - req.param_index = -1; - strncpy(req.param_id, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1); - req.param_id[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; + parameter_request_s parameter_request{}; + parameter_request.message_type = parameter_request_s::MESSAGE_TYPE_PARAM_SET; + parameter_request.node_id = set.target_component; + parameter_request.param_index = -1; + + strncpy(parameter_request.name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1); + parameter_request.name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; if (set.param_type == MAV_PARAM_TYPE_REAL32) { - req.param_type = MAV_PARAM_TYPE_REAL32; - req.real_value = set.param_value; + parameter_request.type = parameter_request_s::TYPE_FLOAT32; + parameter_request.float32_value = set.param_value; } else { - int32_t val; - memcpy(&val, &set.param_value, sizeof(int32_t)); - req.param_type = MAV_PARAM_TYPE_INT64; - req.int_value = val; + memcpy(¶meter_request.int32_value, &set.param_value, sizeof(int32_t)); + parameter_request.type = parameter_request_s::TYPE_INT32; } - req.timestamp = hrt_absolute_time(); - _uavcan_parameter_request_pub.publish(req); + parameter_request.timestamp = hrt_absolute_time(); + _uavcan_parameter_request_pub.publish(parameter_request); } break; @@ -177,7 +180,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) uint32_t hash = param_hash_check(); /* build the one-off response message */ - mavlink_param_value_t param_value; + mavlink_param_value_t param_value{}; param_value.param_count = param_count_used(); param_value.param_index = -1; strncpy(param_value.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); @@ -210,17 +213,18 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) if (req_read.target_system == mavlink_system.sysid && req_read.target_component < 127 && (req_read.target_component != mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) { + // publish set request to UAVCAN driver via uORB. - uavcan_parameter_request_s req{}; - req.timestamp = hrt_absolute_time(); - req.message_type = msg->msgid; - req.node_id = req_read.target_component; - req.param_index = req_read.param_index; - strncpy(req.param_id, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1); - req.param_id[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; + parameter_request_s parameter_request{}; + parameter_request.timestamp = hrt_absolute_time(); + parameter_request.message_type = parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_READ; + parameter_request.node_id = req_read.target_component; + parameter_request.param_index = req_read.param_index; + strncpy(parameter_request.name, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1); + parameter_request.name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; // Enque the request and forward the first to the uavcan node - enque_uavcan_request(&req); + enque_uavcan_request(¶meter_request); request_next_uavcan_parameter(); } @@ -397,7 +401,7 @@ bool MavlinkParametersManager::send_uavcan() { /* Send parameter values received from the UAVCAN topic */ - uavcan_parameter_value_s value{}; + parameter_value_s value; if (_uavcan_parameter_value_sub.update(&value)) { @@ -413,9 +417,7 @@ MavlinkParametersManager::send_uavcan() mavlink_param_value_t msg{}; msg.param_count = value.param_count; msg.param_index = value.param_index; -#if defined(__GNUC__) && __GNUC__ >= 8 -#pragma GCC diagnostic ignored "-Wstringop-truncation" -#endif + /* * coverity[buffer_size_warning : FALSE] * @@ -423,17 +425,14 @@ MavlinkParametersManager::send_uavcan() * has length 16. In this case the receiving end needs to terminate it * when copying it. */ - strncpy(msg.param_id, value.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); -#if defined(__GNUC__) && __GNUC__ >= 8 -#pragma GCC diagnostic pop -#endif + memcpy(msg.param_id, value.param_name, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); - if (value.param_type == MAV_PARAM_TYPE_REAL32) { + if (value.type == parameter_request_s::TYPE_FLOAT32) { msg.param_type = MAVLINK_TYPE_FLOAT; - msg.param_value = value.real_value; + msg.param_value = value.float32_value; } else { - int32_t val = (int32_t)value.int_value; + int32_t val = value.int32_value; memcpy(&msg.param_value, &val, sizeof(int32_t)); msg.param_type = MAVLINK_TYPE_INT32_T; } @@ -516,7 +515,7 @@ MavlinkParametersManager::send_param(param_t param, int component_id) return 1; } - mavlink_param_value_t msg; + mavlink_param_value_t msg{}; /* * get param value, since MAVLink encodes float and int params in the same @@ -544,10 +543,6 @@ MavlinkParametersManager::send_param(param_t param, int component_id) msg.param_count = param_count_used(); msg.param_index = param_get_used_index(param); -#if defined(__GNUC__) && __GNUC__ >= 8 -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wstringop-truncation" -#endif /* * coverity[buffer_size_warning : FALSE] * @@ -555,10 +550,7 @@ MavlinkParametersManager::send_param(param_t param, int component_id) * has length 16. In this case the receiving end needs to terminate it * when copying it. */ - strncpy(msg.param_id, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); -#if defined(__GNUC__) && __GNUC__ >= 8 -#pragma GCC diagnostic pop -#endif + memcpy(msg.param_id, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); /* query parameter type */ param_type_t type = param_type(param); @@ -595,7 +587,7 @@ void MavlinkParametersManager::request_next_uavcan_parameter() { // Request a parameter if we are not already waiting on a response and if the list is not empty if (!_uavcan_waiting_for_request_response && _uavcan_open_request_list != nullptr) { - uavcan_parameter_request_s req = _uavcan_open_request_list->req; + parameter_request_s req = _uavcan_open_request_list->req; _uavcan_parameter_request_pub.publish(req); @@ -603,7 +595,7 @@ void MavlinkParametersManager::request_next_uavcan_parameter() } } -void MavlinkParametersManager::enque_uavcan_request(uavcan_parameter_request_s *req) +void MavlinkParametersManager::enque_uavcan_request(parameter_request_s *req) { // We store at max 10 requests to keep memory consumption low. // Dropped requests will be repeated by the ground station diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h index d55ed8e029..fab6df7b4d 100644 --- a/src/modules/mavlink/mavlink_parameters.h +++ b/src/modules/mavlink/mavlink_parameters.h @@ -49,8 +49,8 @@ #include #include #include -#include -#include +#include +#include #include #include @@ -105,8 +105,8 @@ protected: // Item of a single-linked list to store requested uavcan parameters struct _uavcan_open_request_list_item { - uavcan_parameter_request_s req; - struct _uavcan_open_request_list_item *next; + parameter_request_s req{}; + _uavcan_open_request_list_item *next{nullptr}; }; /** @@ -117,7 +117,7 @@ protected: /** * Enqueue one uavcan parameter reqest. We store 10 at max. */ - void enque_uavcan_request(uavcan_parameter_request_s *req); + void enque_uavcan_request(parameter_request_s *req); /** * Drop the first reqest from the list @@ -131,23 +131,7 @@ protected: uORB::Publication _rc_param_map_pub{ORB_ID(rc_parameter_map)}; rc_parameter_map_s _rc_param_map{}; - uORB::Publication _uavcan_parameter_request_pub{ORB_ID(uavcan_parameter_request)}; - // enforce ORB_ID(uavcan_parameter_request) constants that map to MAVLINK defines - static_assert(uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_READ == MAVLINK_MSG_ID_PARAM_REQUEST_READ, - "uavcan_parameter_request_s MAVLINK_MSG_ID_PARAM_REQUEST_READ constant mismatch"); - static_assert(uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_SET == MAVLINK_MSG_ID_PARAM_SET, - "uavcan_parameter_request_s MAVLINK_MSG_ID_PARAM_SET constant mismatch"); - static_assert(uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_LIST == MAVLINK_MSG_ID_PARAM_REQUEST_LIST, - "uavcan_parameter_request_s MAVLINK_MSG_ID_PARAM_REQUEST_LIST constant mismatch"); - static_assert(uavcan_parameter_request_s::NODE_ID_ALL == MAV_COMP_ID_ALL, - "uavcan_parameter_request_s MAV_COMP_ID_ALL constant mismatch"); - static_assert(uavcan_parameter_request_s::PARAM_TYPE_UINT8 == MAV_PARAM_TYPE_UINT8, - "uavcan_parameter_request_s MAV_PARAM_TYPE_UINT8 constant mismatch"); - static_assert(uavcan_parameter_request_s::PARAM_TYPE_REAL32 == MAV_PARAM_TYPE_REAL32, - "uavcan_parameter_request_s MAV_PARAM_TYPE_REAL32 constant mismatch"); - static_assert(uavcan_parameter_request_s::PARAM_TYPE_INT64 == MAV_PARAM_TYPE_INT64, - "uavcan_parameter_request_s MAV_PARAM_TYPE_INT64 constant mismatch"); - + uORB::Publication _uavcan_parameter_request_pub{ORB_ID(uavcan_parameter_request)}; uORB::Subscription _uavcan_parameter_value_sub{ORB_ID(uavcan_parameter_value)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; diff --git a/src/systemcmds/bsondump/bsondump.cpp b/src/systemcmds/bsondump/bsondump.cpp index 50021eba14..842c25cb8e 100644 --- a/src/systemcmds/bsondump/bsondump.cpp +++ b/src/systemcmds/bsondump/bsondump.cpp @@ -51,7 +51,7 @@ static void print_usage(const char *reason = nullptr) PRINT_MODULE_USAGE_ARG("", "File name", false); } -static int bson_print_callback(bson_decoder_t decoder, bson_node_t node) +static int bson_print_callback(bson_decoder_t decoder, void *priv, bson_node_t node) { switch (node->type) { case BSON_EOO: @@ -102,7 +102,7 @@ extern "C" __EXPORT int bsondump_main(int argc, char *argv[]) bson_decoder_s decoder{}; - if (bson_decoder_init_file(&decoder, fd, bson_print_callback) == 0) { + if (bson_decoder_init_file(&decoder, fd, bson_print_callback, nullptr) == 0) { PX4_INFO_RAW("BSON document size %" PRId32 "\n", decoder.total_document_size); int result = -1; diff --git a/src/systemcmds/tests/test_bson.cpp b/src/systemcmds/tests/test_bson.cpp index 939d51fe8d..5878cebc46 100644 --- a/src/systemcmds/tests/test_bson.cpp +++ b/src/systemcmds/tests/test_bson.cpp @@ -96,7 +96,7 @@ encode(bson_encoder_t encoder) } static int -decode_callback(bson_decoder_t decoder, bson_node_t node) +decode_callback(bson_decoder_t decoder, void *priv, bson_node_t node) { unsigned len; @@ -287,7 +287,7 @@ test_bson(int argc, char *argv[]) } /* now test-decode it */ - if (bson_decoder_init_buf(&decoder, buf, len, decode_callback)) { + if (bson_decoder_init_buf(&decoder, buf, len, decode_callback, nullptr)) { PX4_ERR("FAIL: bson_decoder_init_buf"); return 1; }