diff options
-rw-r--r-- | src/modules/systemlib/param/param.c | 9 | ||||
-rw-r--r-- | src/modules/systemlib/param/param.h | 5 | ||||
-rw-r--r-- | src/systemcmds/param/param.c | 105 |
3 files changed, 79 insertions, 40 deletions
diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 6b8d0e634..7f5caf18f 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -441,10 +441,11 @@ param_set(param_t param, const void *val) return param_set_internal(param, val, false); } -void +int param_reset(param_t param) { struct param_wbuf_s *s = NULL; + bool param_found = false; param_lock(); @@ -458,12 +459,16 @@ param_reset(param_t param) int pos = utarray_eltidx(param_values, s); utarray_erase(param_values, pos, 1); } + + param_found = true; } param_unlock(); if (s != NULL) param_notify_changes(); + + return (!param_found); } void diff --git a/src/modules/systemlib/param/param.h b/src/modules/systemlib/param/param.h index dc73b37e9..e003572d9 100644 --- a/src/modules/systemlib/param/param.h +++ b/src/modules/systemlib/param/param.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -178,8 +178,9 @@ __EXPORT int param_set(param_t param, const void *val); * to its default value. * * @param param A handle returned by param_find or passed by param_foreach. + * @return Zero on success, nonzero on failure */ -__EXPORT void param_reset(param_t param); +__EXPORT int param_reset(param_t param); /** * Reset all parameters to their default values. diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 80ee204e8..0b6d3b8d3 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -57,13 +57,13 @@ __EXPORT int param_main(int argc, char *argv[]); -static void do_save(const char* param_file_name); -static void do_load(const char* param_file_name); -static void do_import(const char* param_file_name); -static void do_show(const char* search_string); +static void do_save(const char *param_file_name); +static void do_load(const char *param_file_name); +static void do_import(const char *param_file_name); +static void do_show(const char *search_string); static void do_show_print(void *arg, param_t param); -static void do_set(const char* name, const char* val, bool fail_on_not_found); -static void do_compare(const char* name, char* vals[], unsigned comparisons); +static void do_set(const char *name, const char *val, bool fail_on_not_found); +static void do_compare(const char *name, char *vals[], unsigned comparisons); static void do_reset(void); static void do_reset_nostart(void); @@ -74,10 +74,12 @@ param_main(int argc, char *argv[]) if (!strcmp(argv[1], "save")) { if (argc >= 3) { do_save(argv[2]); + } else { if (param_save_default()) { warnx("Param export failed."); exit(1); + } else { exit(0); } @@ -87,6 +89,7 @@ param_main(int argc, char *argv[]) if (!strcmp(argv[1], "load")) { if (argc >= 3) { do_load(argv[2]); + } else { do_load(param_get_default_file()); } @@ -95,6 +98,7 @@ param_main(int argc, char *argv[]) if (!strcmp(argv[1], "import")) { if (argc >= 3) { do_import(argv[2]); + } else { do_import(param_get_default_file()); } @@ -103,9 +107,11 @@ param_main(int argc, char *argv[]) if (!strcmp(argv[1], "select")) { if (argc >= 3) { param_set_default_file(argv[2]); + } else { param_set_default_file(NULL); } + warnx("selected parameter default file %s", param_get_default_file()); exit(0); } @@ -113,6 +119,7 @@ param_main(int argc, char *argv[]) if (!strcmp(argv[1], "show")) { if (argc >= 3) { do_show(argv[2]); + } else { do_show(NULL); } @@ -128,6 +135,7 @@ param_main(int argc, char *argv[]) } else if (argc >= 4) { do_set(argv[2], argv[3], false); + } else { errx(1, "not enough arguments.\nTry 'param set PARAM_NAME 3 [fail]'"); } @@ -136,6 +144,7 @@ param_main(int argc, char *argv[]) if (!strcmp(argv[1], "compare")) { if (argc >= 4) { do_compare(argv[2], &argv[3], argc - 3); + } else { errx(1, "not enough arguments.\nTry 'param compare PARAM_NAME 3'"); } @@ -149,18 +158,19 @@ param_main(int argc, char *argv[]) do_reset_nostart(); } } - + errx(1, "expected a command, try 'load', 'import', 'show', 'set', 'compare', 'select' or 'save'"); } static void -do_save(const char* param_file_name) +do_save(const char *param_file_name) { /* create the file */ int fd = open(param_file_name, O_WRONLY | O_CREAT); - if (fd < 0) + if (fd < 0) { err(1, "opening '%s' failed", param_file_name); + } int result = param_export(fd, false); close(fd); @@ -174,12 +184,13 @@ do_save(const char* param_file_name) } static void -do_load(const char* param_file_name) +do_load(const char *param_file_name) { int fd = open(param_file_name, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "open '%s'", param_file_name); + } int result = param_load(fd); close(fd); @@ -192,24 +203,26 @@ do_load(const char* param_file_name) } static void -do_import(const char* param_file_name) +do_import(const char *param_file_name) { int fd = open(param_file_name, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "open '%s'", param_file_name); + } int result = param_import(fd); close(fd); - if (result < 0) + if (result < 0) { errx(1, "error importing from '%s'", param_file_name); + } exit(0); } static void -do_show(const char* search_string) +do_show(const char *search_string) { printf(" + = saved, * = unsaved\n"); param_foreach(do_show_print, (char *)search_string, false); @@ -222,8 +235,8 @@ 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); + 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)) { @@ -238,6 +251,7 @@ do_show_print(void *arg, param_t param) if (*ss == *pp) { ss++; pp++; + } else if (*ss == '*') { if (*(ss + 1) != '\0') { warnx("* symbol only allowed at end of search string."); @@ -245,6 +259,7 @@ do_show_print(void *arg, param_t param) } pp++; + } else { /* param not found */ return; @@ -252,8 +267,9 @@ do_show_print(void *arg, param_t param) } /* the search string must have been consumed */ - if (!(*ss == '\0' || *ss == '*')) + if (!(*ss == '\0' || *ss == '*')) { return; + } } printf("%c %s: ", @@ -294,7 +310,7 @@ do_show_print(void *arg, param_t param) } static void -do_set(const char* name, const char* val, bool fail_on_not_found) +do_set(const char *name, const char *val, bool fail_on_not_found) { int32_t i; float f; @@ -317,27 +333,41 @@ do_set(const char* name, const char* val, bool fail_on_not_found) switch (param_type(param)) { case PARAM_TYPE_INT32: if (!param_get(param, &i)) { - printf("curr: %d", i); /* convert string */ - char* end; - i = strtol(val,&end,10); - param_set(param, &i); - printf(" -> new: %d\n", i); + 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)) { - printf("curr: %4.4f", (double)f); /* convert string */ - char* end; - f = strtod(val,&end); - param_set(param, &f); - printf(" -> new: %4.4f\n", (double)f); + 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); + } } @@ -351,7 +381,7 @@ do_set(const char* name, const char* val, bool fail_on_not_found) } static void -do_compare(const char* name, char* vals[], unsigned comparisons) +do_compare(const char *name, char *vals[], unsigned comparisons) { int32_t i; float f; @@ -374,11 +404,11 @@ do_compare(const char* name, char* vals[], unsigned comparisons) if (!param_get(param, &i)) { /* convert string */ - char* end; + char *end; for (unsigned k = 0; k < comparisons; k++) { - int j = strtol(vals[k],&end,10); + int j = strtol(vals[k], &end, 10); if (i == j) { printf(" %d: ", i); @@ -393,14 +423,15 @@ do_compare(const char* name, char* vals[], unsigned comparisons) if (!param_get(param, &f)) { /* convert string */ - char* end; + 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; + ret = 0; } } } @@ -413,8 +444,8 @@ do_compare(const char* name, char* vals[], unsigned comparisons) if (ret == 0) { printf("%c %s: match\n", - param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), - param_name(param)); + param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), + param_name(param)); } exit(ret); @@ -428,6 +459,7 @@ do_reset(void) if (param_save_default()) { warnx("Param export failed."); exit(1); + } else { exit(0); } @@ -451,6 +483,7 @@ do_reset_nostart(void) if (param_save_default()) { warnx("Param export failed."); exit(1); + } else { exit(0); } |