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(); }