diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2015-02-09 14:50:07 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2015-02-09 22:56:25 +0100 |
commit | 3079b5560f97f45ec0e95c6aca72117eabaa81da (patch) | |
tree | c0687a416c7f886e323acf84adc7a8c04ee0d7a1 /src/modules/commander | |
parent | 3a436498507974f7286d6d18d27c1f8bd1f5f9e8 (diff) | |
download | px4-firmware-3079b5560f97f45ec0e95c6aca72117eabaa81da.tar.gz px4-firmware-3079b5560f97f45ec0e95c6aca72117eabaa81da.tar.bz2 px4-firmware-3079b5560f97f45ec0e95c6aca72117eabaa81da.zip |
commander: Mag calibration provides feedback on console as well
Diffstat (limited to 'src/modules/commander')
-rw-r--r-- | src/modules/commander/mag_calibration.cpp | 40 |
1 files changed, 20 insertions, 20 deletions
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index d5ff0c3f8..a94b478c4 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -70,7 +70,7 @@ int do_mag_calibration(int mavlink_fd) const unsigned max_mags = 3; int32_t device_id[max_mags]; - mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); + mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); sleep(1); struct mag_scale mscale_null[max_mags] = { @@ -100,7 +100,7 @@ int do_mag_calibration(int mavlink_fd) continue; } - mavlink_log_info(mavlink_fd, "Calibrating magnetometer #%u..", s); + mavlink_and_console_log_info(mavlink_fd, "Calibrating magnetometer #%u..", s); sleep(3); device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0); @@ -111,7 +111,7 @@ int do_mag_calibration(int mavlink_fd) res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null[s]); if (res != OK) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG); } if (res == OK) { @@ -119,7 +119,7 @@ int do_mag_calibration(int mavlink_fd) res = ioctl(fd, MAGIOCCALIBRATE, fd); if (res != OK) { - mavlink_log_info(mavlink_fd, "Skipped scale calibration"); + mavlink_and_console_log_info(mavlink_fd, "Skipped scale calibration"); /* this is non-fatal - mark it accordingly */ res = OK; } @@ -137,7 +137,7 @@ int do_mag_calibration(int mavlink_fd) } if (calibrated_ok) { - mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); + mavlink_and_console_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); /* auto-save to EEPROM */ res = param_save_default(); @@ -169,14 +169,14 @@ int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id) int res = OK; /* allocate memory */ - mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20); + mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20); x = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount)); y = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount)); z = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount)); if (x == NULL || y == NULL || z == NULL) { - mavlink_log_critical(mavlink_fd, "ERROR: out of memory"); + mavlink_and_console_log_critical(mavlink_fd, "ERROR: out of memory"); /* clean up */ if (x != NULL) { @@ -199,7 +199,7 @@ int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id) int sub_mag = orb_subscribe_multi(ORB_ID(sensor_mag), s); if (sub_mag < 0) { - mavlink_log_critical(mavlink_fd, "No mag found, abort"); + mavlink_and_console_log_critical(mavlink_fd, "No mag found, abort"); res = ERROR; } else { struct mag_report mag; @@ -211,7 +211,7 @@ int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id) uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; unsigned poll_errcount = 0; - mavlink_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down"); + mavlink_and_console_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down"); calibration_counter = 0U; @@ -235,7 +235,7 @@ int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id) calibration_counter++; if (calibration_counter % (calibration_maxcount / 20) == 0) { - mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount); + mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount); } } else { @@ -243,7 +243,7 @@ int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id) } if (poll_errcount > 1000) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); res = ERROR; break; } @@ -261,12 +261,12 @@ int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id) if (res == OK && calibration_counter > (calibration_maxcount / 2)) { /* sphere fit */ - mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70); + mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70); sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); - mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 80); + mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 80); if (!isfinite(sphere_x) || !isfinite(sphere_y) || !isfinite(sphere_z)) { - mavlink_log_critical(mavlink_fd, "ERROR: NaN in sphere fit"); + mavlink_and_console_log_critical(mavlink_fd, "ERROR: NaN in sphere fit"); res = ERROR; } } @@ -291,7 +291,7 @@ int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id) res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale); if (res != OK) { - mavlink_log_critical(mavlink_fd, "ERROR: failed to get current calibration"); + mavlink_and_console_log_critical(mavlink_fd, "ERROR: failed to get current calibration"); } if (res == OK) { @@ -302,7 +302,7 @@ int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id) res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale); if (res != OK) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); } } @@ -329,15 +329,15 @@ int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id) if (failed) { res = ERROR; - mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); } - mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 90); + mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 90); } - mavlink_log_info(mavlink_fd, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset, + mavlink_and_console_log_info(mavlink_fd, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset); - mavlink_log_info(mavlink_fd, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale, + mavlink_and_console_log_info(mavlink_fd, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale); } |