static int do_show(const char *search_string, bool only_changed) { PARAM_PRINT("Symbols: x = used, + = saved, * = unsaved\n"); param_foreach(do_show_print, (char *)search_string, only_changed, false); PARAM_PRINT("\n %u parameters total, %u used.\n", param_count(), param_count_used()); return 0; }
static int do_show_index(const char *index, bool used_index) { char *end; int i = strtol(index, &end, 10); param_t param; int32_t ii; float ff; if (used_index) { param = param_for_used_index(i); } else { param = param_for_index(i); } if (param == PARAM_INVALID) { PX4_ERR("param not found for index %u", i); return 1; } PARAM_PRINT("index %d: %c %c %s [%d,%d] : ", i, (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)); switch (param_type(param)) { case PARAM_TYPE_INT32: if (!param_get(param, &ii)) { PARAM_PRINT("%ld\n", (long)ii); } break; case PARAM_TYPE_FLOAT: if (!param_get(param, &ff)) { PARAM_PRINT("%4.4f\n", (double)ff); } break; default: PARAM_PRINT("<unknown type %d>\n", 0 + param_type(param)); } return 0; }
void MsgEnvironment::print() { std::cout << "####################:\n"; std::cout << "MsgEnvironment:\n"; std::cout << "####################:\n"; std::cout << PARAM_PRINT(message); std::cout << PARAM_PRINT(time); std::cout << PARAM_PRINT(x_max); std::cout << PARAM_PRINT(y_max); std::cout << PARAM_PRINT(x_base_robot_start); std::cout << PARAM_PRINT(y_base_robot_start); std::cout << PARAM_PRINT(x_base_left_corner); std::cout << PARAM_PRINT(y_base_left_corner); std::cout << PARAM_PRINT(phi_base); }
static int do_find(const char *name) { param_t ret = param_find_no_notification(name); if (ret == PARAM_INVALID) { PX4_ERR("Parameter %s not found", name); return 1; } PARAM_PRINT("Found param %s at index %" PRIxPTR "\n", name, ret); return 0; }
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; } }
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); }