diff --git a/src/lib/parameters/param.h b/src/lib/parameters/param.h index 02a59cc184..177cf17b2d 100644 --- a/src/lib/parameters/param.h +++ b/src/lib/parameters/param.h @@ -210,7 +210,6 @@ __EXPORT size_t param_size(param_t param); * * @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. - * For structures, a bitwise copy of the structure is performed to this address. * @return Zero if the parameter's value could be returned, nonzero otherwise. */ __EXPORT int param_get(param_t param, void *val); @@ -220,7 +219,6 @@ __EXPORT int param_get(param_t param, void *val); * * @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. - * For structures, a bitwise copy of the structure is performed to this address. * @return Zero if the parameter's deafult value could be returned, nonzero otherwise. */ __EXPORT int param_get_default_value(param_t param, void *val); @@ -230,11 +228,19 @@ __EXPORT int param_get_default_value(param_t param, void *val); * * @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. * @return Zero if the parameter's value could be set from a scalar, nonzero otherwise. */ __EXPORT int param_set(param_t param, const void *val); +/** + * 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. + */ +__EXPORT int param_set_default_value(param_t param, const void *val); + /** * Mark a parameter as used. Only marked parameters will be sent to a GCS. * A call to param_find() will mark a param as used as well. @@ -248,7 +254,6 @@ __EXPORT void param_set_used(param_t param); * * @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. * @return Zero if the parameter's value could be set from a scalar, nonzero otherwise. */ __EXPORT int param_set_no_notification(param_t param, const void *val); @@ -262,9 +267,6 @@ __EXPORT void param_notify_changes(void); /** * Reset a parameter to its default value. * - * This function frees any storage used by struct parameters, and returns the 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 */ @@ -273,9 +275,6 @@ __EXPORT int param_reset(param_t param); /** * Reset a parameter to its default value, but do not notify the system about the change. * - * This function frees any storage used by struct parameters, and returns the 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 */ @@ -283,16 +282,12 @@ __EXPORT int param_reset_no_notification(param_t param); /** * Reset all parameters to their default values. - * - * This function also releases the storage used by struct parameters. */ __EXPORT void param_reset_all(void); /** * Reset all parameters to their default values except for excluded parameters. * - * This function also releases the storage used by struct 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. @@ -304,8 +299,6 @@ typedef bool(*param_filter_func)(param_t handle); /** * Reset only specific parameters to their default values. * - * This function also releases the storage used by struct parameters. - * * @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. */ diff --git a/src/lib/parameters/parameters.cpp b/src/lib/parameters/parameters.cpp index cdd0fd6971..582d24c4b2 100644 --- a/src/lib/parameters/parameters.cpp +++ b/src/lib/parameters/parameters.cpp @@ -55,6 +55,7 @@ #include #include #include +#include #include #include #include @@ -96,20 +97,21 @@ static px4::atomic autosave_scheduled{false}; static bool autosave_disabled = false; static constexpr uint16_t param_info_count = sizeof(px4::parameters) / sizeof(param_info_s); -static px4::Bitset params_active; // params found -static px4::Bitset params_changed; // params non-default +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 // Storage for modified parameters. struct param_wbuf_s { - union param_value_u val; - param_t param; - bool unsaved; + union param_value_u val; + param_t param; + bool unsaved; }; /** flexible array holding modified parameter values */ UT_array *param_values{nullptr}; +UT_array *param_custom_default_values{nullptr}; -/** array info for the modified parameters array */ const UT_icd param_icd = {sizeof(param_wbuf_s), nullptr, nullptr, nullptr}; /** parameter update topic handle */ @@ -456,28 +458,39 @@ size_t param_size(param_t param) static const void * param_get_value_ptr(param_t param) { - const void *result = nullptr; - param_assert_locked(); if (handle_in_range(param)) { - - const union param_value_u *v; - /* work out whether we're fetching the default or a written value */ struct param_wbuf_s *s = param_find_changed(param); if (s != nullptr) { - v = &s->val; + return &s->val; } else { - v = &px4::parameters[param].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); - result = v; + 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; + } + } } - return result; + return nullptr; } int @@ -487,14 +500,14 @@ param_get(param_t param, void *val) if (!handle_in_range(param)) { PX4_ERR("get: param %d invalid", param); - return -1; + return PX4_ERROR; } if (!params_active[param]) { PX4_WARN("get: param %d (%s) not active", param, param_name(param)); } - int result = -1; + int result = PX4_ERROR; if (val) { param_lock_reader(); @@ -503,7 +516,7 @@ param_get(param_t param, void *val) if (v) { memcpy(val, v, param_size(param)); - result = 0; + result = PX4_OK; } param_unlock_reader(); @@ -513,21 +526,48 @@ param_get(param_t param, void *val) } int -param_get_default_value(param_t param, void *default_val) +param_get_default_value_internal(param_t param, void *default_val) { - if (default_val && handle_in_range(param)) { + if (!handle_in_range(param)) { + PX4_ERR("get default value: param %d invalid", param); + return PX4_ERROR; + } + + if (default_val) { + if (params_custom_default[param] && param_custom_default_values) { + // get default from custom default storage + param_wbuf_s key{}; + key.param = param; + param_wbuf_s *pbuf = (param_wbuf_s *)utarray_find(param_custom_default_values, &key, param_compare_values); + + if (pbuf != nullptr) { + memcpy(default_val, &pbuf->val, param_size(param)); + return PX4_OK; + } + } + + // otherwise return static default value switch (param_type(param)) { case PARAM_TYPE_INT32: memcpy(default_val, &px4::parameters[param].val.i, param_size(param)); - return 0; + return PX4_OK; case PARAM_TYPE_FLOAT: memcpy(default_val, &px4::parameters[param].val.f, param_size(param)); - return 0; + return PX4_OK; } } - return -1; + return PX4_ERROR; +} + +int +param_get_default_value(param_t param, void *default_val) +{ + param_lock_reader(); + int ret = param_get_default_value_internal(param, default_val); + param_unlock_reader(); + return ret; } /** @@ -615,6 +655,16 @@ param_control_autosave(bool enable) static int param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_changes) { + if (!handle_in_range(param)) { + PX4_ERR("set invalid param %d", param); + return PX4_ERROR; + } + + if (val == nullptr) { + PX4_ERR("set invalid value"); + return PX4_ERROR; + } + int result = -1; bool param_changed = false; @@ -634,19 +684,28 @@ param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_ if (param_values == nullptr) { PX4_ERR("failed to allocate modified values array"); goto out; - } - if (handle_in_range(param)) { + } else { // check if param being set to default value bool set_to_default = false; switch (param_type(param)) { - case PARAM_TYPE_INT32: - set_to_default = (px4::parameters[param].val.i == *(int32_t *)val); + case PARAM_TYPE_INT32: { + int32_t default_val = 0; + + if (param_get_default_value_internal(param, &default_val) == PX4_OK) { + set_to_default = (default_val == *(int32_t *)val); + } + } break; - case PARAM_TYPE_FLOAT: - set_to_default = (fabsf(px4::parameters[param].val.f - * (float *)val) < FLT_EPSILON); + case PARAM_TYPE_FLOAT: { + float default_val = 0; + + if (param_get_default_value_internal(param, &default_val) == PX4_OK) { + set_to_default = (fabsf(default_val - * (float *)val) < FLT_EPSILON); + } + } break; } @@ -654,14 +713,17 @@ param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_ if (set_to_default) { if (s != nullptr) { - // param in memory and set to non-default value, clear + // param is being set non-default -> default, simply clear storage int pos = utarray_eltidx(param_values, s); utarray_erase(param_values, pos, 1); - params_changed.set(param, false); param_changed = true; + + } else { + // do nothing if param not already set and being set to default } - // do nothing if param not already set and being set to default + params_changed.set(param, false); + result = PX4_OK; } else { if (s == nullptr) { @@ -680,28 +742,32 @@ param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_ s = param_find_changed(param); } - /* update the changed value */ - switch (param_type(param)) { - case PARAM_TYPE_INT32: - param_changed = param_changed || s->val.i != *(int32_t *)val; - s->val.i = *(int32_t *)val; - break; + if (s != nullptr) { + /* update the changed value */ + switch (param_type(param)) { + case PARAM_TYPE_INT32: + param_changed = param_changed || s->val.i != *(int32_t *)val; + s->val.i = *(int32_t *)val; + s->unsaved = !mark_saved; + params_changed.set(param, true); + result = PX4_OK; + break; - case PARAM_TYPE_FLOAT: - param_changed = param_changed || fabsf(s->val.f - * (float *)val) > FLT_EPSILON; - s->val.f = *(float *)val; - break; + case PARAM_TYPE_FLOAT: + param_changed = param_changed || fabsf(s->val.f - * (float *)val) > FLT_EPSILON; + s->val.f = *(float *)val; + s->unsaved = !mark_saved; + params_changed.set(param, true); + result = PX4_OK; + break; - default: - goto out; + default: + break; + } } - - s->unsaved = !mark_saved; } - result = 0; - - if (!mark_saved) { // this is false when importing parameters + if ((result == PX4_OK) && !mark_saved) { // this is false when importing parameters param_autosave(); } } @@ -714,7 +780,7 @@ out: * If we set something, now that we have unlocked, go ahead and advertise that * a thing has been set. */ - if (param_changed && notify_changes) { + if ((result == PX4_OK) && param_changed && notify_changes) { param_notify_changes(); } @@ -759,6 +825,116 @@ void param_set_used(param_t 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 (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) + for (int i = 0; i < params_custom_default.size(); i++) { + params_custom_default.set(i, false); + } + } + + 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; +} + + static int param_reset_internal(param_t param, bool notify = true) { param_wbuf_s *s = nullptr; @@ -1038,6 +1214,11 @@ param_export(int fd, bool only_unsaved, param_filter_func filter) continue; } + // don't export default values + if (param_value_is_default(s->param)) { + continue; + } + s->unsaved = false; const char *name = param_name(s->param); @@ -1334,6 +1515,12 @@ void param_print_status() 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)) { diff --git a/src/systemcmds/param/param.cpp b/src/systemcmds/param/param.cpp index 80adb32c27..6baef0ab26 100644 --- a/src/systemcmds/param/param.cpp +++ b/src/systemcmds/param/param.cpp @@ -44,6 +44,7 @@ #include #include +#include #include #include #include @@ -92,6 +93,7 @@ static int do_show_index(const char *index, bool used_index); static void do_show_print(void *arg, param_t param); static void do_show_print_for_airframe(void *arg, param_t param); static int do_set(const char *name, const char *val, bool fail_on_not_found); +static int do_set_custom_default(const char *name, const char *val); static int do_compare(const char *name, char *vals[], unsigned comparisons, enum COMPARE_OPERATOR cmd_op, enum COMPARE_ERROR_LEVEL err_level); static int do_reset_all(const char *excludes[], int num_excludes); @@ -151,6 +153,10 @@ $ reboot PRINT_MODULE_USAGE_ARG(" ", "Parameter name and value to set", false); PRINT_MODULE_USAGE_ARG("fail", "If provided, let the command fail if param is not found", true); + PRINT_MODULE_USAGE_COMMAND_DESCR("set-default", "Set parameter default to a value"); + PRINT_MODULE_USAGE_ARG(" ", "Parameter name and value to set", false); + PRINT_MODULE_USAGE_ARG("fail", "If provided, let the command fail if param is not found", true); + PRINT_MODULE_USAGE_COMMAND_DESCR("compare", "Compare a param with a value. Command will succeed if equal"); PRINT_MODULE_USAGE_PARAM_FLAG('s', "If provided, silent errors if parameter doesn't exists", true); PRINT_MODULE_USAGE_ARG(" ", "Parameter name and value to compare", false); @@ -226,9 +232,11 @@ param_main(int argc, char *argv[]) } const char *default_file = param_get_default_file(); + if (default_file) { PX4_INFO("selected parameter default file %s", default_file); } + return 0; } @@ -250,6 +258,7 @@ param_main(int argc, char *argv[]) if (argc >= 4) { return do_show_quiet(argv[3]); } + } else { return do_show(argv[2], false); } @@ -285,11 +294,23 @@ param_main(int argc, char *argv[]) } } + if (!strcmp(argv[1], "set-default")) { + if (argc == 4) { + return do_set_custom_default(argv[2], argv[3]); + + } else { + PX4_ERR("not enough arguments.\nTry 'param set-default PARAM_NAME 3'"); + return 1; + } + } + if (!strcmp(argv[1], "compare")) { - if(argc >= 5 && !strcmp(argv[2], "-s")) { + if (argc >= 5 && !strcmp(argv[2], "-s")) { return do_compare(argv[3], &argv[4], argc - 4, COMPARE_OPERATOR::EQUAL, COMPARE_ERROR_LEVEL::SILENT); + } else if (argc >= 4) { return do_compare(argv[2], &argv[3], argc - 3, COMPARE_OPERATOR::EQUAL, COMPARE_ERROR_LEVEL::DO_ERROR); + } else { PX4_ERR("not enough arguments.\nTry 'param compare PARAM_NAME 3'"); return 1; @@ -297,10 +318,12 @@ param_main(int argc, char *argv[]) } if (!strcmp(argv[1], "greater")) { - if(argc >= 5 && !strcmp(argv[2], "-s")) { + if (argc >= 5 && !strcmp(argv[2], "-s")) { return do_compare(argv[3], &argv[4], argc - 4, COMPARE_OPERATOR::GREATER, COMPARE_ERROR_LEVEL::SILENT); + } else if (argc >= 4) { return do_compare(argv[2], &argv[3], argc - 3, COMPARE_OPERATOR::GREATER, COMPARE_ERROR_LEVEL::DO_ERROR); + } else { PX4_ERR("not enough arguments.\nTry 'param greater PARAM_NAME 3'"); return 1; @@ -329,6 +352,7 @@ param_main(int argc, char *argv[]) if (!strcmp(argv[1], "touch")) { if (argc >= 3) { return do_touch((const char **) &argv[2], argc - 2); + } else { PX4_ERR("not enough arguments."); return 1; @@ -399,6 +423,7 @@ static int do_load(const char *param_file_name) { int fd = -1; + if (param_file_name) { // passing NULL means to select the flash storage fd = open(param_file_name, O_RDONLY); @@ -409,6 +434,7 @@ do_load(const char *param_file_name) } int result = param_load(fd); + if (fd >= 0) { close(fd); } @@ -416,9 +442,11 @@ do_load(const char *param_file_name) if (result < 0) { if (param_file_name) { PX4_ERR("importing from '%s' failed (%i)", param_file_name, result); + } else { PX4_ERR("importing failed (%i)", result); } + return 1; } @@ -429,12 +457,14 @@ static int do_import(const char *param_file_name) { bool mark_saved = false; + if (param_file_name == nullptr) { param_file_name = param_get_default_file(); mark_saved = true; // if imported from default storage, mark as saved } int fd = -1; + if (param_file_name) { // passing NULL means to select the flash storage fd = open(param_file_name, O_RDONLY); @@ -445,6 +475,7 @@ do_import(const char *param_file_name) } int result = param_import(fd, mark_saved); + if (fd >= 0) { close(fd); } @@ -452,9 +483,11 @@ do_import(const char *param_file_name) if (result < 0) { if (param_file_name) { PX4_ERR("importing from '%s' failed (%i)", param_file_name, result); + } else { PX4_ERR("importing failed (%i)", result); } + return 1; } @@ -674,16 +707,19 @@ static void do_show_print_for_airframe(void *arg, param_t param) { // exceptions - const char* p_name = param_name(param); + const char *p_name = param_name(param); + if (!p_name || param_is_volatile(param)) { return; } + if (!strcmp(p_name, "SYS_AUTOSTART") || !strcmp(p_name, "SYS_AUTOCONFIG")) { return; } + if (!strncmp(p_name, "RC", 2) || !strncmp(p_name, "TC_", 3) || !strncmp(p_name, "CAL_", 4) || - !strncmp(p_name, "SENS_BOARD_", 11) || !strcmp(p_name, "SENS_DPRES_OFF") || - !strcmp(p_name, "MAV_TYPE")) { + !strncmp(p_name, "SENS_BOARD_", 11) || !strcmp(p_name, "SENS_DPRES_OFF") || + !strcmp(p_name, "MAV_TYPE")) { return; } @@ -697,6 +733,7 @@ do_show_print_for_airframe(void *arg, param_t param) PARAM_PRINT("%ld\n", (long)i); return; } + break; case PARAM_TYPE_FLOAT: @@ -704,6 +741,7 @@ do_show_print_for_airframe(void *arg, param_t param) PARAM_PRINT("%4.4f\n", (double)f); return; } + break; default: @@ -783,7 +821,62 @@ do_set(const char *name, const char *val, bool fail_on_not_found) } static int -do_compare(const char *name, char *vals[], unsigned comparisons, enum COMPARE_OPERATOR cmp_op, enum COMPARE_ERROR_LEVEL err_level) +do_set_custom_default(const char *name, const char *val) +{ + param_t param = param_find_no_notification(name); + + /* set nothing if parameter cannot be found */ + if (param == PARAM_INVALID) { + /* param not found - fail silenty in scripts as it prevents booting */ + PX4_ERR("Parameter %s not found.", name); + return PX4_ERROR; + } + + // Set parameter if type is known and conversion from string to value turns out fine + switch (param_type(param)) { + case PARAM_TYPE_INT32: { + int32_t i; + + if (param_get_default_value(param, &i) == PX4_OK) { + /* convert string */ + char *end; + int32_t newval = strtol(val, &end, 10); + + if ((i != newval) && (param_set_default_value(param, &newval) == PX4_OK)) { + PARAM_PRINT(" parameter default: %s %d -> %d\n", param_name(param), i, newval); + } + } + } + + break; + + case PARAM_TYPE_FLOAT: { + float f; + + if (param_get_default_value(param, &f) == PX4_OK) { + /* convert string */ + char *end; + float newval = strtod(val, &end); + + if ((fabsf(f - newval) > FLT_EPSILON) && (param_set_default_value(param, &newval) == PX4_OK)) { + PARAM_PRINT(" parameter default: %s %4.2f -> %4.2f\n", param_name(param), (double)f, (double)newval); + } + } + } + + break; + + default: + PX4_ERR("\n", 0 + param_type(param)); + return 1; + } + + return 0; +} + +static int +do_compare(const char *name, char *vals[], unsigned comparisons, enum COMPARE_OPERATOR cmp_op, + enum COMPARE_ERROR_LEVEL err_level) { int32_t i; float f; @@ -792,10 +885,10 @@ do_compare(const char *name, char *vals[], unsigned comparisons, enum COMPARE_OP /* set nothing if parameter cannot be found */ if (param == PARAM_INVALID) { /* param not found */ - if(err_level == COMPARE_ERROR_LEVEL::DO_ERROR) - { + if (err_level == COMPARE_ERROR_LEVEL::DO_ERROR) { PX4_ERR("Parameter %s not found", name); } + return 1; } @@ -888,5 +981,6 @@ do_touch(const char *params[], int num_params) PX4_ERR("param %s not found", params[i]); } } + return 0; }