forked from Archive/PX4-Autopilot
parameters runtime defaults
This commit is contained in:
parent
da1a38b44e
commit
b8b13bb882
|
@ -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.
|
||||
*/
|
||||
|
|
|
@ -55,6 +55,7 @@
|
|||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/atomic_bitset.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
#include <px4_platform_common/posix.h>
|
||||
#include <px4_platform_common/sem.h>
|
||||
|
@ -96,20 +97,21 @@ static px4::atomic<bool> 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<param_info_count> params_active; // params found
|
||||
static px4::Bitset<param_info_count> params_changed; // params non-default
|
||||
static px4::AtomicBitset<param_info_count> params_active; // params found
|
||||
static px4::AtomicBitset<param_info_count> params_changed; // params non-default
|
||||
static px4::Bitset<param_info_count> 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)) {
|
||||
|
|
|
@ -44,6 +44,7 @@
|
|||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/posix.h>
|
||||
|
||||
#include <float.h>
|
||||
#include <errno.h>
|
||||
#include <stdio.h>
|
||||
#include <ctype.h>
|
||||
|
@ -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("<param_name> <value>", "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("<param_name> <value>", "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("<param_name> <value>", "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("<unknown / unsupported type %d>\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;
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue