Commit 859b3453 authored by Lorenz Meier's avatar Lorenz Meier
Browse files

Mag cal: Make error handling more explicit

parent 12ce2de7
......@@ -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);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment