int write_parameters(int fd)
{
/* construct parameter message */
struct {
LOG_PACKET_HEADER;
struct log_PARM_s body;
} log_msg_PARM = {
LOG_PACKET_HEADER_INIT(LOG_PARM_MSG),
};
int written = 0;
param_t params_cnt = param_count();
for (param_t param = 0; param < params_cnt; param++) {
/* fill parameter message and write it */
strncpy(log_msg_PARM.body.name, param_name(param), sizeof(log_msg_PARM.body.name));
float value = NAN;
switch (param_type(param)) {
case PARAM_TYPE_INT32: {
int32_t i;
param_get(param, &i);
value = i; // cast integer to float
break;
}
case PARAM_TYPE_FLOAT:
param_get(param, &value);
break;
default:
break;
}
log_msg_PARM.body.value = value;
written += write(fd, &log_msg_PARM, sizeof(log_msg_PARM));
}
return written;
}
void Ekf2Replay::setUserParams(const char *filename)
{
std::string line;
std::ifstream myfile(filename);
std::string param_name;
std::string value_string;
if (myfile.is_open()) {
while (! myfile.eof()) {
getline(myfile, line);
if (line.empty()) {
continue;
}
std::istringstream mystrstream(line);
mystrstream >> param_name;
mystrstream >> value_string;
double param_value_double = std::stod(value_string);
param_t handle = param_find(param_name.c_str());
param_type_t param_format = param_type(handle);
if (param_format == PARAM_TYPE_INT32) {
int32_t value = 0;
value = (int32_t)param_value_double;
param_set(handle, (const void *)&value);
} else if (param_format == PARAM_TYPE_FLOAT) {
float value = 0;
value = (float)param_value_double;
param_set(handle, (const void *)&value);
}
}
myfile.close();
}
}
int
param_export(int fd, bool only_unsaved)
{
perf_begin(param_export_perf);
struct param_wbuf_s *s = NULL;
int result = -1;
struct bson_encoder_s encoder;
int shutdown_lock_ret = px4_shutdown_lock();
if (shutdown_lock_ret) {
PX4_ERR("px4_shutdown_lock() failed (%i)", shutdown_lock_ret);
}
// take the file lock
do {} while (px4_sem_wait(¶m_sem_save) != 0);
param_lock_reader();
uint8_t bson_buffer[256];
bson_encoder_init_buf_file(&encoder, fd, &bson_buffer, sizeof(bson_buffer));
/* 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) {
/*
* 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;
const char *name = param_name(s->param);
const size_t size = param_size(s->param);
/* append the appropriate BSON type object */
switch (param_type(s->param)) {
case PARAM_TYPE_INT32: {
const int32_t i = s->val.i;
debug("exporting: %s (%d) size: %d val: %d", name, s->param, size, i);
if (bson_encoder_append_int(&encoder, name, i)) {
PX4_ERR("BSON append failed for '%s'", name);
goto out;
}
}
break;
case PARAM_TYPE_FLOAT: {
const float f = s->val.f;
debug("exporting: %s (%d) size: %d val: %.3f", name, s->param, size, (double)f);
if (bson_encoder_append_double(&encoder, name, f)) {
PX4_ERR("BSON append failed for '%s'", name);
goto out;
}
}
break;
case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: {
const void *value_ptr = param_get_value_ptr(s->param);
/* lock as short as possible */
if (bson_encoder_append_binary(&encoder,
name,
BSON_BIN_BINARY,
size,
value_ptr)) {
PX4_ERR("BSON append failed for '%s'", name);
goto out;
}
}
break;
default:
PX4_ERR("unrecognized parameter type");
goto out;
}
}
result = 0;
out:
if (result == 0) {
if (bson_encoder_fini(&encoder) != PX4_OK) {
PX4_ERR("bson encoder finish failed");
}
}
param_unlock_reader();
px4_sem_post(¶m_sem_save);
if (shutdown_lock_ret == 0) {
px4_shutdown_unlock();
}
perf_end(param_export_perf);
return result;
}
static void
do_compare(const char* name, const char* vals[], unsigned comparisons)
{
int32_t i;
float f;
param_t param = param_find(name);
/* set nothing if parameter cannot be found */
if (param == PARAM_INVALID) {
/* param not found */
errx(1, "Error: Parameter %s not found.", name);
}
/*
* Set parameter if type is known and conversion from string to value turns out fine
*/
int ret = 1;
switch (param_type(param)) {
case PARAM_TYPE_INT32:
if (!param_get(param, &i)) {
/* convert string */
char* end;
for (unsigned k = 0; k < comparisons; k++) {
int j = strtol(vals[k],&end,10);
if (i == j) {
printf(" %d: ", i);
ret = 0;
}
}
}
break;
case PARAM_TYPE_FLOAT:
if (!param_get(param, &f)) {
/* convert string */
char* end;
for (unsigned k = 0; k < comparisons; k++) {
float g = strtod(vals[k], &end);
if (fabsf(f - g) < 1e-7f) {
printf(" %4.4f: ", (double)f);
ret = 0;
}
}
}
break;
default:
errx(1, "<unknown / unsupported type %d>\n", 0 + param_type(param));
}
if (ret == 0) {
printf("%c %s: match\n",
param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
param_name(param));
}
exit(ret);
}
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);
}
/** Returns the parameters of the distribution */
param_type param() const { return param_type(this->a(), this->b()); }
static void
do_show_print(void *arg, param_t param)
{
int32_t i;
float f;
const char *search_string = (const char*)arg;
const char *p_name = (const char*)param_name(param);
/* print nothing if search string is invalid and not matching */
if (!(arg == NULL)) {
/* start search */
char *ss = search_string;
char *pp = p_name;
bool mismatch = false;
/* XXX this comparison is only ok for trailing wildcards */
while (*ss != '\0' && *pp != '\0') {
if (*ss == *pp) {
ss++;
pp++;
} else if (*ss == '*') {
if (*(ss + 1) != '\0') {
warnx("* symbol only allowed at end of search string.");
exit(1);
}
pp++;
} else {
/* param not found */
return;
}
}
/* the search string must have been consumed */
if (!(*ss == '\0' || *ss == '*'))
return;
}
printf("%c %s: ",
param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
param_name(param));
/*
* This case can be expanded to handle printing common structure types.
*/
switch (param_type(param)) {
case PARAM_TYPE_INT32:
if (!param_get(param, &i)) {
printf("%d\n", i);
return;
}
break;
case PARAM_TYPE_FLOAT:
if (!param_get(param, &f)) {
printf("%4.4f\n", (double)f);
return;
}
break;
case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX:
printf("<struct type %d size %u>\n", 0 + param_type(param), param_size(param));
return;
default:
printf("<unknown type %d>\n", 0 + param_type(param));
return;
}
printf("<error fetching parameter %d>\n", param);
}
static void
do_set(const char *name, const char *val, bool fail_on_not_found)
{
int32_t i;
float f;
param_t param = param_find(name);
/* set nothing if parameter cannot be found */
if (param == PARAM_INVALID) {
/* param not found - fail silenty in scripts as it prevents booting */
errx(((fail_on_not_found) ? 1 : 0), "Error: Parameter %s not found.", name);
}
printf("%c %s: ",
param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
param_name(param));
/*
* Set parameter if type is known and conversion from string to value turns out fine
*/
switch (param_type(param)) {
case PARAM_TYPE_INT32:
if (!param_get(param, &i)) {
/* convert string */
char *end;
int32_t newval = strtol(val, &end, 10);
if (i == newval) {
printf("unchanged\n");
} else {
printf("curr: %d", i);
param_set(param, &newval);
printf(" -> new: %d\n", newval);
}
}
break;
case PARAM_TYPE_FLOAT:
if (!param_get(param, &f)) {
/* convert string */
char *end;
float newval = strtod(val, &end);
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wfloat-equal"
if (f == newval) {
#pragma GCC diagnostic pop
printf("unchanged\n");
} else {
printf("curr: %4.4f", (double)f);
param_set(param, &newval);
printf(" -> new: %4.4f\n", (double)newval);
}
}
break;
default:
errx(1, "<unknown / unsupported type %d>\n", 0 + param_type(param));
}
exit(0);
}
static int
param_export_internal(bool only_unsaved)
{
struct param_wbuf_s *s = NULL;
struct bson_encoder_s encoder;
int result = -1;
param_lock();
/* Use realloc */
bson_encoder_init_buf(&encoder, NULL, 0);
/* 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_external(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) {
/* Finalize the bison encoding*/
bson_encoder_fini(&encoder);
/* Get requiered space */
size_t buf_size = bson_encoder_buf_size(&encoder);
/* Get a buffer from the flash driver with enough space */
uint8_t *buffer;
result = parameter_flashfs_alloc(parameters_token, &buffer, &buf_size);
if (result == OK) {
/* Check for a write that has no changes */
uint8_t *was_buffer;
size_t was_buf_size;
int was_result = parameter_flashfs_read(parameters_token, &was_buffer, &was_buf_size);
void *enc_buff = bson_encoder_buf_data(&encoder);
bool commit = was_result < OK || was_buf_size != buf_size || 0 != memcmp(was_buffer, enc_buff, was_buf_size);
if (commit) {
memcpy(buffer, enc_buff, buf_size);
result = parameter_flashfs_write(parameters_token, buffer, buf_size);
result = result == buf_size ? OK : -EFBIG;
}
free(enc_buff);
}
}
return result;
}
static int
do_set(const char *name, const char *val, bool fail_on_not_found)
{
int32_t i;
float f;
param_t param = param_find(name);
/* set nothing if parameter cannot be found */
if (param == PARAM_INVALID) {
/* param not found - fail silenty in scripts as it prevents booting */
PX4_ERR("Parameter %s not found.", name);
return (fail_on_not_found) ? 1 : 0;
}
/*
* Set parameter if type is known and conversion from string to value turns out fine
*/
switch (param_type(param)) {
case PARAM_TYPE_INT32:
if (!param_get(param, &i)) {
/* convert string */
char *end;
int32_t newval = strtol(val, &end, 10);
if (i != newval) {
PARAM_PRINT("%c %s: ",
param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
param_name(param));
PARAM_PRINT("curr: %ld", (long)i);
param_set_no_autosave(param, &newval);
PARAM_PRINT(" -> new: %ld\n", (long)newval);
}
}
break;
case PARAM_TYPE_FLOAT:
if (!param_get(param, &f)) {
/* convert string */
char *end;
float newval = strtod(val, &end);
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wfloat-equal"
if (f != newval) {
#pragma GCC diagnostic pop
PARAM_PRINT("%c %s: ",
param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
param_name(param));
PARAM_PRINT("curr: %4.4f", (double)f);
param_set_no_autosave(param, &newval);
PARAM_PRINT(" -> new: %4.4f\n", (double)newval);
}
}
break;
default:
PX4_ERR("<unknown / unsupported type %d>\n", 0 + param_type(param));
return 1;
}
int ret = param_save_default();
if (ret) {
PX4_ERR("Param save failed (%i)", ret);
return 1;
} else {
return 0;
}
}
void Ekf2Replay::task_main()
{
// formats
const int _k_max_data_size = 1024; // 16x16 bytes
uint8_t data[_k_max_data_size] = {};
const char param_file[] = "./rootfs/replay_params.txt";
// Open log file from which we read data
// TODO Check if file exists
int fd = ::open(_file_name, O_RDONLY);
// create path to write a replay file
char *replay_log_name;
replay_log_name = strtok(_file_name, ".");
char tmp[] = "_replayed.px4log";
char *path_to_replay_log = (char *) malloc(1 + strlen(tmp) + strlen(replay_log_name));
strcpy(path_to_replay_log, ".");
strcat(path_to_replay_log, replay_log_name);
strcat(path_to_replay_log, tmp);
// create path which tells user location of replay file
char tmp2[] = "./build_posix_sitl_replay/src/firmware/posix";
char *replay_file_location = (char *) malloc(1 + strlen(tmp) + strlen(tmp2) + strlen(replay_log_name));
strcat(replay_file_location, tmp2);
strcat(replay_file_location, replay_log_name);
strcat(replay_file_location, tmp);
// open logfile to write
_write_fd = ::open(path_to_replay_log, O_WRONLY | O_CREAT, S_IRWXU);
std::ifstream tmp_file;
tmp_file.open(param_file);
std::string line;
bool set_default_params_in_file = false;
if (tmp_file.is_open() && ! tmp_file.eof()) {
getline(tmp_file, line);
if (line.empty()) {
// the parameter file is empty so write the default values to it
set_default_params_in_file = true;
}
}
tmp_file.close();
std::ofstream myfile(param_file, std::ios::app);
// subscribe to estimator topics
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_estimator_status_sub = orb_subscribe(ORB_ID(estimator_status));
_innov_sub = orb_subscribe(ORB_ID(ekf2_innovations));
_lpos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_control_state_sub = orb_subscribe(ORB_ID(control_state));
// we use attitude updates from the estimator for synchronisation
_fds[0].fd = _att_sub;
_fds[0].events = POLLIN;
bool read_first_header = false;
bool set_user_params = false;
PX4_INFO("Replay in progress... \n");
PX4_INFO("Log data will be written to %s\n", replay_file_location);
while (!_task_should_exit) {
_message_counter++;
uint8_t header[3] = {};
if (::read(fd, header, 3) != 3) {
if (!read_first_header) {
PX4_WARN("error reading log file, is the path printed above correct?");
} else {
PX4_INFO("Done!");
}
_task_should_exit = true;
continue;
}
read_first_header = true;
if ((header[0] != HEAD_BYTE1 || header[1] != HEAD_BYTE2)) {
// we assume that the log file is finished here
PX4_WARN("Done!");
_task_should_exit = true;
continue;
}
// write header but only for messages which are not generated by the estimator
if (needToSaveMessage(header[2])) {
writeMessage(_write_fd, &header[0], 3);
}
if (header[2] == LOG_FORMAT_MSG) {
// format message
struct log_format_s f;
if (::read(fd, &f.type, sizeof(f)) != sizeof(f)) {
PRINT_READ_ERROR;
_task_should_exit = true;
continue;
}
writeMessage(_write_fd, &f.type, sizeof(log_format_s));
memcpy(&_formats[f.type], &f, sizeof(f));
} else if (header[2] == LOG_PARM_MSG) {
// parameter message
if (::read(fd, &data[0], sizeof(log_PARM_s)) != sizeof(log_PARM_s)) {
PRINT_READ_ERROR;
_task_should_exit = true;
continue;
}
writeMessage(_write_fd, &data[0], sizeof(log_PARM_s));
// apply the parameters
char param_name[16];
for (unsigned i = 0; i < 16; i++) {
param_name[i] = data[i];
if (data[i] == '\0') {
break;
}
}
float param_data = 0;
memcpy(¶m_data, &data[16], sizeof(float));
param_t handle = param_find(param_name);
param_type_t param_format = param_type(handle);
if (param_format == PARAM_TYPE_INT32) {
int32_t value = 0;
value = (int32_t)param_data;
param_set(handle, (const void *)&value);
} else if (param_format == PARAM_TYPE_FLOAT) {
param_set(handle, (const void *)¶m_data);
}
// if the user param file was empty then we fill it with the ekf2 parameter values from
// the log file
if (set_default_params_in_file) {
if (strncmp(param_name, "EKF2", 4) == 0) {
std::ostringstream os;
double value = (double)param_data;
os << std::string(param_name) << " ";
os << value << "\n";
myfile << os.str();
}
}
} else if (header[2] == LOG_VER_MSG) {
// version message
if (::read(fd, &data[0], sizeof(log_VER_s)) != sizeof(log_VER_s)) {
PRINT_READ_ERROR;
_task_should_exit = true;
continue;
}
writeMessage(_write_fd, &data[0], sizeof(log_VER_s));
} else if (header[2] == LOG_TIME_MSG) {
// time message
if (::read(fd, &data[0], sizeof(log_TIME_s)) != sizeof(log_TIME_s)) {
// assume that this is because we have reached the end of the file
PX4_INFO("Done!");
_task_should_exit = true;
continue;
}
writeMessage(_write_fd, &data[0], sizeof(log_TIME_s));
} else {
// the first time we arrive here we should apply the parameters specified in the user file
// this makes sure they are applied after the parameter values of the log file
if (!set_user_params) {
myfile.close();
setUserParams(param_file);
set_user_params = true;
}
// data message
if (::read(fd, &data[0], _formats[header[2]].length - 3) != _formats[header[2]].length - 3) {
PX4_INFO("Done!");
_task_should_exit = true;
continue;
}
// all messages which we are not getting from the estimator are written
// back into the replay log file
if (needToSaveMessage(header[2])) {
writeMessage(_write_fd, &data[0], _formats[header[2]].length - 3);
}
if (header[2] == LOG_RPL1_MSG && _part1_counter_ref > 0) {
// we have found another imu replay message while we still have one waiting to be published.
// so publish that now
publishAndWaitForEstimator();
}
// set estimator input data
setEstimatorInput(&data[0], header[2]);
// we have read the imu replay message (part 1) and have waited 3 more cycles for other replay message parts
// e.g. flow, gps or range. we know that in case they were written to the log file they should come right after
// the first replay message, therefore, we can kick the estimator now
if (_part1_counter_ref > 0 && _part1_counter_ref < _message_counter - 3) {
publishAndWaitForEstimator();
}
}
}
::close(_write_fd);
::close(fd);
delete ekf2_replay::instance;
ekf2_replay::instance = nullptr;
}
int
param_export(int fd, bool only_unsaved)
{
struct param_wbuf_s *s = NULL;
struct bson_encoder_s encoder;
int result = -1;
param_lock();
param_bus_lock(true);
bson_encoder_init_file(&encoder, fd);
param_bus_lock(false);
/* 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);
const char *name = param_name(s->param);
/* lock as short as possible */
param_bus_lock(true);
if (bson_encoder_append_int(&encoder, name, i)) {
param_bus_lock(false);
debug("BSON append failed for '%s'", name);
goto out;
}
}
break;
case PARAM_TYPE_FLOAT: {
param_get(s->param, &f);
const char *name = param_name(s->param);
/* lock as short as possible */
param_bus_lock(true);
if (bson_encoder_append_double(&encoder, name, f)) {
param_bus_lock(false);
debug("BSON append failed for '%s'", name);
goto out;
}
}
break;
case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: {
const char *name = param_name(s->param);
const size_t size = param_size(s->param);
const void *value_ptr = param_get_value_ptr(s->param);
/* lock as short as possible */
param_bus_lock(true);
if (bson_encoder_append_binary(&encoder,
name,
BSON_BIN_BINARY,
size,
value_ptr)) {
param_bus_lock(false);
debug("BSON append failed for '%s'", name);
goto out;
}
}
break;
default:
debug("unrecognized parameter type");
goto out;
}
param_bus_lock(false);
/* allow this process to be interrupted by another process / thread */
usleep(5);
}
result = 0;
out:
param_unlock();
if (result == 0) {
result = bson_encoder_fini(&encoder);
}
return result;
}
static void
do_show_print(void *arg, param_t param)
{
int32_t i;
float f;
const char *search_string = (const char *)arg;
const char *p_name = (const char *)param_name(param);
/* print nothing if search string is invalid and not matching */
if (!(arg == NULL)) {
/* start search */
const char *ss = search_string;
const char *pp = p_name;
/* XXX this comparison is only ok for trailing wildcards */
while (*ss != '\0' && *pp != '\0') {
// case insensitive comparison (param_name is always upper case)
if (toupper(*ss) == *pp) {
ss++;
pp++;
} else if (*ss == '*') {
if (*(ss + 1) != '\0') {
PX4_WARN("* symbol only allowed at end of search string");
// FIXME - should exit
return;
}
pp++;
} else {
/* param not found */
return;
}
}
/* the search string must have been consumed */
if (!(*ss == '\0' || *ss == '*') || *pp != '\0') {
return;
}
}
PARAM_PRINT("%c %c %s [%d,%d] : ", (param_used(param) ? 'x' : ' '),
param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
param_name(param), param_get_used_index(param), param_get_index(param));
/*
* This case can be expanded to handle printing common structure types.
*/
switch (param_type(param)) {
case PARAM_TYPE_INT32:
if (!param_get(param, &i)) {
PARAM_PRINT("%ld\n", (long)i);
return;
}
break;
case PARAM_TYPE_FLOAT:
if (!param_get(param, &f)) {
PARAM_PRINT("%4.4f\n", (double)f);
return;
}
break;
case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX:
PARAM_PRINT("<struct type %d size %zu>\n", 0 + param_type(param), param_size(param));
return;
default:
PARAM_PRINT("<unknown type %d>\n", 0 + param_type(param));
return;
}
PARAM_PRINT("<error fetching parameter %lu>\n", (unsigned long)param);
}
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();
}
bool ParameterTest::exportImportAll()
{
static constexpr float MAGIC_FLOAT_VAL = 0.217828f;
// backup current parameters
const char *param_file_name = PX4_STORAGEDIR "/param_backup";
int fd = open(param_file_name, O_WRONLY | O_CREAT, PX4_O_MODE_666);
if (fd < 0) {
PX4_ERR("open '%s' failed (%i)", param_file_name, errno);
return false;
}
int result = param_export(fd, false);
if (result != PX4_OK) {
PX4_ERR("param_export failed");
close(fd);
return false;
}
close(fd);
bool ret = true;
int N = param_count();
// set all params to corresponding param_t value
for (unsigned i = 0; i < N; i++) {
param_t p = param_for_index(i);
if (p == PARAM_INVALID) {
PX4_ERR("param invalid: %d(%d)", p, i);
break;
}
if (param_type(p) == PARAM_TYPE_INT32) {
const int32_t set_val = p;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
PX4_ERR("param_set_no_notification failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
int32_t get_val = 0;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", p, get_val);
}
if (param_type(p) == PARAM_TYPE_FLOAT) {
const float set_val = (float)p + MAGIC_FLOAT_VAL;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
PX4_ERR("param_set_no_notification failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
float get_val = 0.0f;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", p, (float)p + MAGIC_FLOAT_VAL);
}
}
// save
if (param_save_default() != PX4_OK) {
PX4_ERR("param_save_default failed");
return false;
}
// zero all params and verify, but don't save
for (unsigned i = 0; i < N; i++) {
param_t p = param_for_index(i);
if (param_type(p) == PARAM_TYPE_INT32) {
const int32_t set_val = 0;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
PX4_ERR("param set failed: %d", p);
ut_assert("param_set_no_notification failed", false);
}
int32_t get_val = -1;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", set_val, get_val);
}
if (param_type(p) == PARAM_TYPE_FLOAT) {
float set_val = 0.0f;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
PX4_ERR("param set failed: %d", p);
ut_assert("param_set_no_notification failed", false);
}
float get_val = -1.0f;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", set_val, get_val);
}
}
// load saved params
if (param_load_default() != PX4_OK) {
PX4_ERR("param_save_default failed");
ret = true;
}
// check every param
for (unsigned i = 0; i < N; i++) {
param_t p = param_for_index(i);
if (param_type(p) == PARAM_TYPE_INT32) {
int32_t get_val = 0;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", p, get_val);
}
if (param_type(p) == PARAM_TYPE_FLOAT) {
float get_val = 0.0f;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", p, (float)p + MAGIC_FLOAT_VAL);
}
}
param_reset_all();
// restore original params
fd = open(param_file_name, O_RDONLY);
if (fd < 0) {
PX4_ERR("open '%s' failed (%i)", param_file_name, errno);
return false;
}
result = param_import(fd);
close(fd);
if (result < 0) {
PX4_ERR("importing from '%s' failed (%i)", param_file_name, result);
return false;
}
// save
if (param_save_default() != PX4_OK) {
PX4_ERR("param_save_default failed");
return false;
}
return ret;
}
bool ParameterTest::exportImport()
{
static constexpr float MAGIC_FLOAT_VAL = 0.314159f;
bool ret = true;
param_t test_params[] = {p0, p1, p2, p3, p4};
// set all params to corresponding param_t value
for (auto p : test_params) {
if (param_type(p) == PARAM_TYPE_INT32) {
const int32_t set_val = p;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
PX4_ERR("param_set_no_notification failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
int32_t get_val = 0;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", p, get_val);
}
if (param_type(p) == PARAM_TYPE_FLOAT) {
const float set_val = (float)p + MAGIC_FLOAT_VAL;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
PX4_ERR("param_set_no_notification failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
float get_val = 0.0f;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", p, (float)p + MAGIC_FLOAT_VAL);
}
}
// save
if (param_save_default() != PX4_OK) {
PX4_ERR("param_save_default failed");
return false;
}
// zero all params and verify, but don't save
for (auto p : test_params) {
if (param_type(p) == PARAM_TYPE_INT32) {
const int32_t set_val = 0;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
PX4_ERR("param_set_no_notification failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
int32_t get_val = -1;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", set_val, get_val);
}
if (param_type(p) == PARAM_TYPE_FLOAT) {
const float set_val = 0.0f;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
PX4_ERR("param_set_no_notification failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
float get_val = -1.0f;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare_float("value for param doesn't match default value", set_val, get_val, 0.001f);
}
}
// load saved params
if (param_load_default() != PX4_OK) {
PX4_ERR("param_save_default failed");
ret = true;
}
// check every param
for (auto p : test_params) {
if (param_type(p) == PARAM_TYPE_INT32) {
int32_t get_val = 0.0f;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", p, get_val);
}
if (param_type(p) == PARAM_TYPE_FLOAT) {
float get_val = 0.0f;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare_float("value for param doesn't match default value", p, (float)p + MAGIC_FLOAT_VAL, 0.001f);
}
}
return ret;
}
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_writer();
perf_begin(param_set_perf);
if (param_values == NULL) {
utarray_new(param_values, ¶m_icd);
}
if (param_values == NULL) {
PX4_ERR("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
};
params_changed = true;
/* 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:
params_changed = params_changed || s->val.i != *(int32_t *)val;
s->val.i = *(int32_t *)val;
break;
case PARAM_TYPE_FLOAT:
params_changed = params_changed || fabsf(s->val.f - * (float *)val) > FLT_EPSILON;
s->val.f = *(float *)val;
break;
case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX:
if (s->val.p == NULL) {
size_t psize = param_size(param);
if (psize > 0) {
s->val.p = malloc(psize);
} else {
s->val.p = NULL;
}
if (s->val.p == NULL) {
PX4_ERR("failed to allocate parameter storage");
goto out;
}
}
memcpy(s->val.p, val, param_size(param));
params_changed = true;
break;
default:
goto out;
}
s->unsaved = !mark_saved;
result = 0;
if (!mark_saved) { // this is false when importing parameters
param_autosave();
}
}
out:
perf_end(param_set_perf);
param_unlock_writer();
/*
* 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;
}
#if defined(FLASH_BASED_PARAMS)
int param_set_external(param_t param, const void *val, bool mark_saved, bool notify_changes)
{
return param_set_internal(param, val, mark_saved, notify_changes);
}
const void *param_get_value_ptr_external(param_t param)
{
return param_get_value_ptr(param);
}
#endif
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;
}
return param_changed_storage[param_index / bits_per_allocation_unit] &
(1 << param_index % bits_per_allocation_unit);
}
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_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;
}
return param_changed_storage[param_index / bits_per_allocation_unit] &
(1 << param_index % bits_per_allocation_unit);
}
static int
do_compare(const char *name, char *vals[], unsigned comparisons, enum COMPARE_OPERATOR cmp_op)
{
int32_t i;
float f;
param_t param = param_find(name);
/* set nothing if parameter cannot be found */
if (param == PARAM_INVALID) {
/* param not found */
PX4_ERR("Parameter %s not found", name);
return 1;
}
/*
* Set parameter if type is known and conversion from string to value turns out fine
*/
int ret = 1;
switch (param_type(param)) {
case PARAM_TYPE_INT32:
if (!param_get(param, &i)) {
/* convert string */
char *end;
for (unsigned k = 0; k < comparisons; k++) {
int j = strtol(vals[k], &end, 10);
if (((cmp_op == COMPARE_OPERATOR_EQUAL) && (i == j)) ||
((cmp_op == COMPARE_OPERATOR_GREATER) && (i > j))) {
PX4_DEBUG(" %ld: ", (long)i);
ret = 0;
}
}
}
break;
case PARAM_TYPE_FLOAT:
if (!param_get(param, &f)) {
/* convert string */
char *end;
for (unsigned k = 0; k < comparisons; k++) {
float g = strtod(vals[k], &end);
if (((cmp_op == COMPARE_OPERATOR_EQUAL) && (fabsf(f - g) < 1e-7f)) ||
((cmp_op == COMPARE_OPERATOR_GREATER) && (f > g))) {
PX4_DEBUG(" %4.4f: ", (double)f);
ret = 0;
}
}
}
break;
default:
PX4_ERR("<unknown / unsupported type %d>", 0 + param_type(param));
return 1;
}
if (ret == 0) {
PX4_DEBUG("%c %s: match\n",
param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
param_name(param));
}
return ret;
}
inline void random_dirichlet(
std::size_t N, std::size_t M, const RealType *alpha, bool scalar)
{
using param_type =
typename mckl::DirichletDistribution<RealType>::param_type;
MCKLRNGType rng;
MCKLRNGType rng1;
MCKLRNGType rng2;
#if MCKL_HAS_MKL
MKLRNGType rng_mkl;
#endif
mckl::UniformIntDistribution<std::size_t> rsize(N / 2, N);
mckl::DirichletDistribution<RealType> dist;
if (scalar)
dist.param(param_type(2, alpha[0]));
else
dist.param(param_type(2, alpha));
bool pass = true;
mckl::Matrix<RealType, mckl::RowMajor> r1;
mckl::Matrix<RealType, mckl::RowMajor> r2;
#if MCKL_HAS_MKL
mckl::Matrix<RealType, mckl::RowMajor> r3;
#endif
for (std::size_t i = 0; i != M; ++i) {
std::size_t K = rsize(rng);
r1.resize(K, 2);
r2.resize(K, 2);
std::stringstream ss1;
ss1.precision(20);
ss1 << dist;
for (std::size_t j = 0; j != K; ++j)
dist(rng1, r1.data() + j * 2);
ss1 >> dist;
for (std::size_t j = 0; j != K; ++j)
dist(rng2, r2.data() + j * 2);
pass = pass && r1 == r2;
std::stringstream ssb;
ssb << dist;
mckl::rand(rng1, dist, K, r1.data());
ssb >> dist;
mckl::rand(rng2, dist, K, r2.data());
pass = pass && r1 == r2;
}
bool has_cycles = mckl::StopWatch::has_cycles();
double c1 = has_cycles ? std::numeric_limits<double>::max() : 0.0;
double c2 = has_cycles ? std::numeric_limits<double>::max() : 0.0;
#if MCKL_HAS_MKL
double c3 = has_cycles ? std::numeric_limits<double>::max() : 0.0;
#endif
for (std::size_t k = 0; k != 10; ++k) {
std::size_t num = 0;
mckl::StopWatch watch1;
mckl::StopWatch watch2;
#if MCKL_HAS_MKL
mckl::StopWatch watch3;
#endif
for (std::size_t i = 0; i != M; ++i) {
std::size_t K = rsize(rng);
num += K * 2;
r1.resize(K, 2);
r2.resize(K, 2);
#if MCKL_HAS_MKL
r3.resize(K, 2);
#endif
watch1.start();
RealType *p = r1.data();
for (std::size_t j = 0; j != K; ++j, p += 2)
dist(rng, p);
watch1.stop();
watch2.start();
mckl::rand(rng, dist, K, r2.data());
watch2.stop();
#if MCKL_HAS_MKL
watch3.start();
mckl::rand(rng_mkl, dist, K, r3.data());
watch3.stop();
#endif
pass = pass && r1 != r2;
#if MCKL_HAS_MKL
pass = pass && r1 != r3;
#endif
}
if (has_cycles) {
c1 = std::min(c1, 1.0 * watch1.cycles() / num);
c2 = std::min(c2, 1.0 * watch2.cycles() / num);
#if MCKL_HAS_MKL
c3 = std::min(c3, 1.0 * watch3.cycles() / num);
#endif
} else {
c1 = std::max(c1, num / watch1.seconds() * 1e-6);
c2 = std::max(c2, num / watch2.seconds() * 1e-6);
#if MCKL_HAS_MKL
c3 = std::max(c3, num / watch3.seconds() * 1e-6);
#endif
}
}
std::stringstream ss;
ss << "Dirichlet" << '<' << random_typename<RealType>() << ">(";
const RealType *a = dist.alpha();
ss << '{' << a[0] << ", " << a[1] << "})";
std::cout << std::setw(40) << std::left << ss.str();
std::cout << std::setw(12) << std::right << c1;
std::cout << std::setw(12) << std::right << c2;
#if MCKL_HAS_MKL
std::cout << std::setw(12) << std::right << c3;
#endif
std::cout << std::setw(15) << std::right << random_pass(pass);
std::cout << std::endl;
std::ofstream txt("random_dirichlet.txt", std::ios_base::app);
for (std::size_t i = 0; i != r1.nrow(); ++i) {
txt << r1(i, 0) << '\t' << r1(i, 1) << '\t';
txt << '"' << ss.str() << '"' << '\t' << "Single" << '\n';
}
for (std::size_t i = 0; i != r2.nrow(); ++i) {
txt << r2(i, 0) << '\t' << r2(i, 1) << '\t';
txt << '"' << ss.str() << '"' << '\t' << "Batch" << '\n';
}
#if MCKL_HAS_MKL
for (std::size_t i = 0; i != r3.nrow(); ++i) {
txt << r3(i, 0) << '\t' << r3(i, 1) << '\t';
txt << '"' << ss.str() << '"' << '\t' << "VSL" << '\n';
}
#endif
txt.close();
}
int
MavlinkParametersManager::send_param(param_t param, int component_id)
{
if (param == PARAM_INVALID) {
return 1;
}
/* no free TX buf to send this param */
if (_mavlink->get_free_tx_buf() < MAVLINK_MSG_ID_PARAM_VALUE_LEN) {
return 1;
}
mavlink_param_value_t msg;
/*
* get param value, since MAVLink encodes float and int params in the same
* space during transmission, copy param onto float val_buf
*/
if (param_get(param, &msg.param_value) != OK) {
return 2;
}
msg.param_count = param_count_used();
msg.param_index = param_get_used_index(param);
#if defined(__GNUC__) && __GNUC__ >= 8
#pragma GCC diagnostic ignored "-Wstringop-truncation"
#endif
/*
* coverity[buffer_size_warning : FALSE]
*
* The MAVLink spec does not require the string to be NUL-terminated if it
* has length 16. In this case the receiving end needs to terminate it
* when copying it.
*/
strncpy(msg.param_id, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
#if defined(__GNUC__) && __GNUC__ >= 8
#pragma GCC diagnostic pop
#endif
/* query parameter type */
param_type_t type = param_type(param);
/*
* Map onboard parameter type to MAVLink type,
* endianess matches (both little endian)
*/
if (type == PARAM_TYPE_INT32) {
msg.param_type = MAVLINK_TYPE_INT32_T;
} else if (type == PARAM_TYPE_FLOAT) {
msg.param_type = MAVLINK_TYPE_FLOAT;
} else {
msg.param_type = MAVLINK_TYPE_FLOAT;
}
/* default component ID */
if (component_id < 0) {
mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &msg);
} else {
// Re-pack the message with a different component ID
mavlink_message_t mavlink_packet;
mavlink_msg_param_value_encode_chan(mavlink_system.sysid, component_id, _mavlink->get_channel(), &mavlink_packet, &msg);
_mavlink_resend_uart(_mavlink->get_channel(), &mavlink_packet);
}
return 0;
}
IntType operator()(Engine& eng, IntType n) const
{
BOOST_ASSERT(n > 0);
return static_cast<const base_type&>(*this)(eng, param_type(0, n - 1));
}
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;
}
SPROUT_CONSTEXPR param_type param() const {
return param_type();
}
