Replay: reduced verbosity

This commit is contained in:
Andrew Tridgell 2015-07-01 21:11:46 +10:00
parent 2c0d6c0406
commit b93101b1bf
2 changed files with 17 additions and 6 deletions

View File

@ -344,22 +344,26 @@ bool LR_MsgHandler::set_parameter(const char *name, float value)
if (vp == NULL) { if (vp == NULL) {
return false; return false;
} }
float old_value = 0;
if (var_type == AP_PARAM_FLOAT) { if (var_type == AP_PARAM_FLOAT) {
old_value = ((AP_Float *)vp)->cast_to_float();
((AP_Float *)vp)->set(value); ((AP_Float *)vp)->set(value);
::printf("Set %s to %f\n", name, value);
} else if (var_type == AP_PARAM_INT32) { } else if (var_type == AP_PARAM_INT32) {
old_value = ((AP_Int32 *)vp)->cast_to_float();
((AP_Int32 *)vp)->set(value); ((AP_Int32 *)vp)->set(value);
::printf("Set %s to %d\n", name, (int)value);
} else if (var_type == AP_PARAM_INT16) { } else if (var_type == AP_PARAM_INT16) {
old_value = ((AP_Int16 *)vp)->cast_to_float();
((AP_Int16 *)vp)->set(value); ((AP_Int16 *)vp)->set(value);
::printf("Set %s to %d\n", name, (int)value);
} else if (var_type == AP_PARAM_INT8) { } else if (var_type == AP_PARAM_INT8) {
old_value = ((AP_Int8 *)vp)->cast_to_float();
((AP_Int8 *)vp)->set(value); ((AP_Int8 *)vp)->set(value);
::printf("Set %s to %d\n", name, (int)value);
} else { } else {
// we don't support mavlink set on this parameter // we don't support mavlink set on this parameter
return false; return false;
} }
if (fabsf(old_value - value) > 1.0e-4) {
::printf("Changed %s to %.6f from %.6f\n", name, value, old_value);
}
return true; return true;
} }

View File

@ -17,6 +17,13 @@
#include "MsgHandler.h" #include "MsgHandler.h"
#define DEBUG 0
#if DEBUG
# define debug(fmt, args...) printf(fmt "\n", ##args)
#else
# define debug(fmt, args...)
#endif
#define streq(x, y) (!strcmp(x, y)) #define streq(x, y) (!strcmp(x, y))
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
@ -92,7 +99,7 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f) {
char name[5]; char name[5];
memset(name, '\0', 5); memset(name, '\0', 5);
memcpy(name, f.name, 4); memcpy(name, f.name, 4);
::printf("Defining log format for type (%d) (%s)\n", f.type, name); debug("Defining log format for type (%d) (%s)\n", f.type, name);
if (!in_list(name, generated_names)) { if (!in_list(name, generated_names)) {
// any messages which we won't be generating internally in // any messages which we won't be generating internally in
@ -192,7 +199,7 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f) {
last_timestamp_usec, last_timestamp_usec,
check_state); check_state);
} else { } else {
::printf(" No parser for (%s)\n", name); debug(" No parser for (%s)\n", name);
} }
return true; return true;