From f4ebfa6130b158e431bc77d4de67fab67b58f9ed Mon Sep 17 00:00:00 2001 From: Eric Katzfey <53063038+katzfey@users.noreply.github.com> Date: Mon, 11 Mar 2024 10:52:22 -0700 Subject: [PATCH] parameters: support for an optional remote parameter database (#22836) The voxl2 has a split architecture. PX4 runs on a posix platform and a Qurt platform. The two communicate uorb topics back and forth with the muorb module. But each has it's own parameters database and they need to stay in sync with each other. This PR adds support to keep the 2 parameter databases in sync. The main parameters database running on Linux has file system support while the Qurt one does not. The Linux side is considered the primary and the Qurt side is considered the remote. --- boards/modalai/voxl2-slpi/default.px4board | 1 + boards/modalai/voxl2/default.px4board | 1 + msg/CMakeLists.txt | 4 + msg/ParameterResetRequest.msg | 8 + msg/ParameterSetUsedRequest.msg | 6 + msg/ParameterSetValueRequest.msg | 11 + msg/ParameterSetValueResponse.msg | 9 + platforms/common/uORB/uORBManager.cpp | 2 +- src/lib/Kconfig | 1 + src/lib/parameters/CMakeLists.txt | 18 +- src/lib/parameters/Kconfig | 11 + src/lib/parameters/param.h | 12 + src/lib/parameters/parameters.cpp | 101 ++++++- src/lib/parameters/parameters_primary.cpp | 314 +++++++++++++++++++++ src/lib/parameters/parameters_primary.h | 52 ++++ src/lib/parameters/parameters_remote.cpp | 310 ++++++++++++++++++++ src/lib/parameters/parameters_remote.h | 50 ++++ 17 files changed, 907 insertions(+), 4 deletions(-) create mode 100644 msg/ParameterResetRequest.msg create mode 100644 msg/ParameterSetUsedRequest.msg create mode 100644 msg/ParameterSetValueRequest.msg create mode 100644 msg/ParameterSetValueResponse.msg create mode 100644 src/lib/Kconfig create mode 100644 src/lib/parameters/Kconfig create mode 100644 src/lib/parameters/parameters_primary.cpp create mode 100644 src/lib/parameters/parameters_primary.h create mode 100644 src/lib/parameters/parameters_remote.cpp create mode 100644 src/lib/parameters/parameters_remote.h diff --git a/boards/modalai/voxl2-slpi/default.px4board b/boards/modalai/voxl2-slpi/default.px4board index 0182f68bab..73e75dd7bc 100644 --- a/boards/modalai/voxl2-slpi/default.px4board +++ b/boards/modalai/voxl2-slpi/default.px4board @@ -30,3 +30,4 @@ CONFIG_MODULES_SIMULATION_PWM_OUT_SIM=y CONFIG_SYSTEMCMDS_UORB=y CONFIG_SYSTEMCMDS_PARAM=y CONFIG_ORB_COMMUNICATOR=y +CONFIG_PARAM_REMOTE=y diff --git a/boards/modalai/voxl2/default.px4board b/boards/modalai/voxl2/default.px4board index bf42bff4c9..da04429b98 100644 --- a/boards/modalai/voxl2/default.px4board +++ b/boards/modalai/voxl2/default.px4board @@ -26,3 +26,4 @@ CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y CONFIG_SYSTEMCMDS_UORB=y CONFIG_SYSTEMCMDS_VER=y CONFIG_ORB_COMMUNICATOR=y +CONFIG_PARAM_PRIMARY=y diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 2f8bb98c80..0f31ffca2a 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -154,6 +154,10 @@ set(msg_files OrbTest.msg OrbTestLarge.msg OrbTestMedium.msg + ParameterResetRequest.msg + ParameterSetUsedRequest.msg + ParameterSetValueRequest.msg + ParameterSetValueResponse.msg ParameterUpdate.msg Ping.msg PositionControllerLandingStatus.msg diff --git a/msg/ParameterResetRequest.msg b/msg/ParameterResetRequest.msg new file mode 100644 index 0000000000..db08edb379 --- /dev/null +++ b/msg/ParameterResetRequest.msg @@ -0,0 +1,8 @@ +# ParameterResetRequest : Used by the primary to reset one or all parameter value(s) on the remote + +uint64 timestamp +uint16 parameter_index + +bool reset_all # If this is true then ignore parameter_index + +uint8 ORB_QUEUE_LENGTH = 4 diff --git a/msg/ParameterSetUsedRequest.msg b/msg/ParameterSetUsedRequest.msg new file mode 100644 index 0000000000..ca97f9e9e7 --- /dev/null +++ b/msg/ParameterSetUsedRequest.msg @@ -0,0 +1,6 @@ +# ParameterSetUsedRequest : Used by a remote to update the used flag for a parameter on the primary + +uint64 timestamp +uint16 parameter_index + +uint8 ORB_QUEUE_LENGTH = 64 diff --git a/msg/ParameterSetValueRequest.msg b/msg/ParameterSetValueRequest.msg new file mode 100644 index 0000000000..eaf650f243 --- /dev/null +++ b/msg/ParameterSetValueRequest.msg @@ -0,0 +1,11 @@ +# ParameterSetValueRequest : Used by a remote or primary to update the value for a parameter at the other end + +uint64 timestamp +uint16 parameter_index + +int32 int_value # Optional value for an integer parameter +float32 float_value # Optional value for a float parameter + +uint8 ORB_QUEUE_LENGTH = 32 + +# TOPICS parameter_set_value_request parameter_remote_set_value_request parameter_primary_set_value_request diff --git a/msg/ParameterSetValueResponse.msg b/msg/ParameterSetValueResponse.msg new file mode 100644 index 0000000000..09f8e3084a --- /dev/null +++ b/msg/ParameterSetValueResponse.msg @@ -0,0 +1,9 @@ +# ParameterSetValueResponse : Response to a set value request by either primary or secondary + +uint64 timestamp +uint64 request_timestamp +uint16 parameter_index + +uint8 ORB_QUEUE_LENGTH = 4 + +# TOPICS parameter_set_value_response parameter_remote_set_value_response parameter_primary_set_value_response \ No newline at end of file diff --git a/platforms/common/uORB/uORBManager.cpp b/platforms/common/uORB/uORBManager.cpp index 7e2fe4461a..3fa75a3454 100644 --- a/platforms/common/uORB/uORBManager.cpp +++ b/platforms/common/uORB/uORBManager.cpp @@ -630,7 +630,7 @@ int16_t uORB::Manager::process_remote_topic(const char *topic_name) // We didn't find a node so we need to create it via an advertisement PX4_DEBUG("Advertising remote topic %s", topic_name); _remote_topics.insert(topic_name); - orb_advertise(topic_ptr, nullptr, topic_ptr->o_queue); + orb_advertise(topic_ptr, nullptr); return 0; } diff --git a/src/lib/Kconfig b/src/lib/Kconfig new file mode 100644 index 0000000000..6353836988 --- /dev/null +++ b/src/lib/Kconfig @@ -0,0 +1 @@ +rsource "*/Kconfig" diff --git a/src/lib/parameters/CMakeLists.txt b/src/lib/parameters/CMakeLists.txt index a68a6e38ad..b87e652cad 100644 --- a/src/lib/parameters/CMakeLists.txt +++ b/src/lib/parameters/CMakeLists.txt @@ -156,7 +156,23 @@ add_custom_target(parameters_header DEPENDS px4_parameters.hpp) set(SRCS) -list(APPEND SRCS parameters.cpp atomic_transaction.cpp autosave.cpp) +list(APPEND SRCS + parameters.cpp + atomic_transaction.cpp + autosave.cpp +) + +if(CONFIG_PARAM_PRIMARY) +list(APPEND SRCS + parameters_primary.cpp +) +endif() + +if(CONFIG_PARAM_REMOTE) +list(APPEND SRCS + parameters_remote.cpp +) +endif() if(BUILD_TESTING) list(APPEND SRCS param_translation_unit_tests.cpp) diff --git a/src/lib/parameters/Kconfig b/src/lib/parameters/Kconfig new file mode 100644 index 0000000000..f296180d99 --- /dev/null +++ b/src/lib/parameters/Kconfig @@ -0,0 +1,11 @@ +menuconfig PARAM_PRIMARY + bool "parameter primary" + default n + ---help--- + Enable support for the parameter primary in distributed board architectures + +menuconfig PARAM_REMOTE + bool "parameter remote" + default n + ---help--- + Enable support for the parameter remote in distributed board architectures diff --git a/src/lib/parameters/param.h b/src/lib/parameters/param.h index dca29d4f21..c8b8dbd79d 100644 --- a/src/lib/parameters/param.h +++ b/src/lib/parameters/param.h @@ -267,6 +267,18 @@ __EXPORT void param_set_used(param_t param); */ __EXPORT int param_set_no_notification(param_t param, const void *val); +/** + * Set the value of a parameter, but do not update the remote system. This avoids + * a set loop between primary and remote. + * + * @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. + * For structures, the pointer is assumed to point to a structure to be copied. + * @param notify Set this to true for the primary (to send out a param update) and false on client + * @return Zero if the parameter's value could be set from a scalar, nonzero otherwise. + */ +__EXPORT int param_set_no_remote_update(param_t param, const void *val, bool notify); + /** * Notify the system about parameter changes. Can be used for example after several calls to * param_set_no_notification() to avoid unnecessary system notifications. diff --git a/src/lib/parameters/parameters.cpp b/src/lib/parameters/parameters.cpp index 0b20bd0fe7..f8cd3d6fa3 100644 --- a/src/lib/parameters/parameters.cpp +++ b/src/lib/parameters/parameters.cpp @@ -105,8 +105,10 @@ static DynamicSparseLayer runtime_defaults{&firmware_defaults}; DynamicSparseLayer user_config{&runtime_defaults}; /** parameter update topic handle */ +#if not defined(CONFIG_PARAM_REMOTE) static orb_advert_t param_topic = nullptr; static unsigned int param_instance = 0; +#endif static perf_counter_t param_export_perf; static perf_counter_t param_find_perf; @@ -116,6 +118,14 @@ static perf_counter_t param_set_perf; static pthread_mutex_t file_mutex = PTHREAD_MUTEX_INITIALIZER; ///< this protects against concurrent param saves (file or flash access). +// Support for remote parameter node +#if defined(CONFIG_PARAM_PRIMARY) +# include "parameters_primary.h" +#endif // CONFIG_PARAM_PRIMARY +#if defined(CONFIG_PARAM_REMOTE) +# include "parameters_remote.h" +#endif // CONFIG_PARAM_REMOTE + void param_init() { @@ -128,14 +138,27 @@ param_init() px4_register_boardct_ioctl(_PARAMIOCBASE, param_ioctl); #endif +#if defined(CONFIG_PARAM_PRIMARY) + param_primary_init(); +#endif // CONFIG_PARAM_PRIMARY + +#if defined(CONFIG_PARAM_REMOTE) + param_remote_init(); +#endif // CONFIG_PARAM_REMOTE + +#if not defined(CONFIG_PARAM_REMOTE) autosave_instance = new ParamAutosave(); +#endif } void param_notify_changes() { - parameter_update_s pup{}; +// Don't send if this is a remote node. Only the primary +// sends out update notices +#if not defined(CONFIG_PARAM_REMOTE) + 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); @@ -152,6 +175,8 @@ param_notify_changes() } else { orb_publish(ORB_ID(parameter_update), param_topic, &pup); } + +#endif } static param_t param_find_internal(const char *name, bool notification) @@ -372,7 +397,7 @@ param_control_autosave(bool enable) } static int -param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_changes) +param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_changes, bool update_remote = true) { if (!handle_in_range(param)) { PX4_ERR("set invalid param %d", param); @@ -421,6 +446,24 @@ param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_ param_autosave(); } + // If this is the parameter server, make sure that the remote is updated +#if defined(CONFIG_PARAM_PRIMARY) + + if (param_changed && update_remote) { + param_primary_set_value(param, val); + } + +#endif + + // If this is the parameter remote, make sure that the primary is updated +#if defined(CONFIG_PARAM_REMOTE) + + if (param_changed && update_remote) { + param_remote_set_value(param, val); + } + +#endif + perf_end(param_set_perf); /* @@ -456,6 +499,11 @@ int param_set_no_notification(param_t param, const void *val) return param_set_internal(param, val, false, false); } +int param_set_no_remote_update(param_t param, const void *val, bool notify) +{ + return param_set_internal(param, val, false, notify, false); +} + bool param_used(param_t param) { if (handle_in_range(param)) { @@ -468,6 +516,14 @@ bool param_used(param_t param) void param_set_used(param_t param) { if (handle_in_range(param)) { +#if defined(CONFIG_PARAM_REMOTE) + + if (!param_used(param)) { + param_remote_set_used(param); + } + +#endif + params_active.set(param, true); } } @@ -544,6 +600,11 @@ int param_set_default_value(param_t param, const void *val) static int param_reset_internal(param_t param, bool notify = true, bool autosave = true) { +#if defined(CONFIG_PARAM_REMOTE) + // Remote doesn't support reset + return false; +#endif + bool param_found = user_config.contains(param); if (handle_in_range(param)) { @@ -558,6 +619,10 @@ static int param_reset_internal(param_t param, bool notify = true, bool autosave param_notify_changes(); } +#if defined(CONFIG_PARAM_PRIMARY) + param_primary_reset(param); +#endif + return param_found; } @@ -567,6 +632,11 @@ int param_reset_no_notification(param_t param) { return param_reset_internal(par static void param_reset_all_internal(bool auto_save) { +#if defined(CONFIG_PARAM_REMOTE) + // Remote doesn't support reset + return; +#endif + for (param_t param = 0; handle_in_range(param); param++) { param_reset_internal(param, false, false); } @@ -575,6 +645,10 @@ param_reset_all_internal(bool auto_save) param_autosave(); } +#if defined(CONFIG_PARAM_PRIMARY) + param_primary_reset_all(); +#endif + param_notify_changes(); } @@ -1280,4 +1354,27 @@ void param_print_status() perf_print_counter(param_find_perf); perf_print_counter(param_get_perf); perf_print_counter(param_set_perf); + +#if defined(CONFIG_PARAM_PRIMARY) + struct param_primary_counters counts; + param_primary_get_counters(&counts); + PX4_INFO("set value requests received: %" PRIu32 ", set value responses sent: %" PRIu32, + counts.set_value_request_received, counts.set_value_response_sent); + PX4_INFO("set value requests sent: %" PRIu32 ", set value responses received: %" PRIu32, + counts.set_value_request_sent, counts.set_value_response_received); + PX4_INFO("resets sent: %" PRIu32 ", set used requests received: %" PRIu32, + counts.reset_sent, counts.set_used_received); +#endif + +#if defined(CONFIG_PARAM_REMOTE) + struct param_remote_counters counts; + param_remote_get_counters(&counts); + PX4_INFO("set value requests received: %" PRIu32 ", set value responses sent: %" PRIu32, + counts.set_value_request_received, counts.set_value_response_sent); + PX4_INFO("set value requests sent: %" PRIu32 ", set value responses received: %" PRIu32, + counts.set_value_request_sent, counts.set_value_response_received); + PX4_INFO("resets received: %" PRIu32 ", set used requests sent: %" PRIu32, + counts.reset_received, counts.set_used_sent); +#endif + } diff --git a/src/lib/parameters/parameters_primary.cpp b/src/lib/parameters/parameters_primary.cpp new file mode 100644 index 0000000000..02de11e608 --- /dev/null +++ b/src/lib/parameters/parameters_primary.cpp @@ -0,0 +1,314 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "parameters_primary.h" + +#include "uORB/uORBManager.hpp" + +#include +#include + +// uORB topics needed to keep parameter server and client in sync +#include +#include +#include +#include + +// Debug flag +static bool debug = false; + +static struct param_primary_counters param_primary_counters; + +#define TIMEOUT_WAIT 1000 +#define TIMEOUT_COUNT 50 + +static px4_task_t sync_thread_tid; +static const char *sync_thread_name = "param_primary_sync"; + +static orb_advert_t param_set_value_req_h = nullptr; +static orb_advert_t param_reset_req_h = nullptr; + +static int param_set_rsp_fd = PX4_ERROR; + +static int primary_sync_thread(int argc, char *argv[]) +{ + // Need to wait until the uORB and muORB are ready + // Check for uORB initialization with get_instance + while (uORB::Manager::get_instance() == nullptr) { px4_usleep(100); } + + // Check for muORB initialization with get_uorb_communicator + while (uORB::Manager::get_instance()->get_uorb_communicator() == nullptr) { px4_usleep(100); } + + orb_advert_t _set_value_rsp_h = nullptr; + + int _set_used_req_fd = orb_subscribe(ORB_ID(parameter_set_used_request)); + int _set_value_req_fd = orb_subscribe(ORB_ID(parameter_primary_set_value_request)); + + struct parameter_set_used_request_s _set_used_request; + struct parameter_set_value_request_s _set_value_request; + struct parameter_set_value_response_s _set_value_response; + + px4_pollfd_struct_t fds[2] = { { .fd = _set_used_req_fd, .events = POLLIN }, + { .fd = _set_value_req_fd, .events = POLLIN } + }; + + PX4_INFO("Starting parameter primary sync thread"); + + while (true) { + px4_poll(fds, 2, 1000); + + if (fds[0].revents & POLLIN) { + bool updated = true; + + while (updated) { + orb_copy(ORB_ID(parameter_set_used_request), _set_used_req_fd, &_set_used_request); + + if (debug) { + PX4_INFO("Got parameter_set_used_request for %s", param_name(_set_used_request.parameter_index)); + } + + param_primary_counters.set_used_received++; + + param_find(param_name(_set_used_request.parameter_index)); + + (void) orb_check(_set_used_req_fd, &updated); + } + + } + + if (fds[1].revents & POLLIN) { + bool updated = true; + + while (updated) { + orb_copy(ORB_ID(parameter_primary_set_value_request), _set_value_req_fd, &_set_value_request); + + if (debug) { + PX4_INFO("Got parameter_primary_set_value_request for %s", param_name(_set_value_request.parameter_index)); + } + + param_primary_counters.set_value_request_received++; + + param_t param = _set_value_request.parameter_index; + + switch (param_type(param)) { + case PARAM_TYPE_INT32: + param_set_no_remote_update(param, (const void *) &_set_value_request.int_value, true); + break; + + case PARAM_TYPE_FLOAT: + param_set_no_remote_update(param, (const void *) &_set_value_request.float_value, true); + break; + + default: + PX4_ERR("Parameter must be either int or float"); + break; + } + + _set_value_response.timestamp = hrt_absolute_time(); + _set_value_response.request_timestamp = _set_value_request.timestamp; + _set_value_response.parameter_index = _set_value_request.parameter_index; + + if (_set_value_rsp_h == nullptr) { + _set_value_rsp_h = orb_advertise(ORB_ID(parameter_primary_set_value_response), &_set_value_response); + + } else { + orb_publish(ORB_ID(parameter_primary_set_value_response), _set_value_rsp_h, &_set_value_response); + } + + param_primary_counters.set_value_response_sent++; + + (void) orb_check(_set_value_req_fd, &updated); + } + } + } + + return 0; +} + +void param_primary_init() +{ + + sync_thread_tid = px4_task_spawn_cmd(sync_thread_name, + SCHED_DEFAULT, + SCHED_PRIORITY_PARAMS, + (1024 * 4), + primary_sync_thread, + NULL); + +} + +// void param_primary_set_value(param_t param, const void *val, bool from_file) +void param_primary_set_value(param_t param, const void *val) +{ + bool send_request = true; + struct parameter_set_value_request_s req; + req.timestamp = hrt_absolute_time(); + req.parameter_index = param; + + switch (param_type(param)) { + case PARAM_TYPE_INT32: + req.int_value = *(int32_t *)val; + + if (debug) { + PX4_INFO("*** Setting %s to %d ***", param_name(req.parameter_index), req.int_value); + } + + break; + + case PARAM_TYPE_FLOAT: + req.float_value = *(float *)val; + + if (debug) { + PX4_INFO("*** Setting %s to %f ***", param_name(req.parameter_index), (double) req.float_value); + } + + break; + + default: + PX4_ERR("Parameter must be either int or float"); + send_request = false; + break; + } + + if (param_set_rsp_fd == PX4_ERROR) { + if (debug) { + PX4_INFO("Subscribing to parameter_client_set_value_response"); + } + + param_set_rsp_fd = orb_subscribe(ORB_ID(parameter_remote_set_value_response)); + + if (param_set_rsp_fd == PX4_ERROR) { + PX4_ERR("Subscription to parameter_remote_set_value_response failed"); + + } else { + if (debug) { + PX4_INFO("Subscription to parameter_client_set_value_response succeeded"); + } + } + } + + if (send_request) { + if (debug) { + PX4_INFO("Sending param set request to remote for %s", param_name(req.parameter_index)); + } + + if (param_set_value_req_h == nullptr) { + param_set_value_req_h = orb_advertise(ORB_ID(parameter_remote_set_value_request), nullptr); + } + + orb_publish(ORB_ID(parameter_remote_set_value_request), param_set_value_req_h, &req); + + param_primary_counters.set_value_request_sent++; + + // Wait for response + bool updated = false; + + if (debug) { + PX4_INFO("Waiting for parameter_client_set_value_response for %s", param_name(req.parameter_index)); + } + + px4_usleep(TIMEOUT_WAIT); + int count = TIMEOUT_COUNT; + + while (--count) { + (void) orb_check(param_set_rsp_fd, &updated); + + struct parameter_set_value_response_s rsp; + + while (updated) { + + orb_copy(ORB_ID(parameter_remote_set_value_response), param_set_rsp_fd, &rsp); + + if ((rsp.request_timestamp == req.timestamp) && (rsp.parameter_index == req.parameter_index)) { + if (debug) { + PX4_INFO("Got parameter_remote_set_value_response for %s", param_name(req.parameter_index)); + } + + param_primary_counters.set_value_response_received++; + return; + } + + (void) orb_check(param_set_rsp_fd, &updated); + } + + px4_usleep(TIMEOUT_WAIT); + } + + PX4_ERR("Timeout waiting for parameter_client_set_value_response for %s", param_name(req.parameter_index)); + } +} + +static void param_primary_reset_internal(param_t param, bool reset_all) +{ + if (debug) { + PX4_INFO("Param reset at primary"); + } + + struct parameter_reset_request_s req; + + req.timestamp = hrt_absolute_time(); + + req.reset_all = reset_all; + + if (reset_all == false) { + req.parameter_index = param; + } + + if (debug) { + PX4_INFO("Sending param reset request to remote"); + } + + if (param_reset_req_h == nullptr) { + param_reset_req_h = orb_advertise(ORB_ID(parameter_reset_request), &req); + + } else { + orb_publish(ORB_ID(parameter_reset_request), param_reset_req_h, &req); + } + + param_primary_counters.reset_sent++; +} + +void param_primary_reset(param_t param) +{ + param_primary_reset_internal(param, false); +} + +void param_primary_reset_all() +{ + param_primary_reset_internal(0, true); +} + +void param_primary_get_counters(struct param_primary_counters *cnt) +{ + *cnt = param_primary_counters; +} \ No newline at end of file diff --git a/src/lib/parameters/parameters_primary.h b/src/lib/parameters/parameters_primary.h new file mode 100644 index 0000000000..7994822d4c --- /dev/null +++ b/src/lib/parameters/parameters_primary.h @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "param.h" + +struct param_primary_counters { + uint32_t set_value_request_received; + uint32_t set_value_response_sent; + uint32_t reset_sent; + uint32_t set_value_request_sent; + uint32_t set_value_response_received; + uint32_t set_used_received; +}; + +void param_primary_init(); +void param_primary_set_value(param_t param, const void *val); +void param_primary_reset(param_t param); +void param_primary_reset_all(); +void param_primary_get_counters(struct param_primary_counters *cnt); + diff --git a/src/lib/parameters/parameters_remote.cpp b/src/lib/parameters/parameters_remote.cpp new file mode 100644 index 0000000000..78b7ee153c --- /dev/null +++ b/src/lib/parameters/parameters_remote.cpp @@ -0,0 +1,310 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "parameters_remote.h" + +#include "uORB/uORBManager.hpp" + +#include + +#include +#include + +// uORB topics needed to keep parameter server and client in sync +#include +#include +#include +#include + +// Debug flag +static bool debug = false; + +static struct param_remote_counters param_remote_counters; + +#define TIMEOUT_WAIT 1000 +#define TIMEOUT_COUNT 50 + +static orb_advert_t parameter_set_used_h = nullptr; +static orb_advert_t param_set_value_req_h = nullptr; + +static int param_set_rsp_fd = PX4_ERROR; + +static px4_task_t sync_thread_tid; +static const char *sync_thread_name = "param_remote_sync"; + +static int remote_sync_thread(int argc, char *argv[]) +{ + // This thread gets started by the remote side during PX4 initialization. + // We cannot send out the subscribe request immediately because the other + // side will not be ready to receive it on the muorb yet and it will get dropped. + // So, sleep a little bit to give other side a chance to finish initialization + // of the muorb. But don't wait too long otherwise a set request from the server + // side could be missed. + usleep(200000); + + orb_advert_t _set_value_rsp_h = nullptr; + + int _reset_req_fd = orb_subscribe(ORB_ID(parameter_reset_request)); + int _set_value_req_fd = orb_subscribe(ORB_ID(parameter_remote_set_value_request)); + + struct parameter_reset_request_s _reset_request; + struct parameter_set_value_request_s _set_value_request; + struct parameter_set_value_response_s _set_value_response; + + px4_pollfd_struct_t fds[2] = { { .fd = _reset_req_fd, .events = POLLIN }, + { .fd = _set_value_req_fd, .events = POLLIN } + }; + + PX4_INFO("Starting parameter remote sync thread"); + + while (true) { + px4_poll(fds, 2, 1000); + + if (fds[0].revents & POLLIN) { + bool updated = true; + + while (updated) { + orb_copy(ORB_ID(parameter_reset_request), _reset_req_fd, &_reset_request); + + if (debug) { + PX4_INFO("Got parameter_reset_request for %s", param_name(_reset_request.parameter_index)); + } + + param_remote_counters.reset_received++; + + if (_reset_request.reset_all) { + param_reset_all(); + + } else { + param_reset_no_notification(_reset_request.parameter_index); + + } + + (void) orb_check(_reset_req_fd, &updated); + } + } + + if (fds[1].revents & POLLIN) { + bool updated = true; + + while (updated) { + orb_copy(ORB_ID(parameter_primary_set_value_request), _set_value_req_fd, &_set_value_request); + + if (debug) { + PX4_INFO("Got parameter_remote_set_value_request for %s", param_name(_set_value_request.parameter_index)); + } + + param_remote_counters.set_value_request_received++; + + switch (param_type(_set_value_request.parameter_index)) { + case PARAM_TYPE_INT32: + param_set_no_remote_update(_set_value_request.parameter_index, + (const void *) &_set_value_request.int_value, + false); + break; + + case PARAM_TYPE_FLOAT: + param_set_no_remote_update(_set_value_request.parameter_index, + (const void *) &_set_value_request.float_value, + false); + break; + + default: + PX4_ERR("Parameter must be either int or float"); + break; + } + + _set_value_response.timestamp = hrt_absolute_time(); + _set_value_response.request_timestamp = _set_value_request.timestamp; + _set_value_response.parameter_index = _set_value_request.parameter_index; + + if (_set_value_rsp_h == nullptr) { + _set_value_rsp_h = orb_advertise(ORB_ID(parameter_remote_set_value_response), &_set_value_response); + + } else { + if (debug) { + PX4_INFO("Sending set value response for %s", param_name(_set_value_request.parameter_index)); + } + + orb_publish(ORB_ID(parameter_remote_set_value_response), _set_value_rsp_h, &_set_value_response); + } + + param_remote_counters.set_value_response_sent++; + + (void) orb_check(_set_value_req_fd, &updated); + } + } + } + + return 0; +} + +void param_remote_init() +{ + + sync_thread_tid = px4_task_spawn_cmd(sync_thread_name, + SCHED_DEFAULT, + SCHED_PRIORITY_PARAMS, + (1024 * 4), + remote_sync_thread, + NULL); + +} + +void param_remote_set_used(param_t param) +{ + // Notify the parameter server that this parameter has been marked as used + if (debug) { + PX4_INFO("Requesting server to mark %s as used", param_name(param)); + } + + struct parameter_set_used_request_s req; + + req.timestamp = hrt_absolute_time(); + + req.parameter_index = param; + + if (parameter_set_used_h == nullptr) { + parameter_set_used_h = orb_advertise(ORB_ID(parameter_set_used_request), &req); + + } else { + orb_publish(ORB_ID(parameter_set_used_request), parameter_set_used_h, &req); + } + + param_remote_counters.set_used_sent++; +} + +void param_remote_set_value(param_t param, const void *val) +{ + bool send_request = true; + struct parameter_set_value_request_s req; + req.timestamp = hrt_absolute_time(); + req.parameter_index = param; + + switch (param_type(param)) { + case PARAM_TYPE_INT32: + req.int_value = *(int32_t *)val; + + if (debug) { + PX4_INFO("*** Setting %s to %" PRIi32 " ***", param_name(req.parameter_index), req.int_value); + } + + break; + + case PARAM_TYPE_FLOAT: + req.float_value = *(float *)val; + + if (debug) { + PX4_INFO("*** Setting %s to %f ***", param_name(req.parameter_index), (double) req.float_value); + } + + break; + + default: + PX4_ERR("Parameter must be either int or float"); + send_request = false; + break; + } + + if (param_set_rsp_fd == PX4_ERROR) { + if (debug) { + PX4_INFO("Subscribing to parameter_primary_set_value_response"); + } + + param_set_rsp_fd = orb_subscribe(ORB_ID(parameter_primary_set_value_response)); + + if (param_set_rsp_fd == PX4_ERROR) { + PX4_ERR("Subscription to parameter_primary_set_value_response failed"); + + } else { + if (debug) { + PX4_INFO("Subscription to parameter_primary_set_value_response succeeded"); + } + } + } + + if (send_request) { + if (debug) { + PX4_INFO("Sending param set value request to primary for %s", param_name(req.parameter_index)); + } + + if (param_set_value_req_h == nullptr) { + param_set_value_req_h = orb_advertise(ORB_ID(parameter_primary_set_value_request), nullptr); + } + + orb_publish(ORB_ID(parameter_primary_set_value_request), param_set_value_req_h, &req); + + param_remote_counters.set_value_request_sent++; + + // Wait for response + bool updated = false; + + if (debug) { + PX4_INFO("Waiting for parameter_client_set_value_response for %s", param_name(req.parameter_index)); + } + + px4_usleep(TIMEOUT_WAIT); + int count = TIMEOUT_COUNT; + + while (--count) { + (void) orb_check(param_set_rsp_fd, &updated); + + struct parameter_set_value_response_s rsp; + + while (updated) { + + orb_copy(ORB_ID(parameter_primary_set_value_response), param_set_rsp_fd, &rsp); + + if ((rsp.request_timestamp == req.timestamp) && (rsp.parameter_index == req.parameter_index)) { + if (debug) { + PX4_INFO("Got parameter_primary_set_value_response for %s", param_name(req.parameter_index)); + } + + param_remote_counters.set_value_response_received++; + return; + } + + (void) orb_check(param_set_rsp_fd, &updated); + } + + px4_usleep(TIMEOUT_WAIT); + } + + PX4_ERR("Timeout waiting for parameter_primary_set_value_response for %s", param_name(req.parameter_index)); + } +} + +void param_remote_get_counters(struct param_remote_counters *cnt) +{ + *cnt = param_remote_counters; +} diff --git a/src/lib/parameters/parameters_remote.h b/src/lib/parameters/parameters_remote.h new file mode 100644 index 0000000000..7eba416643 --- /dev/null +++ b/src/lib/parameters/parameters_remote.h @@ -0,0 +1,50 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "param.h" + +struct param_remote_counters { + uint32_t set_value_request_received; + uint32_t set_value_response_sent; + uint32_t reset_received; + uint32_t set_value_request_sent; + uint32_t set_value_response_received; + uint32_t set_used_sent; +}; + +void param_remote_init(); +void param_remote_set_used(param_t param); +void param_remote_set_value(param_t param, const void *val); +void param_remote_get_counters(struct param_remote_counters *cnt);