/**
* @return 0 on success, 1 if all params have not yet been stored, -1 if device open failed, -2 if writing parameters failed
*/
static int
param_load_default_no_notify(void)
{
int fd_load = open(param_get_default_file(), O_RDONLY);
if (fd_load < 0) {
release_shmem_lock();
/* no parameter file is OK, otherwise this is an error */
if (errno != ENOENT) {
debug("open '%s' for reading failed", param_get_default_file());
return -1;
}
return 1;
}
int result = param_import(fd_load);
close(fd_load);
PX4_INFO("param loading done\n");
if (result != 0) {
warn("error reading parameters from '%s'", param_get_default_file());
return -2;
}
return 0;
}
static void update_value_from_shmem(param_t param, union param_value_u *value)
{
unsigned int byte_changed, bit_changed;
if (get_shmem_lock(__FILE__, __LINE__) != 0) {
PX4_ERR("Could not get shmem lock\n");
return;
}
*value = shmem_info_p->params_val[param];
/*also clear the index since we are holding the lock*/
byte_changed = param / 8;
bit_changed = 1 << param % 8;
shmem_info_p->krait_changed_index[byte_changed] &= ~bit_changed;
release_shmem_lock(__FILE__, __LINE__);
#ifdef SHMEM_DEBUG
if (param_type(param) == PARAM_TYPE_INT32) {
PX4_INFO("Got value %d for param %s from shmem, cleared krait index %d:%d\n", value->i, param_name(param), byte_changed,
bit_changed);
}
else if (param_type(param) == PARAM_TYPE_FLOAT) {
PX4_INFO("Got value %f for param %s from shmem, cleared krait index %d:%d\n", value->f, param_name(param), byte_changed,
bit_changed);
}
#endif
}
void update_index_from_shmem(void)
{
unsigned int i;
if (get_shmem_lock(__FILE__, __LINE__) != 0) {
PX4_ERR("Could not get shmem lock\n");
return;
}
PX4_DEBUG("Updating index from shmem\n");
for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) {
// Check if any param has been changed.
if (krait_changed_index[i] != shmem_info_p->krait_changed_index[i]) {
// If a param has changed, we need to find out which one.
// From the byte and bit that is different, we can resolve the param number.
unsigned bit = log2_for_int(krait_changed_index[i] ^ shmem_info_p->krait_changed_index[i]);
param_t param_to_get = i * 8 + bit;
// Update our krait_changed_index as well.
krait_changed_index[i] = shmem_info_p->krait_changed_index[i];
// FIXME: this is a hack but it gets the param so that it gets added
// to the local list param_values in param_shmem.c.
int32_t dummy;
param_get(param_to_get, &dummy);
}
}
release_shmem_lock();
}
/*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 (get_shmem_lock(__FILE__, __LINE__) != 0) {
PX4_ERR("Could not get shmem lock");
return;
}
shmem_info_p->params_val[param] = value;
byte_changed = param / 8;
bit_changed = 1 << param % 8;
shmem_info_p->krait_changed_index[byte_changed] |= bit_changed;
PX4_DEBUG("set %d bit on krait changed index[%d] to %d", bit_changed, byte_changed,
shmem_info_p->krait_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 krait index %d:%d",
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 krait index %d:%d",
(double)value.f, param_name(param), byte_changed, bit_changed);
}
#endif
release_shmem_lock();
}
/*update value and param's change bit in shared memory*/
int px4muorb_param_update_to_shmem(uint32_t param, const uint8_t *value,
int data_len_in_bytes)
{
unsigned int byte_changed, bit_changed;
union param_value_u *param_value = (union param_value_u *) value;
if (!shmem_info_p) {
init_shared_memory();
}
if (get_shmem_lock(__FILE__, __LINE__) != 0) {
PX4_INFO("Could not get shmem lock\n");
return -1;
}
shmem_info_p->params_val[param] = *param_value;
byte_changed = param / 8;
bit_changed = 1 << param % 8;
shmem_info_p->krait_changed_index[byte_changed] |= bit_changed;
release_shmem_lock(__FILE__, __LINE__);
return 0;
}
int px4muorb_param_update_value_from_shmem(uint32_t param, const uint8_t *value,
int data_len_in_bytes)
{
unsigned int byte_changed, bit_changed;
union param_value_u *param_value = (union param_value_u *) value;
if (!shmem_info_p) {
return -1;
}
if (get_shmem_lock(__FILE__, __LINE__) != 0) {
PX4_INFO("Could not get shmem lock\n");
return -1;
}
*param_value = shmem_info_p->params_val[param];
/*also clear the index since we are holding the lock*/
byte_changed = param / 8;
bit_changed = 1 << param % 8;
shmem_info_p->adsp_changed_index[byte_changed] &= ~bit_changed;
release_shmem_lock(__FILE__, __LINE__);
return 0;
}
int
param_save_default(void)
{
int res = OK;
int fd = -1;
bool is_locked = false;
const char *filename = param_get_default_file();
if (get_shmem_lock(__FILE__, __LINE__) != 0) {
PX4_ERR("Could not get shmem lock\n");
res = ERROR;
goto exit;
}
is_locked = true;
fd = PARAM_OPEN(filename, O_WRONLY | O_CREAT, PX4_O_MODE_666);
if (fd < 0) {
PX4_ERR("failed to open param file: %s", filename);
goto exit;
}
res = param_export(fd, false);
if (res != OK) {
PX4_ERR("failed to write parameters to file: %s", filename);
goto exit;
}
PARAM_CLOSE(fd);
fd = -1;
exit:
if (is_locked) {
release_shmem_lock();
}
if (fd >= 0) {
close(fd);
}
if (res == OK) {
PX4_INFO("saving params completed successfully\n");
}
return res;
}
void copy_params_to_shmem(struct param_info_s *param_info_base)
{
param_t param;
unsigned int i;
if (get_shmem_lock(__FILE__, __LINE__) != 0) {
PX4_ERR("Could not get shmem lock");
return;
}
PX4_DEBUG("%d krait params allocated", param_count());
for (param = 0; param < param_count(); param++) {
struct param_wbuf_s *s = param_find_changed(param);
if (s == NULL) {
shmem_info_p->params_val[param] = param_info_base[param].val;
} else {
shmem_info_p->params_val[param] = s->val;
}
#ifdef SHMEM_DEBUG
if (param_type(param) == PARAM_TYPE_INT32) {
{
PX4_INFO("%d: written %d for param %s to shared mem",
param, shmem_info_p->params_val[param].i, param_name(param));
}
} else if (param_type(param) == PARAM_TYPE_FLOAT) {
{
PX4_INFO("%d: written %f for param %s to shared mem",
param, (double)shmem_info_p->params_val[param].f, param_name(param));
}
}
#endif
}
//PX4_DEBUG("written %u params to shmem offset %lu", param_count(), (unsigned char*)&shmem_info_p->params_count-(unsigned char*)shmem_info_p);
for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) {
shmem_info_p->krait_changed_index[i] = 0;
adsp_changed_index[i] = 0;
}
release_shmem_lock();
}
static void update_index_from_shmem(void)
{
unsigned int i;
if (get_shmem_lock(__FILE__, __LINE__) != 0) {
PX4_ERR("Could not get shmem lock");
return;
}
PX4_DEBUG("Updating index from shmem");
for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) {
adsp_changed_index[i] = shmem_info_p->adsp_changed_index[i];
}
release_shmem_lock();
}
static void update_index_from_shmem(void)
{
unsigned int i;
if (get_shmem_lock(__FILE__, __LINE__) != 0) {
fprintf(stderr, "Could not get shmem lock\n");
return;
}
//PX4_INFO("Updating index from shmem\n");
for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) {
adsp_changed_index[i] = shmem_info_p->adsp_changed_index[i];
}
release_shmem_lock();
}
void copy_params_to_shmem(const param_info_s *param_info_base)
{
param_t param;
unsigned int i;
if (get_shmem_lock(__FILE__, __LINE__) != 0) {
PX4_INFO("Could not get shmem lock\n");
return;
}
//else PX4_INFO("Got lock\n");
for (param = 0; param < param_count(); param++) {
//{PX4_INFO("writing to offset %d\n", (unsigned char*)(shmem_info_p->adsp_params[param].name)-(unsigned char*)shmem_info_p);}
struct param_wbuf_s *s = param_find_changed(param);
if (s == NULL) {
shmem_info_p->params_val[param] = param_info_base[param].val;
}
else {
shmem_info_p->params_val[param] = s->val;
}
#ifdef SHMEM_DEBUG
if (param_type(param) == PARAM_TYPE_INT32) {
PX4_INFO("%d: written %d for param %s to shared mem", param, shmem_info_p->params_val[param].i, param_name(param));
} else if (param_type(param) == PARAM_TYPE_FLOAT) {
PX4_INFO("%d: written %f for param %s to shared mem", param, shmem_info_p->params_val[param].f, param_name(param));
}
#endif
}
for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) {
shmem_info_p->adsp_changed_index[i] = 0;
krait_changed_index[i] = 0;
}
release_shmem_lock(__FILE__, __LINE__);
//PX4_INFO("Released lock\n");
}
int px4muorb_param_update_index_from_shmem(unsigned char *data, int data_len_in_bytes)
{
if (!shmem_info_p) {
return -1;
}
if (get_shmem_lock(__FILE__, __LINE__) != 0) {
PX4_INFO("Could not get shmem lock\n");
return -1;
}
for (int i = 0; i < data_len_in_bytes; i++) {
data[i] = shmem_info_p->adsp_changed_index[i];
}
release_shmem_lock(__FILE__, __LINE__);
return 0;
}
