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;
}
static int
param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_changes)
{
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_set_used_internal(param);
param_unlock();
/*
* If we set something, now that we have unlocked, go ahead and advertise that
* a thing has been set.
*/
if (params_changed && notify_changes)
param_notify_changes();
return result;
}
int
param_set(param_t param, const void *val)
{
return param_set_internal(param, val, false, true);
}
int
param_set_no_notification(param_t param, const void *val)
{
return param_set_internal(param, val, false, false);
}
bool
param_used(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);
}
static void
_param_notify_changes(void)
{
#if !defined(PARAM_NO_ORB)
struct parameter_update_s pup = {
.timestamp = hrt_absolute_time(),
.instance = param_instance++,
};
/*
* If we don't have a handle to our topic, create one now; otherwise
* just publish.
*/
if (param_topic == NULL) {
param_topic = orb_advertise(ORB_ID(parameter_update), &pup);
} else {
orb_publish(ORB_ID(parameter_update), param_topic, &pup);
}
#endif
}
void
param_notify_changes(void)
{
_param_notify_changes();
}
param_t
param_find_internal(const char *name, bool notification)
{
perf_begin(param_find_perf);
param_t middle;
param_t front = 0;
param_t last = get_param_info_count();
/* perform a binary search of the known parameters */
while (front <= last) {
middle = front + (last - front) / 2;
int ret = strcmp(name, param_info_base[middle].name);
if (ret == 0) {
if (notification) {
param_set_used_internal(middle);
}
perf_end(param_find_perf);
return middle;
} else if (middle == front) {
/* An end point has been hit, but there has been no match */
break;
} else if (ret < 0) {
last = middle;
} else {
front = middle;
}
}
perf_end(param_find_perf);
/* not found */
return PARAM_INVALID;
}
