diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2015-01-19 14:07:00 +0100 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2015-01-21 08:58:47 +0100 |
| commit | 859b34531473c9c78a4fee04de3cbdd147381913 (patch) | |
| tree | ed6c32556183717c22cdd5d10db395737e9f6260 | |
| parent | 12ce2de7ae0aa62f9501f7f53837772a6c662724 (diff) | |
| download | px4-firmware-859b34531473c9c78a4fee04de3cbdd147381913.tar.gz px4-firmware-859b34531473c9c78a4fee04de3cbdd147381913.tar.bz2 px4-firmware-859b34531473c9c78a4fee04de3cbdd147381913.zip | |
Mag cal: Make error handling more explicit
| -rw-r--r-- | src/modules/commander/mag_calibration.cpp | 72 |
1 files changed, 39 insertions, 33 deletions
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 9d2abd3ce..53013fdb9 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -150,54 +150,60 @@ int do_mag_calibration(int mavlink_fd) if (res == OK) { int sub_mag = orb_subscribe(ORB_ID(sensor_mag0)); - struct mag_report mag; - /* limit update rate to get equally spaced measurements over time (in ms) */ - orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); + if (sub_mag < 0) { + mavlink_log_critical(mavlink_fd, "No mag found, abort"); + res = ERROR; + } else { + struct mag_report mag; + + /* limit update rate to get equally spaced measurements over time (in ms) */ + orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); - /* calibrate offsets */ - uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; - unsigned poll_errcount = 0; + /* calibrate offsets */ + 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_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down"); - calibration_counter = 0; + calibration_counter = 0U; - while (hrt_absolute_time() < calibration_deadline && - calibration_counter < calibration_maxcount) { + while (hrt_absolute_time() < calibration_deadline && + calibration_counter < calibration_maxcount) { - /* wait blocking for new data */ - struct pollfd fds[1]; - fds[0].fd = sub_mag; - fds[0].events = POLLIN; + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = sub_mag; + fds[0].events = POLLIN; - int poll_ret = poll(fds, 1, 1000); + int poll_ret = poll(fds, 1, 1000); - if (poll_ret > 0) { - orb_copy(ORB_ID(sensor_mag0), sub_mag, &mag); + if (poll_ret > 0) { + orb_copy(ORB_ID(sensor_mag0), sub_mag, &mag); - x[calibration_counter] = mag.x; - y[calibration_counter] = mag.y; - z[calibration_counter] = mag.z; + x[calibration_counter] = mag.x; + y[calibration_counter] = mag.y; + z[calibration_counter] = mag.z; - calibration_counter++; + 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); + if (calibration_counter % (calibration_maxcount / 20) == 0) { + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount); + } + + } else { + poll_errcount++; } - } else { - poll_errcount++; + if (poll_errcount > 1000) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); + res = ERROR; + break; + } } - if (poll_errcount > 1000) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); - res = ERROR; - break; - } + close(sub_mag); } - - close(sub_mag); } float sphere_x; @@ -205,7 +211,7 @@ int do_mag_calibration(int mavlink_fd) float sphere_z; float sphere_radius; - if (res == OK) { + if (res == OK && calibration_counter > (calibration_maxcount / 2)) { /* sphere fit */ mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70); |