From 481addbcac9390da02925a1dc9ecef5ea5499b88 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 9 Dec 2022 15:09:29 -0500 Subject: [PATCH] WIP: parameter server --- msg/CMakeLists.txt | 4 +- msg/ParameterRequest.msg | 30 + msg/ParameterValue.msg | 17 + msg/UavcanParameterRequest.msg | 23 - msg/UavcanParameterValue.msg | 9 - platforms/nuttx/src/px4/common/px4_init.cpp | 4 +- platforms/posix/src/px4/common/px4_init.cpp | 4 +- src/drivers/uavcan/uavcan_main.cpp | 52 +- src/drivers/uavcan/uavcan_main.hpp | 6 +- src/drivers/uavcan/uavcan_servers.cpp | 2 +- src/lib/parameters/CMakeLists.txt | 20 +- src/lib/parameters/ParameterServer.cpp | 1698 +++++++++++++++++ src/lib/parameters/ParameterServer.hpp | 560 ++++++ .../parameters/flashparams/flashparams.cpp | 14 +- src/lib/parameters/param.h | 3 +- src/lib/parameters/parameters.cpp | 1677 ++-------------- src/lib/parameters/parameters_common.cpp | 139 -- src/lib/parameters/tinybson/tinybson.cpp | 8 +- src/lib/parameters/tinybson/tinybson.h | 10 +- src/modules/mavlink/mavlink_parameters.cpp | 104 +- src/modules/mavlink/mavlink_parameters.h | 28 +- src/systemcmds/tests/test_bson.cpp | 4 +- 22 files changed, 2605 insertions(+), 1811 deletions(-) create mode 100644 msg/ParameterRequest.msg create mode 100644 msg/ParameterValue.msg delete mode 100644 msg/UavcanParameterRequest.msg delete mode 100644 msg/UavcanParameterValue.msg create mode 100644 src/lib/parameters/ParameterServer.cpp create mode 100644 src/lib/parameters/ParameterServer.hpp delete mode 100644 src/lib/parameters/parameters_common.cpp diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 2f6358f9eb..0a5876014c 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 diff --git a/msg/ParameterRequest.msg b/msg/ParameterRequest.msg new file mode 100644 index 0000000000..90d27d0b5d --- /dev/null +++ b/msg/ParameterRequest.msg @@ -0,0 +1,30 @@ +# 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_UINT8 = 2 +uint8 TYPE_INT32 = 3 +uint8 TYPE_INT64 = 4 +uint8 TYPE_FLOAT32 = 5 +uint8 TYPE_FLOAT64 = 6 +uint8 type # parameter type + +int64 int64_value # current value if param_type is int-like +float64 float64_value # current value if param_type is float-like + +uint8 ORB_QUEUE_LENGTH = 4 + +# TOPICS parameter_request uavcan_parameter_request diff --git a/msg/ParameterValue.msg b/msg/ParameterValue.msg new file mode 100644 index 0000000000..4a1b8df9a3 --- /dev/null +++ b/msg/ParameterValue.msg @@ -0,0 +1,17 @@ +# 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 + +int64 int64_value # current value if param_type is int-like +float64 float64_value # current value if param_type is float-like + +# TOPICS parameter_value uavcan_parameter_value 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/nuttx/src/px4/common/px4_init.cpp b/platforms/nuttx/src/px4/common/px4_init.cpp index fb371b9093..3dd02f50f5 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(); @@ -183,6 +181,8 @@ int px4_platform_init() px4::WorkQueueManagerStart(); + param_init(); + uorb_start(); px4_log_initialize(); diff --git a/platforms/posix/src/px4/common/px4_init.cpp b/platforms/posix/src/px4/common/px4_init.cpp index 71af70565d..b9989a54e0 100644 --- a/platforms/posix/src/px4/common/px4_init.cpp +++ b/platforms/posix/src/px4/common/px4_init.cpp @@ -44,10 +44,10 @@ int px4_platform_init(void) { hrt_init(); - param_init(); - px4::WorkQueueManagerStart(); + param_init(); + uorb_start(); px4_log_initialize(); diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index 24b677da6f..9228ffd374 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -691,7 +691,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]) { @@ -699,14 +700,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); @@ -719,24 +720,29 @@ 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.int64_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: + case parameter_request_s::TYPE_FLOAT64: + req.value.to() = request.float64_value; + break; - } else { - req.value.to() = request.int_value; + default: + req.value.to() = request.int64_value; + break; } // Set the dirty bit for this node @@ -752,7 +758,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; @@ -762,8 +768,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. @@ -1081,24 +1087,24 @@ UavcanNode::cb_getset(const uavcan::ServiceCallResult(); + response.type = parameter_request_s::TYPE_INT64; + response.int64_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.float64_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.int64_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 2bb8a434c0..731dcbc0a7 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; @@ -266,7 +266,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 e2fed86119..dd0630a6ac 100644 --- a/src/lib/parameters/CMakeLists.txt +++ b/src/lib/parameters/CMakeLists.txt @@ -160,15 +160,15 @@ else() endif() if(${PX4_PLATFORM} STREQUAL "nuttx") - add_subdirectory(flashparams) + # 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() + # # 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 @@ -176,6 +176,8 @@ 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) @@ -193,7 +195,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..f9eec00863 --- /dev/null +++ b/src/lib/parameters/ParameterServer.cpp @@ -0,0 +1,1698 @@ +/**************************************************************************** + * + * 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::lp_default) +{ + 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 + + ScheduleOnInterval(1_s); +} + +ParameterServer::~ParameterServer() +{ + px4_sem_destroy(¶m_sem); + px4_sem_destroy(¶m_sem_save); + px4_sem_destroy(&reader_lock_holders_lock); + + perf_free(param_export_perf); + perf_free(param_find_perf); + perf_free(param_get_perf); + perf_free(param_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(¶m_sem) != 0); + } + + px4_sem_post(&reader_lock_holders_lock); +} + +void ParameterServer::lockWriter() +{ + do {} while (px4_sem_wait(¶m_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(¶m_sem); + } + + px4_sem_post(&reader_lock_holders_lock); +} + +void ParameterServer::unlockWriter() +{ + px4_sem_post(¶m_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(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, getParameterName(middle)); + + if (ret == 0) { + if (notification) { + setParameterUsed(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; +} + +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(param_get_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)); + } + + 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(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 = 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(param_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::setParameterUsed(param_t param) +{ + if (handle_in_range(param)) { + params_active.set(param, true); + } +} + +void ParameterServer::notifyChanges() +{ + 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); + } +} + +int ParameterServer::resetParameter(param_t param, bool notify) +{ + 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 (s != nullptr && notify) { + 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(¶m_sem_save) != 0); + + lockReader(); + + 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 = exportInternal(fd, filter); + + } else { + // TODO: flash params + // result = flash_param_save(filter); + } + + perf_end(param_export_perf); + + unlockReader(); + px4_sem_post(¶m_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 (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 (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 (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); + } + + 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; +} + +int ParameterServer::bsonDumpCallback(bson_decoder_t decoder, bson_node_t node) +{ + switch (node->type) { + case BSON_EOO: + PX4_INFO_RAW("BSON_EOO\n"); + return 0; + + case BSON_DOUBLE: + PX4_INFO_RAW("BSON_DOUBLE: %s = %.6f\n", node->name, node->d); + return 1; + + case BSON_BOOL: + PX4_INFO_RAW("BSON_BOOL: %s = %d\n", node->name, node->b); + return 1; + + case BSON_INT32: + PX4_INFO_RAW("BSON_INT32: %s = %" PRIi32 "\n", node->name, node->i32); + return 1; + + case BSON_INT64: + PX4_INFO_RAW("BSON_INT64: %s = %" PRIi64 "\n", node->name, node->i64); + return 1; + + default: + PX4_INFO_RAW("ERROR %s unhandled bson type %d\n", node->name, node->type); + return 1; // just skip this entry + } + + return -1; +} + +int ParameterServer::bsonDumpCallbackTrampoline(bson_decoder_t decoder, void *priv, bson_node_t node) +{ + if (priv) { + return static_cast(priv)->bsonDumpCallback(decoder, node); + } + + return -1; +} + +int ParameterServer::bsonDump(int fd) +{ + bson_decoder_s decoder{}; + + if (bson_decoder_init_file(&decoder, fd, bsonDumpCallbackTrampoline, this) == 0) { + PX4_INFO_RAW("BSON document size %" PRId32 "\n", decoder.total_document_size); + + int result = -1; + + do { + result = bson_decoder_next(&decoder); + + } while (result > 0); + + if (result == 0) { + PX4_INFO_RAW("BSON decoded %" PRId32 " bytes (double:%" PRIu16 ", string:%" PRIu16 ", bin:%" PRIu16 ", bool:%" PRIu16 + ", int32:%" PRIu16 ", int64:%" PRIu16 ")\n", + decoder.total_decoded_size, + decoder.count_node_double, decoder.count_node_string, decoder.count_node_bindata, decoder.count_node_bool, + decoder.count_node_int32, decoder.count_node_int64); + + return 0; + + } else if (result == -ENODATA) { + PX4_WARN("BSON: no data"); + return 0; + + } else { + PX4_ERR("param dump 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(¶m_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(param_export_perf); + res = exportInternal(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 = 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(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 = 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(¶m_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)", param_count_used(), param_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(param_export_perf); + perf_print_counter(param_find_perf); + perf_print_counter(param_get_perf); + perf_print_counter(param_set_perf); +} + +void ParameterServer::controlAutosave(bool enable) +{ + lockWriter(); + + if (!enable && autosave_scheduled.load()) { + autosave_scheduled.store(false); + } + + autosave_disabled = !enable; + unlockWriter(); +} + +void ParameterServer::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); + + 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() +{ + + // TODO: param_request + // Check for parameter requests (get/set/list) + if (_param_request_sub.updated()) { + parameter_request_s request; + _param_request_sub.copy(&request); + + /* + * We know how many parameters are exposed by this node, so + * process the request. + */ + if (request.message_type == parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_READ) { + //parameter_value_s parameter_value{}; + + + } else if (request.message_type == parameter_request_s::MESSAGE_TYPE_PARAM_SET) { + + } else if (request.message_type == 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) { + + } + } + } + + + + // 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) + 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; + } + } + + 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); + } + + } + +} diff --git a/src/lib/parameters/ParameterServer.hpp b/src/lib/parameters/ParameterServer.hpp new file mode 100644 index 0000000000..3c083e7272 --- /dev/null +++ b/src/lib/parameters/ParameterServer.hpp @@ -0,0 +1,560 @@ +/**************************************************************************** + * + * 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 "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); + + /** + * 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. + */ + void setParameterUsed(param_t param); + + /** + * 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 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, bool notify = true); + + /** + * 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); + + /** + * Read saved parameters from file and dump to console. + * + * This function reads the file and dumps all contents to console + * values from a file. + * + * @param fd File descriptor to read from. + * @return Zero on success, nonzero if an error occurred during import. + */ + int bsonDump(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; } + + /** + * 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(); + + /** + * 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); + + /** + * 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); + + // internal parameter export, caller is responsible for locking + int exportInternal(int fd, param_filter_func filter); + + /** + * 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(); + + + 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); + + static int bsonDumpCallbackTrampoline(bson_decoder_t decoder, void *priv, bson_node_t node); + int bsonDumpCallback(bson_decoder_t decoder, bson_node_t node); + + + 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}; + + /** parameter update topic handle */ + orb_advert_t param_topic = 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 param_export_perf; + perf_counter_t param_find_perf; + perf_counter_t param_get_perf; + perf_counter_t param_set_perf; + + 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 _param_response_pub{ORB_ID(parameter_value)}; + + uORB::Subscription _param_request_sub{ORB_ID(parameter_request)}; + + + // autosaving variables + hrt_abstime last_autosave_timestamp = 0; + px4::atomic_bool autosave_scheduled{false}; + bool autosave_disabled = false; + + + + /* + * The MAVLink parameter bridge needs to know the maximum parameter index + * of each node so that clients can determine when parameter listings have + * finished. We do that by querying a node's entire parameter set whenever + * a parameter message is received for a node with a zero _param_count, + * and storing the count here. If a node doesn't respond, or doesn't have + * any parameters, its count will stay at zero and we'll try to query the + * set again next time. + * + * The node's UAVCAN ID is used as the index into the _param_counts array. + */ + uint8_t _param_counts[128] {}; + bool _count_in_progress{false}; + uint8_t _count_index{0}; + + bool _param_in_progress{false}; + uint8_t _param_index{0}; + bool _param_list_in_progress{false}; + bool _param_list_all_nodes{false}; + uint8_t _param_list_node_id{1}; + + uint32_t _param_dirty_bitmap[4] {}; + uint8_t _param_save_opcode{0}; + + bool _cmd_in_progress{false}; + + +}; diff --git a/src/lib/parameters/flashparams/flashparams.cpp b/src/lib/parameters/flashparams/flashparams.cpp index 6eb8c9e8a2..7ba3f2c943 100644 --- a/src/lib/parameters/flashparams/flashparams.cpp +++ b/src/lib/parameters/flashparams/flashparams.cpp @@ -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 a7b669c1c0..e68eabbbaf 100644 --- a/src/lib/parameters/param.h +++ b/src/lib/parameters/param.h @@ -303,8 +303,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. * @@ -322,6 +320,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 0822842e45..d40161f95c 100644 --- a/src/lib/parameters/parameters.cpp +++ b/src/lib/parameters/parameters.cpp @@ -42,759 +42,209 @@ */ #define PARAM_IMPLEMENTATION + +#include "ParameterServer.hpp" +static ParameterServer *parameter_server {nullptr}; + #include "param.h" -#include "param_translation.h" -#include -#include "tinybson/tinybson.h" -#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) { + 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); +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; } -/** unlock the parameter store */ -static void -param_unlock_writer() +size_t param_size(param_t param) { - 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->getParameterSize(param); } 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) +unsigned param_count() { - 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); + if (parameter_server) { + return parameter_server->count(); } - 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); + return 0; } unsigned param_count_used() { - return params_active.count(); + if (parameter_server) { + return parameter_server->countUsed(); + } + + return 0; } -param_t param_for_used_index(unsigned index) +param_t param_for_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->forIndex(index); } return PARAM_INVALID; } -int param_get_used_index(param_t param) +param_t param_for_used_index(unsigned index) { - /* this tests for out of bounds and does a constant time lookup */ - if (!param_used(param)) { - return -1; + if (parameter_server) { + return parameter_server->forUsedIndex(index); } - /* walk all params and count, now knowing that it has a valid index */ - int used_count = 0; + return PARAM_INVALID; +} - for (int i = 0; i < params_active.size(); i++) { - if (params_active[i]) { - - if (param == i) { - return used_count; - } - - used_count++; - } +int param_get_index(param_t param) +{ + 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 (parameter_server) { + return parameter_server->isParameterVolatile(param); } - 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; + return false; } -int -param_get_default_value(param_t param, void *default_val) + +bool param_value_unsaved(param_t param) { - 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; - } + if (parameter_server) { + return parameter_server->isParameterValueUnsaved(param); } return true; } -/** - * worker callback method to save the parameters - * @param arg unused - */ -static void -autosave_worker(void *arg) +int param_get(param_t param, void *val) { - 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; - } + if (parameter_server) { + return parameter_server->getParameterValue(param, val); } - 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); - } + return -1; } -/** - * 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() +int param_get_default_value(param_t param, void *default_val) { - if (autosave_scheduled.load() || autosave_disabled) { - return; + if (parameter_server) { + parameter_server->getParameterDefaultValue(param, default_val); } - // 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)); + return -1; } -void -param_control_autosave(bool enable) +int param_get_system_default_value(param_t param, void *default_val) { - param_lock_writer(); - - if (!enable && autosave_scheduled.load()) { - work_cancel(LPWORK, &autosave_work); - autosave_scheduled.store(false); + if (parameter_server) { + parameter_server->getParameterSystemDefaultValue(param, default_val); } - autosave_disabled = !enable; - param_unlock_writer(); + return -1; } -static int -param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_changes) +bool param_value_is_default(param_t param) { - if (!handle_in_range(param)) { - PX4_ERR("set invalid param %d", param); - return PX4_ERROR; + if (parameter_server) { + parameter_server->isParameterValueDefault(param); } - 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; + return true; } -#if defined(FLASH_BASED_PARAMS) -int param_set_external(param_t param, const void *val, bool mark_saved, bool notify_changes) +void param_control_autosave(bool enable) { - return param_set_internal(param, val, mark_saved, notify_changes); + if (parameter_server) { + parameter_server->controlAutosave(enable); + } } -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); + if (parameter_server) { + return parameter_server->setParameter(param, val, false, true); + } + + return -1; } int param_set_no_notification(param_t param, const void *val) { - return param_set_internal(param, val, false, false); + if (parameter_server) { + return parameter_server->setParameter(param, val, false, false); + } + + return -1; } bool param_used(param_t param) { - if (handle_in_range(param)) { - return params_active[param]; + if (parameter_server) { + return parameter_server->isParameterUsed(param); } return false; @@ -802,949 +252,168 @@ bool param_used(param_t param) void param_set_used(param_t param) { - if (handle_in_range(param)) { - params_active.set(param, true); + if (parameter_server) { + return parameter_server->setParameterUsed(param); } } 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, bool notify = true) +int param_reset(param_t param) { - 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; + if (parameter_server) { + return parameter_server->resetParameter(param, true); } - param_autosave(); - - param_unlock_writer(); - - if (s != nullptr && notify) { - param_notify_changes(); - } - - return (!param_found); + return -1; } -int param_reset(param_t param) { return param_reset_internal(param, true); } -int param_reset_no_notification(param_t param) { return param_reset_internal(param, false); } - -static void -param_reset_all_internal(bool auto_save) +int param_reset_no_notification(param_t param) { - param_lock_writer(); - - if (param_values != nullptr) { - utarray_free(param_values); - - params_changed.reset(); + if (parameter_server) { + return parameter_server->resetParameter(param, false); } - /* mark as reset / deleted */ - param_values = nullptr; - - if (auto_save) { - param_autosave(); - } - - param_unlock_writer(); - - param_notify_changes(); + return -1; } -void -param_reset_all() +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(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(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) { - return 0; - } - - // find the parameter this node represents - param_t param = param_find_no_notification(node->name); - - 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); + if (parameter_server) { + return parameter_server->saveDefault(); } return -1; } -static int param_verify(int fd) +int param_load_default() { - 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->loadDefault(); } return -1; } -int -param_export(const char *filename, param_filter_func filter) +int param_export(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(¶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_dump_callback(bson_decoder_t decoder, bson_node_t node) -{ - switch (node->type) { - case BSON_EOO: - PX4_INFO_RAW("BSON_EOO\n"); - return 0; - - case BSON_DOUBLE: - PX4_INFO_RAW("BSON_DOUBLE: %s = %.6f\n", node->name, node->d); - return 1; - - case BSON_BOOL: - PX4_INFO_RAW("BSON_BOOL: %s = %d\n", node->name, node->b); - return 1; - - case BSON_INT32: - PX4_INFO_RAW("BSON_INT32: %s = %" PRIi32 "\n", node->name, node->i32); - return 1; - - case BSON_INT64: - PX4_INFO_RAW("BSON_INT64: %s = %" PRIi64 "\n", node->name, node->i64); - return 1; - - default: - PX4_INFO_RAW("ERROR %s unhandled bson type %d\n", node->name, node->type); - return 1; // just skip this entry + if (parameter_server) { + return parameter_server->exportToFile(filename, filter); } return -1; } -static int -param_import_internal(int fd) +int param_import(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(); - } - - return param_import_internal(fd); -} - -int -param_load(int fd) -{ - if (fd < 0) { - return flash_param_load(); - } - - param_reset_all_internal(false); - return param_import_internal(fd); -} - -int -param_dump(int fd) -{ - bson_decoder_s decoder{}; - - if (bson_decoder_init_file(&decoder, fd, param_dump_callback) == 0) { - PX4_INFO_RAW("BSON document size %" PRId32 "\n", decoder.total_document_size); - - int result = -1; - - do { - result = bson_decoder_next(&decoder); - - } while (result > 0); - - if (result == 0) { - PX4_INFO_RAW("BSON decoded %" PRId32 " bytes (double:%" PRIu16 ", string:%" PRIu16 ", bin:%" PRIu16 ", bool:%" PRIu16 - ", int32:%" PRIu16 ", int64:%" PRIu16 ")\n", - decoder.total_decoded_size, - decoder.count_node_double, decoder.count_node_string, decoder.count_node_bindata, decoder.count_node_bool, - decoder.count_node_int32, decoder.count_node_int64); - - return 0; - - } else if (result == -ENODATA) { - PX4_WARN("BSON: no data"); - return 0; - - } else { - PX4_ERR("param dump failed (%d)", result); - } + if (parameter_server) { + return parameter_server->loadFromFileDescriptor(fd); } return -1; } -void -param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_changed, bool only_used) +int param_dump(int fd) { - param_t param; + if (parameter_server) { + return parameter_server->bsonDump(fd); + } - for (param = 0; handle_in_range(param); param++) { + return -1; +} - /* if requested, skip unchanged values */ - if (only_changed && (param_find_changed(param) == nullptr)) { - continue; - } - - if (only_used && !param_used(param)) { - continue; - } - - func(arg, param); +void param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_changed, bool only_used) +{ + 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/tinybson/tinybson.cpp b/src/lib/parameters/tinybson/tinybson.cpp index 568bdd6816..e4e11fc0ff 100644 --- a/src/lib/parameters/tinybson/tinybson.cpp +++ b/src/lib/parameters/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/parameters/tinybson/tinybson.h b/src/lib/parameters/tinybson/tinybson.h index eb370454ee..96629061d6 100644 --- a/src/lib/parameters/tinybson/tinybson.h +++ b/src/lib/parameters/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/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index 1ffafb796e..75d1921581 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,29 @@ 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.float64_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; + parameter_request.type = parameter_request_s::TYPE_INT64; + parameter_request.int64_value = val; } - 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 +182,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 +215,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 +403,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 +419,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 +427,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.float64_value; } else { - int32_t val = (int32_t)value.int_value; + int32_t val = value.int64_value; memcpy(&msg.param_value, &val, sizeof(int32_t)); msg.param_type = MAVLINK_TYPE_INT32_T; } @@ -516,7 +517,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 +545,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 +552,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 +589,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 +597,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/tests/test_bson.cpp b/src/systemcmds/tests/test_bson.cpp index caaaa3474f..817db2d794 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; }