From 1bfca24fa9c5e33bf401ae9669807eb9c7a2096c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 14 Mar 2023 09:38:59 +0100 Subject: [PATCH] refactor param: move autosave to px4::wq_configurations::lp_default work queue Changes initialization order as param_init now depends on wq manager --- platforms/nuttx/src/px4/common/px4_init.cpp | 5 +- platforms/posix/src/px4/common/px4_init.cpp | 4 +- src/lib/parameters/CMakeLists.txt | 2 +- src/lib/parameters/autosave.cpp | 119 ++++++++++++++++++++ src/lib/parameters/autosave.h | 63 +++++++++++ src/lib/parameters/parameters.cpp | 95 +++------------- 6 files changed, 202 insertions(+), 86 deletions(-) create mode 100644 src/lib/parameters/autosave.cpp create mode 100644 src/lib/parameters/autosave.h diff --git a/platforms/nuttx/src/px4/common/px4_init.cpp b/platforms/nuttx/src/px4/common/px4_init.cpp index fb371b9093..3331eab90e 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(); @@ -180,9 +178,10 @@ int px4_platform_init() #endif // CONFIG_FS_BINFS - 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/lib/parameters/CMakeLists.txt b/src/lib/parameters/CMakeLists.txt index d2cd47a129..f4fbeb0eb4 100644 --- a/src/lib/parameters/CMakeLists.txt +++ b/src/lib/parameters/CMakeLists.txt @@ -156,7 +156,7 @@ add_custom_target(parameters_header DEPENDS px4_parameters.hpp) set(SRCS) -list(APPEND SRCS parameters.cpp atomic_transaction.cpp) +list(APPEND SRCS parameters.cpp atomic_transaction.cpp autosave.cpp) if(BUILD_TESTING) list(APPEND SRCS param_translation_unit_tests.cpp) diff --git a/src/lib/parameters/autosave.cpp b/src/lib/parameters/autosave.cpp new file mode 100644 index 0000000000..7a1be1ae7c --- /dev/null +++ b/src/lib/parameters/autosave.cpp @@ -0,0 +1,119 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 "autosave.h" + +#include +#include +#include + +#include "param.h" +#include "atomic_transaction.h" + +using namespace time_literals; + +ParamAutosave::ParamAutosave() + : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) +{ +} + +void ParamAutosave::request() +{ + if (_scheduled.load() || _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_timestamp); + + if (last_save_elapsed < rate_limit && rate_limit > last_save_elapsed + delay) { + delay = rate_limit - last_save_elapsed; + } + + _scheduled.store(true); + ScheduleDelayed(delay); +} + +void ParamAutosave::enable(bool enable) +{ + AtomicTransaction transaction; + _disabled = !enable; + + if (!enable && _scheduled.load()) { + _scheduled.store(false); + px4::ScheduledWorkItem::ScheduleClear(); + } +} + +void ParamAutosave::Run() +{ + 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) { + ScheduleDelayed(1_s); + return; + } + } + + { + const AtomicTransaction transaction; + _last_timestamp = hrt_absolute_time(); + // Note that the order is important here: we first clear _scheduled, then save the parameters, as during export, + // more parameter changes could happen. + _scheduled.store(false); + disabled = _disabled; + } + + if (disabled) { + return; + } + + PX4_DEBUG("Autosaving params"); + int ret = param_save_default(); + + if (ret != 0) { + PX4_ERR("param auto save failed (%i)", ret); + } +} + diff --git a/src/lib/parameters/autosave.h b/src/lib/parameters/autosave.h new file mode 100644 index 0000000000..7a8857888e --- /dev/null +++ b/src/lib/parameters/autosave.h @@ -0,0 +1,63 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 + +class ParamAutosave : public px4::ScheduledWorkItem +{ +public: + + ParamAutosave(); + + /** + * Automatically save the parameters after a timeout and at limited rate. + */ + void request(); + + void enable(bool enable); + + bool enabled() const { return !_disabled; } + + hrt_abstime lastAutosave() const { return _last_timestamp; } + + void Run() override; + +private: + hrt_abstime _last_timestamp{0}; + px4::atomic_bool _scheduled{false}; + bool _disabled{false}; +}; diff --git a/src/lib/parameters/parameters.cpp b/src/lib/parameters/parameters.cpp index 918dbe25fb..9550979fc6 100644 --- a/src/lib/parameters/parameters.cpp +++ b/src/lib/parameters/parameters.cpp @@ -66,7 +66,6 @@ using namespace time_literals; #include "uORB/uORB.h" #include "uORB/topics/parameter_update.h" -#include #include #include "ExhaustiveLayer.h" @@ -95,12 +94,8 @@ inline static int flash_param_import() { return -1; } 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; +#include "autosave.h" +static ParamAutosave *autosave_instance {nullptr}; static px4::AtomicBitset params_active; // params found static px4::AtomicBitset params_unsaved; @@ -132,6 +127,8 @@ param_init() #if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) px4_register_boardct_ioctl(_PARAMIOCBASE, param_ioctl); #endif + + autosave_instance = new ParamAutosave(); } @@ -355,85 +352,19 @@ bool param_value_is_default(param_t param) return true; } -/** - * worker callback method to save the parameters - * @param arg unused - */ -static void -autosave_worker(void *arg) -{ - bool disabled = false; - - if (!param_get_default_file()) { - // In case we save to FLASH, defer param writes until disarmed, - // as writing to FLASH can stall the entire CPU (in rare cases around 300ms on STM32F7) - uORB::SubscriptionData armed_sub{ORB_ID(actuator_armed)}; - - if (armed_sub.get().armed) { - work_queue(LPWORK, &autosave_work, (worker_t)&autosave_worker, nullptr, USEC2TICK(1_s)); - return; - } - } - - { - const AtomicTransaction transaction; - last_autosave_timestamp = hrt_absolute_time(); - autosave_scheduled.store(false); - disabled = autosave_disabled; - } - - if (disabled) { - return; - } - - PX4_DEBUG("Autosaving params"); - int ret = param_save_default(); - - if (ret != 0) { - PX4_ERR("param auto save failed (%i)", ret); - } -} - -/** - * Automatically save the parameters after a timeout and limited rate. - * - * This needs to be called with the writer lock held (it's not necessary that it's the writer lock, but it - * needs to be the same lock as autosave_worker() and param_control_autosave() use). - */ static void param_autosave() { - if (autosave_scheduled.load() || autosave_disabled) { - return; + if (autosave_instance) { + autosave_instance->request(); } - - // wait at least 300ms before saving, because: - // - tasks often call param_set() for multiple params, so this avoids unnecessary save calls - // - the logger stores changed params. He gets notified on a param change via uORB and then - // looks at all unsaved params. - hrt_abstime delay = 300_ms; - - static constexpr const hrt_abstime rate_limit = 2_s; // rate-limit saving to 2 seconds - const hrt_abstime last_save_elapsed = hrt_elapsed_time(&last_autosave_timestamp); - - if (last_save_elapsed < rate_limit && rate_limit > last_save_elapsed + delay) { - delay = rate_limit - last_save_elapsed; - } - - autosave_scheduled.store(true); - work_queue(LPWORK, &autosave_work, (worker_t)&autosave_worker, nullptr, USEC2TICK(delay)); } void param_control_autosave(bool enable) { - AtomicTransaction transaction; - autosave_disabled = !enable; - - if (!enable && autosave_scheduled.load()) { - autosave_scheduled.store(false); - transaction.unlock(); - work_cancel(LPWORK, &autosave_work); + if (autosave_instance) { + autosave_instance->enable(enable); } } @@ -1325,10 +1256,14 @@ void param_print_status() PX4_INFO("storage array (custom defaults): %d/%d elements (%zu bytes total)", runtime_defaults.size(), firmware_defaults.size(), (size_t)runtime_defaults.byteSize()); - PX4_INFO("auto save: %s", autosave_disabled ? "off" : "on"); + if (autosave_instance) { + PX4_INFO("auto save: %s", autosave_instance->enabled() ? "on" : "off"); - if (!autosave_disabled && (last_autosave_timestamp > 0)) { - PX4_INFO("last auto save: %.3f seconds ago", hrt_elapsed_time(&last_autosave_timestamp) * 1e-6); + hrt_abstime last_autosave = autosave_instance->lastAutosave(); + + if (last_autosave > 0) { + PX4_INFO("last auto save: %.3f seconds ago", hrt_elapsed_time(&last_autosave) * 1e-6); + } } perf_print_counter(param_export_perf);