int
param_reset(param_t param)
{
struct param_wbuf_s *s = NULL;
bool param_found = false;
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_found = true;
}
param_unlock();
if (s != NULL) {
param_notify_changes();
}
return (!param_found);
}
/**
* 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 &&
param_type(param) <= PARAM_TYPE_STRUCT_MAX) {
result = v->p;
} else {
result = v;
}
}
return result;
}
int update_from_shmem(param_t param, union param_value_u *value)
{
unsigned int byte_changed, bit_changed;
unsigned int retval = 0;
if (!handle_in_range(param) || value == NULL) {
return retval;
}
update_from_shmem_current_time = hrt_absolute_time();
if ((update_from_shmem_current_time - update_from_shmem_prev_time)
> 1000000) { //update every 1 second
update_from_shmem_prev_time = update_from_shmem_current_time;
update_index_from_shmem();
}
byte_changed = param / 8;
bit_changed = 1 << param % 8;
if (krait_changed_index[byte_changed] & bit_changed) {
update_value_from_shmem(param, value);
krait_changed_index[byte_changed] &= ~bit_changed;
retval = 1;
}
//else {PX4_INFO("no change to param %s\n", param_name(param));}
PX4_DEBUG("%s %d bit on krait changed index[%d]\n",
(retval) ? "cleared" : "unchanged", bit_changed, byte_changed);
return retval;
}
void
param_reset_excludes(const char *excludes[], int num_excludes)
{
param_lock();
param_t param;
for (param = 0; handle_in_range(param); param++) {
const char *name = param_name(param);
bool exclude = false;
for (int index = 0; index < num_excludes; index ++) {
int len = strlen(excludes[index]);
if ((excludes[index][len - 1] == '*'
&& strncmp(name, excludes[index], len - 1) == 0)
|| strcmp(name, excludes[index]) == 0) {
exclude = true;
break;
}
}
if (!exclude) {
param_reset(param);
}
}
param_unlock();
param_notify_changes();
}
int
param_get_index(param_t param)
{
if (handle_in_range(param))
return (unsigned)param;
return -1;
}
enum param_type_e
param_type(param_t param)
{
if (handle_in_range(param))
return param_info_base[param].type;
return PARAM_TYPE_UNKNOWN;
}
const char *
param_name(param_t param)
{
if (handle_in_range(param))
return param_info_base[param].name;
return NULL;
}
static const void *
param_get_value_ptr(param_t param)
{
const void *result = NULL;
if (handle_in_range(param)) {
result = ¶m_info_base[param].val;
}
return result;
}
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;
}
int
param_get(param_t param, void *val)
{
int result = -1;
param_lock();
if (!handle_in_range(param)) {
return result;
}
union param_value_u value;
if (update_from_shmem(param, &value)) {
set_called_from_get = 1;
param_set_internal(param, &value, true, false, false);
set_called_from_get = 0;
}
const void *v = param_get_value_ptr(param);
if (val != NULL) {
memcpy(val, v, param_size(param));
result = 0;
}
#ifdef ENABLE_SHMEM_DEBUG
if (param_type(param) == PARAM_TYPE_INT32) {
PX4_INFO("param_get for %s : %d\n", param_name(param), ((union param_value_u *)val)->i);
}
else if (param_type(param) == PARAM_TYPE_FLOAT) {
PX4_INFO("param_get for %s : %f\n", param_name(param), (double)((union param_value_u *)val)->f);
}
else {
PX4_INFO("Unknown param type for %s\n", param_name(param));
}
#endif
param_unlock();
return result;
}
param_t
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 (notification) {
param_set_used_internal(param);
}
return param;
}
}
/* not found */
return PARAM_INVALID;
}
int
param_get_used_index(param_t param)
{
if (!handle_in_range(param)) {
return -1;
}
/* walk all params and count */
int count = 0;
for (unsigned i = 0; i < (unsigned)param + 1; i++) {
for (unsigned j = 0; j < 8; j++) {
if (param_changed_storage[i] & (1 << j)) {
count++;
}
}
}
return count;
}
/*update value and param's change bit in shared memory*/
void update_to_shmem(param_t param, union param_value_u value)
{
unsigned int byte_changed, bit_changed;
if (!handle_in_range(param)) {
return;
}
if (get_shmem_lock(__FILE__, __LINE__) != 0) {
PX4_ERR("Could not get shmem lock\n");
return;
}
shmem_info_p->params_val[param] = value;
byte_changed = param / 8;
bit_changed = 1 << param % 8;
shmem_info_p->adsp_changed_index[byte_changed] |= bit_changed;
//PX4_INFO("set %d bit on adsp index[%d] to %d\n", bit_changed, byte_changed, shmem_info_p->adsp_changed_index[byte_changed]);
#ifdef SHMEM_DEBUG
if (param_type(param) == PARAM_TYPE_INT32) {
PX4_INFO("Set value %d for param %s to shmem, set adsp index %d:%d\n", value.i, param_name(param), byte_changed,
bit_changed);
}
else if (param_type(param) == PARAM_TYPE_FLOAT) {
PX4_INFO("Set value %f for param %s to shmem, set adsp index %d:%d\n", value.f, param_name(param), byte_changed,
bit_changed);
}
#endif
release_shmem_lock(__FILE__, __LINE__);
}
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();
}
enum param_type_e
param_type(param_t param) {
return handle_in_range(param) ? param_info_base[param].type : PARAM_TYPE_UNKNOWN;
}
const char *
param_name(param_t param)
{
return handle_in_range(param) ? param_info_base[param].name : NULL;
}
bool
param_is_volatile(param_t param)
{
return handle_in_range(param) ? param_info_base[param].volatile_param : false;
}
static int
param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_changes, bool is_saved)
{
int result = -1;
bool params_changed = false;
PX4_DEBUG("param_set_internal params: param = %d, val = 0x%X, mark_saved: %d, notify_changes: %d",
param, val, (int)mark_saved, (int)notify_changes);
param_lock();
if (!handle_in_range(param)) {
return result;
}
mark_saved = true; //mark all params as saved
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 (!param_import_done) { notify_changes = 0; }
if (params_changed && notify_changes) {
param_notify_changes(is_saved);
}
if (result == 0 && !set_called_from_get) {
update_to_shmem(param, *(union param_value_u *)val);
}
#ifdef ENABLE_SHMEM_DEBUG
if (param_type(param) == PARAM_TYPE_INT32) {
PX4_INFO("param_set for %s : %d\n", param_name(param), ((union param_value_u *)val)->i);
}
else if (param_type(param) == PARAM_TYPE_FLOAT) {
PX4_INFO("param_set for %s : %f\n", param_name(param), (double)((union param_value_u *)val)->f);
}
else {
PX4_INFO("Unknown param type for %s\n", param_name(param));
}
#endif
return result;
}
int
param_set(param_t param, const void *val)
{
return param_set_internal(param, val, false, true, false);
}
int
param_set_no_autosave(param_t param, const void *val)
{
return param_set_internal(param, val, false, true, true);
}
int
param_set_no_notification(param_t param, const void *val)
{
return param_set_internal(param, val, false, false, false);
}
bool
param_used(param_t param)
{
// TODO FIXME: for now all params are used
return true;
int param_index = param_get_index(param);
if (param_index < 0) {
return false;
}
return param_changed_storage[param_index / bits_per_allocation_unit] &
(1 << param_index % bits_per_allocation_unit);
}
void param_set_used_internal(param_t param)
{
int param_index = param_get_index(param);
if (param_index < 0) {
return;
}
param_changed_storage[param_index / bits_per_allocation_unit] |=
(1 << param_index % bits_per_allocation_unit);
}
