diff options
Diffstat (limited to 'apps/systemlib/param/param.c')
| -rw-r--r-- | apps/systemlib/param/param.c | 805 |
1 files changed, 0 insertions, 805 deletions
diff --git a/apps/systemlib/param/param.c b/apps/systemlib/param/param.c deleted file mode 100644 index 8073570d1..000000000 --- a/apps/systemlib/param/param.c +++ /dev/null @@ -1,805 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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. - * - ****************************************************************************/ - -/** - * @file param.c - * - * Global parameter store. - * - * Note that it might make sense to convert this into a driver. That would - * offer some interesting options regarding state for e.g. ORB advertisements - * and background parameter saving. - */ - -#include <debug.h> -#include <string.h> -#include <stdbool.h> -#include <fcntl.h> -#include <unistd.h> -#include <err.h> -#include <errno.h> - -#include <sys/stat.h> - -#include <drivers/drv_hrt.h> - -#include "systemlib/param/param.h" -#include "systemlib/uthash/utarray.h" -#include "systemlib/bson/tinybson.h" - -#include "uORB/uORB.h" -#include "uORB/topics/parameter_update.h" - -#if 1 -# define debug(fmt, args...) do { warnx(fmt, ##args); } while(0) -#else -# define debug(fmt, args...) do { } while(0) -#endif - -/** - * Array of static parameter info. - */ -extern char __param_start, __param_end; -static const struct param_info_s *param_info_base = (struct param_info_s *) &__param_start; -static const struct param_info_s *param_info_limit = (struct param_info_s *) &__param_end; -#define param_info_count ((unsigned)(param_info_limit - param_info_base)) - -/** - * Storage for modified parameters. - */ -struct param_wbuf_s { - param_t param; - union param_value_u val; - bool unsaved; -}; - -/** flexible array holding modified parameter values */ -UT_array *param_values; - -/** array info for the modified parameters array */ -const UT_icd param_icd = {sizeof(struct param_wbuf_s), NULL, NULL, NULL}; - -/** parameter update topic */ -ORB_DEFINE(parameter_update, struct parameter_update_s); - -/** parameter update topic handle */ -static orb_advert_t param_topic = -1; - -/** lock the parameter store */ -static void -param_lock(void) -{ - /* XXX */ -} - -/** unlock the parameter store */ -static void -param_unlock(void) -{ - /* XXX */ -} - -/** assert that the parameter store is locked */ -static void -param_assert_locked(void) -{ - /* XXX */ -} - -/** - * Test whether a param_t is value. - * - * @param param The parameter handle to test. - * @return True if the handle is valid. - */ -static bool -handle_in_range(param_t param) -{ - return (param < param_info_count); -} - -/** - * Compare two modifid parameter structures to determine ordering. - * - * This function is suitable for passing to qsort or bsearch. - */ -static int -param_compare_values(const void *a, const void *b) -{ - struct param_wbuf_s *pa = (struct param_wbuf_s *)a; - struct param_wbuf_s *pb = (struct param_wbuf_s *)b; - - if (pa->param < pb->param) - return -1; - - if (pa->param > pb->param) - return 1; - - return 0; -} - -/** - * Locate the modified parameter structure for a parameter, if it exists. - * - * @param param The parameter being searched. - * @return The structure holding the modified value, or - * NULL if the parameter has not been modified. - */ -static struct param_wbuf_s * -param_find_changed(param_t param) { - struct param_wbuf_s *s = NULL; - - param_assert_locked(); - - if (param_values != NULL) { -#if 0 /* utarray_find requires bsearch, not available */ - struct param_wbuf_s key; - key.param = param; - s = utarray_find(param_values, &key, param_compare_values); -#else - - while ((s = (struct param_wbuf_s *)utarray_next(param_values, s)) != NULL) { - if (s->param == param) - break; - } - -#endif - } - - return s; -} - -static void -param_notify_changes(void) -{ - struct parameter_update_s pup = { .timestamp = hrt_absolute_time() }; - - /* - * If we don't have a handle to our topic, create one now; otherwise - * just publish. - */ - if (param_topic == -1) { - param_topic = orb_advertise(ORB_ID(parameter_update), &pup); - - } else { - orb_publish(ORB_ID(parameter_update), param_topic, &pup); - } -} - -param_t -param_find(const char *name) -{ - 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)) - return param; - } - - /* not found */ - return PARAM_INVALID; -} - -unsigned -param_count(void) -{ - return param_info_count; -} - -param_t -param_for_index(unsigned index) -{ - if (index < param_info_count) - return (param_t)index; - - return PARAM_INVALID; -} - -int -param_get_index(param_t param) -{ - if (handle_in_range(param)) - return (unsigned)param; - - return -1; -} - -const char * -param_name(param_t param) -{ - if (handle_in_range(param)) - return param_info_base[param].name; - - return NULL; -} - -bool -param_value_is_default(param_t param) -{ - return param_find_changed(param) ? false : true; -} - -bool -param_value_unsaved(param_t param) -{ - static struct param_wbuf_s *s; - - s = param_find_changed(param); - - if (s && s->unsaved) - return true; - - return false; -} - -enum param_type_e -param_type(param_t param) -{ - if (handle_in_range(param)) - return param_info_base[param].type; - - return PARAM_TYPE_UNKNOWN; -} - -size_t -param_size(param_t param) -{ - if (handle_in_range(param)) { - switch (param_type(param)) { - case PARAM_TYPE_INT32: - case PARAM_TYPE_FLOAT: - return 4; - - case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: - /* decode structure size from type value */ - return param_type(param) - PARAM_TYPE_STRUCT; - - default: - return 0; - } - } - - return 0; -} - -/** - * Obtain a pointer to the storage allocated for a parameter. - * - * @param param The parameter whose storage is sought. - * @return A pointer to the parameter value, or NULL - * if the parameter does not exist. - */ -static const void * -param_get_value_ptr(param_t param) -{ - const void *result = NULL; - - 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 != NULL) { - v = &s->val; - - } else { - v = ¶m_info_base[param].val; - } - - if (param_type(param) == PARAM_TYPE_STRUCT) { - result = v->p; - - } else { - result = v; - } - } - - return result; -} - -int -param_get(param_t param, void *val) -{ - int result = -1; - - param_lock(); - - const void *v = param_get_value_ptr(param); - - if (val != NULL) { - memcpy(val, v, param_size(param)); - result = 0; - } - - param_unlock(); - - return result; -} - -static int -param_set_internal(param_t param, const void *val, bool mark_saved) -{ - int result = -1; - bool params_changed = false; - - param_lock(); - - if (param_values == NULL) - utarray_new(param_values, ¶m_icd); - - if (param_values == NULL) { - debug("failed to allocate modified values array"); - goto out; - } - - if (handle_in_range(param)) { - - struct param_wbuf_s *s = param_find_changed(param); - - if (s == NULL) { - - /* construct a new parameter */ - struct param_wbuf_s buf = { - .param = param, - .val.p = NULL, - .unsaved = false - }; - - /* add it to the array and sort */ - utarray_push_back(param_values, &buf); - utarray_sort(param_values, param_compare_values); - - /* find it after sorting */ - s = param_find_changed(param); - } - - /* update the changed value */ - switch (param_type(param)) { - case PARAM_TYPE_INT32: - s->val.i = *(int32_t *)val; - break; - - case PARAM_TYPE_FLOAT: - s->val.f = *(float *)val; - break; - - case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: - if (s->val.p == NULL) { - s->val.p = malloc(param_size(param)); - - if (s->val.p == NULL) { - debug("failed to allocate parameter storage"); - goto out; - } - } - - memcpy(s->val.p, val, param_size(param)); - break; - - default: - goto out; - } - - s->unsaved = !mark_saved; - params_changed = true; - result = 0; - } - -out: - param_unlock(); - - /* - * If we set something, now that we have unlocked, go ahead and advertise that - * a thing has been set. - */ - if (params_changed) - param_notify_changes(); - - return result; -} - -int -param_set(param_t param, const void *val) -{ - return param_set_internal(param, val, false); -} - -void -param_reset(param_t param) -{ - struct param_wbuf_s *s = NULL; - - param_lock(); - - if (handle_in_range(param)) { - - /* look for a saved value */ - s = param_find_changed(param); - - /* if we found one, erase it */ - if (s != NULL) { - int pos = utarray_eltidx(param_values, s); - utarray_erase(param_values, pos, 1); - } - } - - param_unlock(); - - if (s != NULL) - param_notify_changes(); -} - -void -param_reset_all(void) -{ - param_lock(); - - if (param_values != NULL) { - utarray_free(param_values); - } - - /* mark as reset / deleted */ - param_values = NULL; - - param_unlock(); - - param_notify_changes(); -} - -static const char *param_default_file = "/eeprom/parameters"; -static char *param_user_file = NULL; - -int -param_set_default_file(const char* filename) -{ - if (param_user_file != NULL) { - free(param_user_file); - param_user_file = NULL; - } - if (filename) - param_user_file = strdup(filename); - return 0; -} - -const char * -param_get_default_file(void) -{ - return (param_user_file != NULL) ? param_user_file : param_default_file; -} - -int -param_save_default(void) -{ - /* delete the file in case it exists */ - unlink(param_get_default_file()); - - /* create the file */ - int fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL); - - if (fd < 0) { - warn("opening '%s' for writing failed", param_get_default_file()); - return -1; - } - - int result = param_export(fd, false); - close(fd); - - if (result != 0) { - warn("error exporting parameters to '%s'", param_get_default_file()); - unlink(param_get_default_file()); - return -2; - } - - return 0; -} - -/** - * @return 0 on success, 1 if all params have not yet been stored, -1 if device open failed, -2 if writing parameters failed - */ -int -param_load_default(void) -{ - int fd = open(param_get_default_file(), O_RDONLY); - - if (fd < 0) { - /* no parameter file is OK, otherwise this is an error */ - if (errno != ENOENT) { - warn("open '%s' for reading failed", param_get_default_file()); - return -1; - } - return 1; - } - - int result = param_load(fd); - close(fd); - - if (result != 0) { - warn("error reading parameters from '%s'", param_get_default_file()); - return -2; - } - - return 0; -} - -int -param_export(int fd, bool only_unsaved) -{ - struct param_wbuf_s *s = NULL; - struct bson_encoder_s encoder; - int result = -1; - - param_lock(); - - bson_encoder_init_file(&encoder, fd); - - /* no modified parameters -> we are done */ - if (param_values == NULL) { - result = 0; - goto out; - } - - while ((s = (struct param_wbuf_s *)utarray_next(param_values, s)) != NULL) { - - int32_t i; - float f; - - /* - * If we are only saving values changed since last save, and this - * one hasn't, then skip it - */ - if (only_unsaved && !s->unsaved) - continue; - - s->unsaved = false; - - /* append the appropriate BSON type object */ - switch (param_type(s->param)) { - case PARAM_TYPE_INT32: - param_get(s->param, &i); - - if (bson_encoder_append_int(&encoder, param_name(s->param), i)) { - debug("BSON append failed for '%s'", param_name(s->param)); - goto out; - } - - break; - - case PARAM_TYPE_FLOAT: - param_get(s->param, &f); - - if (bson_encoder_append_double(&encoder, param_name(s->param), f)) { - debug("BSON append failed for '%s'", param_name(s->param)); - goto out; - } - - break; - - case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: - if (bson_encoder_append_binary(&encoder, - param_name(s->param), - BSON_BIN_BINARY, - param_size(s->param), - param_get_value_ptr(s->param))) { - debug("BSON append failed for '%s'", param_name(s->param)); - goto out; - } - - break; - - default: - debug("unrecognized parameter type"); - goto out; - } - } - - result = 0; - -out: - param_unlock(); - - if (result == 0) - result = bson_encoder_fini(&encoder); - - return result; -} - -struct param_import_state { - bool mark_saved; -}; - -static int -param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node) -{ - float f; - int32_t i; - void *v, *tmp = NULL; - int result = -1; - struct param_import_state *state = (struct param_import_state *)private; - - /* - * EOO means the end of the parameter object. (Currently not supporting - * nested BSON objects). - */ - if (node->type == BSON_EOO) { - debug("end of parameters"); - return 0; - } - - /* - * Find the parameter this node represents. If we don't know it, - * ignore the node. - */ - param_t param = param_find(node->name); - - if (param == PARAM_INVALID) { - debug("ignoring unrecognised parameter '%s'", node->name); - return 1; - } - - /* - * Handle setting the parameter from the node - */ - - switch (node->type) { - case BSON_INT32: - if (param_type(param) != PARAM_TYPE_INT32) { - debug("unexpected type for '%s", node->name); - goto out; - } - - i = node->i; - v = &i; - break; - - case BSON_DOUBLE: - if (param_type(param) != PARAM_TYPE_FLOAT) { - debug("unexpected type for '%s", node->name); - goto out; - } - - f = node->d; - v = &f; - break; - - case BSON_BINDATA: - if (node->subtype != BSON_BIN_BINARY) { - debug("unexpected subtype for '%s", node->name); - goto out; - } - - if (bson_decoder_data_pending(decoder) != param_size(param)) { - debug("bad size for '%s'", node->name); - goto out; - } - - /* XXX check actual file data size? */ - tmp = malloc(param_size(param)); - - if (tmp == NULL) { - debug("failed allocating for '%s'", node->name); - goto out; - } - - if (bson_decoder_copy_data(decoder, tmp)) { - debug("failed copying data for '%s'", node->name); - goto out; - } - - v = tmp; - break; - - default: - debug("unrecognised node type"); - goto out; - } - - if (param_set_internal(param, v, state->mark_saved)) { - debug("error setting value for '%s'", node->name); - goto out; - } - - if (tmp != NULL) { - free(tmp); - tmp = NULL; - } - - /* don't return zero, that means EOF */ - result = 1; - -out: - - if (tmp != NULL) - free(tmp); - - return result; -} - -static int -param_import_internal(int fd, bool mark_saved) -{ - struct bson_decoder_s decoder; - int result = -1; - struct param_import_state state; - - if (bson_decoder_init_file(&decoder, fd, param_import_callback, &state)) { - debug("decoder init failed"); - goto out; - } - - state.mark_saved = mark_saved; - - do { - result = bson_decoder_next(&decoder); - - } while (result > 0); - -out: - - if (result < 0) - debug("BSON error decoding parameters"); - - return result; -} - -int -param_import(int fd) -{ - return param_import_internal(fd, false); -} - -int -param_load(int fd) -{ - param_reset_all(); - return param_import_internal(fd, true); -} - -void -param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_changed) -{ - param_t param; - - for (param = 0; handle_in_range(param); param++) { - - /* if requested, skip unchanged values */ - if (only_changed && (param_find_changed(param) == NULL)) - continue; - - func(arg, param); - } -} |