diff options
-rw-r--r-- | src/modules/mavlink/mavlink_parameters.cpp | 4 | ||||
-rw-r--r-- | src/modules/systemlib/param/param.c | 64 | ||||
-rw-r--r-- | src/modules/systemlib/param/param.h | 14 | ||||
-rw-r--r-- | src/systemcmds/param/param.c | 2 |
4 files changed, 75 insertions, 9 deletions
diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index e9858b73c..e910f674a 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -94,7 +94,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) /* enforce null termination */ name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; /* attempt to find parameter, set and send it */ - param_t param = param_find(name); + param_t param = param_find_no_notification(name); if (param == PARAM_INVALID) { char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; @@ -127,7 +127,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) /* enforce null termination */ name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; /* attempt to find parameter and send it */ - send_param(param_find(name)); + send_param(param_find_no_notification(name)); } else { /* when index is >= 0, send this parameter again */ diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index d0d24960e..ae680eeb0 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -91,6 +91,9 @@ struct param_wbuf_s { bool unsaved; }; +// XXX this should be param_info_count, but need to work out linking +uint8_t param_changed_storage[(600 / sizeof(uint8_t)) + 1] = {}; + /** flexible array holding modified parameter values */ UT_array *param_values; @@ -103,6 +106,12 @@ ORB_DEFINE(parameter_update, struct parameter_update_s); /** parameter update topic handle */ static orb_advert_t param_topic = -1; +static bool param_used_internal(param_t param); + +static void param_set_used_internal(param_t param); + +static param_t param_find_internal(const char *name, bool notification); + /** lock the parameter store */ static void param_lock(void) @@ -205,20 +214,36 @@ param_notify_changes(void) } param_t -param_find(const char *name) +param_find_internal(const char *name, bool notification) { param_t param; /* perform a linear search of the known parameters */ for (param = 0; handle_in_range(param); param++) { - if (!strcmp(param_info_base[param].name, name)) + if (!strcmp(param_info_base[param].name, name)) { + if (notification) { + param_set_used_internal(param); + } return param; + } } /* not found */ return PARAM_INVALID; } +param_t +param_find(const char *name) +{ + return param_find_internal(name, true); +} + +param_t +param_find_no_notification(const char *name) +{ + return param_find_internal(name, false); +} + unsigned param_count(void) { @@ -430,6 +455,8 @@ param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_ } out: + param_set_used_internal(param); + param_unlock(); /* @@ -454,6 +481,28 @@ param_set_no_notification(param_t param, const void *val) return param_set_internal(param, val, false, false); } +bool param_used_internal(param_t param) +{ + int param_index = param_get_index(param); + if (param_index < 0) { + return false; + } + + unsigned bitindex = param_index - (param_index / sizeof(param_changed_storage[0])); + return param_changed_storage[param_index / sizeof(param_changed_storage[0])] & (1 << bitindex); +} + +void param_set_used_internal(param_t param) +{ + int param_index = param_get_index(param); + if (param_index < 0) { + return; + } + + unsigned bitindex = param_index - (param_index / sizeof(param_changed_storage[0])); + param_changed_storage[param_index / sizeof(param_changed_storage[0])] |= (1 << bitindex); +} + int param_reset(param_t param) { @@ -717,7 +766,7 @@ param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node) * Find the parameter this node represents. If we don't know it, * ignore the node. */ - param_t param = param_find(node->name); + param_t param = param_find_no_notification(node->name); if (param == PARAM_INVALID) { debug("ignoring unrecognised parameter '%s'", node->name); @@ -843,15 +892,20 @@ param_load(int fd) } void -param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_changed) +param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_changed, bool only_used) { param_t param; for (param = 0; handle_in_range(param); param++) { /* if requested, skip unchanged values */ - if (only_changed && (param_find_changed(param) == NULL)) + if (only_changed && (param_find_changed(param) == NULL)) { continue; + } + + if (only_used && !param_used_internal(param)) { + continue; + } func(arg, param); } diff --git a/src/modules/systemlib/param/param.h b/src/modules/systemlib/param/param.h index 69e984a8f..e3e1b50eb 100644 --- a/src/modules/systemlib/param/param.h +++ b/src/modules/systemlib/param/param.h @@ -87,10 +87,20 @@ typedef uintptr_t param_t; * * @param name The canonical name of the parameter being looked up. * @return A handle to the parameter, or PARAM_INVALID if the parameter does not exist. + * This call will also set the parameter as "used" in the system, which is used + * to e.g. show the parameter via the RC interface */ __EXPORT param_t param_find(const char *name); /** + * Look up a parameter by name. + * + * @param name The canonical name of the parameter being looked up. + * @return A handle to the parameter, or PARAM_INVALID if the parameter does not exist. + */ +__EXPORT param_t param_find_no_notification(const char *name); + +/** * Return the total number of parameters. * * @return The number of parameters. @@ -254,8 +264,10 @@ __EXPORT int param_load(int fd); * @param arg Argument passed to the function. * @param only_changed If true, the function is only called for parameters whose values have * been changed from the default. + * @param only_changed If true, the function is only called for parameters which have been + * used in one of the running applications. */ -__EXPORT void param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_changed); +__EXPORT void param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_changed, bool only_used); /** * Set the default parameter file name. diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index c15e042a7..36a00c0f0 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -236,7 +236,7 @@ static void do_show(const char *search_string) { printf(" + = saved, * = unsaved\n"); - param_foreach(do_show_print, (char *)search_string, false); + param_foreach(do_show_print, (char *)search_string, false, false); exit(0); } |