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);