static calibrate_return mag_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data)
{
calibrate_return result = calibrate_return_ok;
unsigned int calibration_counter_side;
mag_worker_data_t* worker_data = (mag_worker_data_t*)(data);
calibration_log_info(worker_data->mavlink_log_pub, "[cal] Rotate vehicle around the detected orientation");
calibration_log_info(worker_data->mavlink_log_pub, "[cal] Continue rotation for %s %u s", detect_orientation_str(orientation), worker_data->calibration_interval_perside_seconds);
/*
* Detect if the system is rotating.
*
* We're detecting this as a general rotation on any axis, not necessary on the one we
* asked the user for. This is because we really just need two roughly orthogonal axes
* for a good result, so we're not constraining the user more than we have to.
*/
hrt_abstime detection_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds * 5;
hrt_abstime last_gyro = 0;
float gyro_x_integral = 0.0f;
float gyro_y_integral = 0.0f;
float gyro_z_integral = 0.0f;
const float gyro_int_thresh_rad = 0.5f;
int sub_gyro = orb_subscribe(ORB_ID(sensor_gyro));
while (fabsf(gyro_x_integral) < gyro_int_thresh_rad &&
fabsf(gyro_y_integral) < gyro_int_thresh_rad &&
fabsf(gyro_z_integral) < gyro_int_thresh_rad) {
/* abort on request */
if (calibrate_cancel_check(worker_data->mavlink_log_pub, cancel_sub)) {
result = calibrate_return_cancelled;
px4_close(sub_gyro);
return result;
}
/* abort with timeout */
if (hrt_absolute_time() > detection_deadline) {
result = calibrate_return_error;
warnx("int: %8.4f, %8.4f, %8.4f", (double)gyro_x_integral, (double)gyro_y_integral, (double)gyro_z_integral);
calibration_log_critical(worker_data->mavlink_log_pub, "Failed: This calibration requires rotation.");
break;
}
/* Wait clocking for new data on all gyro */
px4_pollfd_struct_t fds[1];
fds[0].fd = sub_gyro;
fds[0].events = POLLIN;
size_t fd_count = 1;
int poll_ret = px4_poll(fds, fd_count, 1000);
if (poll_ret > 0) {
struct gyro_report gyro;
orb_copy(ORB_ID(sensor_gyro), sub_gyro, &gyro);
/* ensure we have a valid first timestamp */
if (last_gyro > 0) {
/* integrate */
float delta_t = (gyro.timestamp - last_gyro) / 1e6f;
gyro_x_integral += gyro.x * delta_t;
gyro_y_integral += gyro.y * delta_t;
gyro_z_integral += gyro.z * delta_t;
}
last_gyro = gyro.timestamp;
}
}
px4_close(sub_gyro);
uint64_t calibration_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds;
unsigned poll_errcount = 0;
calibration_counter_side = 0;
while (hrt_absolute_time() < calibration_deadline &&
calibration_counter_side < worker_data->calibration_points_perside) {
if (calibrate_cancel_check(worker_data->mavlink_log_pub, cancel_sub)) {
result = calibrate_return_cancelled;
break;
}
// Wait clocking for new data on all mags
px4_pollfd_struct_t fds[max_mags];
size_t fd_count = 0;
for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (worker_data->sub_mag[cur_mag] >= 0 && device_ids[cur_mag] != 0) {
fds[fd_count].fd = worker_data->sub_mag[cur_mag];
fds[fd_count].events = POLLIN;
fd_count++;
}
}
int poll_ret = px4_poll(fds, fd_count, 1000);
if (poll_ret > 0) {
int prev_count[max_mags];
bool rejected = false;
for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
prev_count[cur_mag] = worker_data->calibration_counter_total[cur_mag];
if (worker_data->sub_mag[cur_mag] >= 0) {
struct mag_report mag;
orb_copy(ORB_ID(sensor_mag), worker_data->sub_mag[cur_mag], &mag);
// Check if this measurement is good to go in
rejected = rejected || reject_sample(mag.x, mag.y, mag.z,
worker_data->x[cur_mag], worker_data->y[cur_mag], worker_data->z[cur_mag],
worker_data->calibration_counter_total[cur_mag],
calibration_sides * worker_data->calibration_points_perside);
worker_data->x[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.x;
worker_data->y[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.y;
worker_data->z[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.z;
worker_data->calibration_counter_total[cur_mag]++;
}
}
// Keep calibration of all mags in lockstep
if (rejected) {
// Reset counts, since one of the mags rejected the measurement
for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
worker_data->calibration_counter_total[cur_mag] = prev_count[cur_mag];
}
} else {
calibration_counter_side++;
unsigned new_progress = progress_percentage(worker_data) +
(unsigned)((100 / calibration_sides) * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside));
if (new_progress - _last_mag_progress > 3) {
// Progress indicator for side
calibration_log_info(worker_data->mavlink_log_pub,
"[cal] %s side calibration: progress <%u>",
detect_orientation_str(orientation), new_progress);
usleep(20000);
_last_mag_progress = new_progress;
}
}
} else {
poll_errcount++;
}
if (poll_errcount > worker_data->calibration_points_perside * 3) {
result = calibrate_return_error;
calibration_log_info(worker_data->mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
break;
}
}
if (result == calibrate_return_ok) {
calibration_log_info(worker_data->mavlink_log_pub, "[cal] %s side done, rotate to a different side", detect_orientation_str(orientation));
worker_data->done_count++;
usleep(20000);
calibration_log_info(worker_data->mavlink_log_pub, CAL_QGC_PROGRESS_MSG, progress_percentage(worker_data));
}
return result;
}
calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
{
calibrate_return result = calibrate_return_ok;
mag_worker_data_t worker_data;
worker_data.mavlink_log_pub = mavlink_log_pub;
worker_data.done_count = 0;
worker_data.calibration_points_perside = calibration_total_points / calibration_sides;
worker_data.calibration_interval_perside_seconds = calibraton_duration_seconds / calibration_sides;
worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000;
// Collect: As defined by configuration
// start with a full mask, all six bits set
uint32_t cal_mask = (1 << 6) - 1;
param_get(param_find("CAL_MAG_SIDES"), &cal_mask);
calibration_sides = 0;
for (unsigned i = 0; i < (sizeof(worker_data.side_data_collected) /
sizeof(worker_data.side_data_collected[0])); i++) {
if ((cal_mask & (1 << i)) > 0) {
// mark as missing
worker_data.side_data_collected[i] = false;
calibration_sides++;
} else {
// mark as completed from the beginning
worker_data.side_data_collected[i] = true;
calibration_log_info(mavlink_log_pub,
"[cal] %s side done, rotate to a different side",
detect_orientation_str(static_cast<enum detect_orientation_return>(i)));
usleep(100000);
}
}
for (size_t cur_mag = 0; cur_mag<max_mags; cur_mag++) {
// Initialize to no subscription
worker_data.sub_mag[cur_mag] = -1;
// Initialize to no memory allocated
worker_data.x[cur_mag] = NULL;
worker_data.y[cur_mag] = NULL;
worker_data.z[cur_mag] = NULL;
worker_data.calibration_counter_total[cur_mag] = 0;
}
const unsigned int calibration_points_maxcount = calibration_sides * worker_data.calibration_points_perside;
char str[30];
for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
worker_data.x[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
worker_data.y[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
worker_data.z[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
if (worker_data.x[cur_mag] == NULL || worker_data.y[cur_mag] == NULL || worker_data.z[cur_mag] == NULL) {
calibration_log_critical(mavlink_log_pub, "[cal] ERROR: out of memory");
result = calibrate_return_error;
}
}
// Setup subscriptions to mag sensors
if (result == calibrate_return_ok) {
// We should not try to subscribe if the topic doesn't actually exist and can be counted.
const unsigned mag_count = orb_group_count(ORB_ID(sensor_mag));
for (unsigned cur_mag = 0; cur_mag < mag_count; cur_mag++) {
// Mag in this slot is available
worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), cur_mag);
#if defined(__PX4_QURT) || defined(__PX4_POSIX_RPI2)
// For QURT respectively the driver framework, we need to get the device ID by copying one report.
struct mag_report mag_report;
orb_copy(ORB_ID(sensor_mag), worker_data.sub_mag[cur_mag], &mag_report);
device_ids[cur_mag] = mag_report.device_id;
#endif
if (worker_data.sub_mag[cur_mag] < 0) {
calibration_log_critical(mavlink_log_pub, "[cal] Mag #%u not found, abort", cur_mag);
result = calibrate_return_error;
break;
}
if (device_ids[cur_mag] != 0) {
// Get priority
int32_t prio;
orb_priority(worker_data.sub_mag[cur_mag], &prio);
if (prio > device_prio_max) {
device_prio_max = prio;
device_id_primary = device_ids[cur_mag];
}
} else {
calibration_log_critical(mavlink_log_pub, "[cal] Mag #%u no device id, abort", cur_mag);
result = calibrate_return_error;
break;
}
}
}
// Limit update rate to get equally spaced measurements over time (in ms)
if (result == calibrate_return_ok) {
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {
// Mag in this slot is available
unsigned int orb_interval_msecs = (worker_data.calibration_interval_perside_useconds / 1000) / worker_data.calibration_points_perside;
//calibration_log_info(mavlink_log_pub, "Orb interval %u msecs", orb_interval_msecs);
orb_set_interval(worker_data.sub_mag[cur_mag], orb_interval_msecs);
}
}
}
if (result == calibrate_return_ok) {
int cancel_sub = calibrate_cancel_subscribe();
result = calibrate_from_orientation(mavlink_log_pub, // uORB handle to write output
cancel_sub, // Subscription to vehicle_command for cancel support
worker_data.side_data_collected, // Sides to calibrate
mag_calibration_worker, // Calibration worker
&worker_data, // Opaque data for calibration worked
true); // true: lenient still detection
calibrate_cancel_unsubscribe(cancel_sub);
}
// Close subscriptions
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (worker_data.sub_mag[cur_mag] >= 0) {
px4_close(worker_data.sub_mag[cur_mag]);
}
}
// Calculate calibration values for each mag
float sphere_x[max_mags];
float sphere_y[max_mags];
float sphere_z[max_mags];
float sphere_radius[max_mags];
// Sphere fit the data to get calibration values
if (result == calibrate_return_ok) {
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {
// Mag in this slot is available and we should have values for it to calibrate
sphere_fit_least_squares(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag],
worker_data.calibration_counter_total[cur_mag],
100, 0.0f,
&sphere_x[cur_mag], &sphere_y[cur_mag], &sphere_z[cur_mag],
&sphere_radius[cur_mag]);
if (!PX4_ISFINITE(sphere_x[cur_mag]) || !PX4_ISFINITE(sphere_y[cur_mag]) || !PX4_ISFINITE(sphere_z[cur_mag])) {
calibration_log_emergency(mavlink_log_pub, "ERROR: Retry calibration (sphere NaN, #%u)", cur_mag);
result = calibrate_return_error;
}
if (fabsf(sphere_x[cur_mag]) > MAG_MAX_OFFSET_LEN ||
fabsf(sphere_y[cur_mag]) > MAG_MAX_OFFSET_LEN ||
fabsf(sphere_z[cur_mag]) > MAG_MAX_OFFSET_LEN) {
calibration_log_critical(mavlink_log_pub, "Warning: %s mag with large offsets", (internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external");
calibration_log_info(mavlink_log_pub, "Offsets: x: %8.4f, y: %8.4f, z: %8.4f, #%u", (double)sphere_x[cur_mag],
(double)sphere_y[cur_mag], (double)sphere_z[cur_mag], cur_mag);
result = calibrate_return_ok;
}
}
}
}
// Print uncalibrated data points
if (result == calibrate_return_ok) {
// DO NOT REMOVE! Critical validation data!
// printf("RAW DATA:\n--------------------\n");
// for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
// if (worker_data.calibration_counter_total[cur_mag] == 0) {
// continue;
// }
// printf("RAW: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]);
// for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {
// float x = worker_data.x[cur_mag][i];
// float y = worker_data.y[cur_mag][i];
// float z = worker_data.z[cur_mag][i];
// printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z);
// }
// printf(">>>>>>>\n");
// }
// printf("CALIBRATED DATA:\n--------------------\n");
// for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
// if (worker_data.calibration_counter_total[cur_mag] == 0) {
// continue;
// }
// printf("Calibrated: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]);
// for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {
// float x = worker_data.x[cur_mag][i] - sphere_x[cur_mag];
// float y = worker_data.y[cur_mag][i] - sphere_y[cur_mag];
// float z = worker_data.z[cur_mag][i] - sphere_z[cur_mag];
// printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z);
// }
// printf("SPHERE RADIUS: %8.4f\n", (double)sphere_radius[cur_mag]);
// printf(">>>>>>>\n");
// }
}
// Data points are no longer needed
for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
free(worker_data.x[cur_mag]);
free(worker_data.y[cur_mag]);
free(worker_data.z[cur_mag]);
}
if (result == calibrate_return_ok) {
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {
struct mag_calibration_s mscale;
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI2)
int fd_mag = -1;
// Set new scale
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
fd_mag = px4_open(str, 0);
if (fd_mag < 0) {
calibration_log_critical(mavlink_log_pub, "[cal] ERROR: unable to open mag device #%u", cur_mag);
result = calibrate_return_error;
}
if (result == calibrate_return_ok) {
if (px4_ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale) != OK) {
calibration_log_critical(mavlink_log_pub, "[cal] ERROR: failed to get current calibration #%u", cur_mag);
result = calibrate_return_error;
}
}
#endif
if (result == calibrate_return_ok) {
mscale.x_offset = sphere_x[cur_mag];
mscale.y_offset = sphere_y[cur_mag];
mscale.z_offset = sphere_z[cur_mag];
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI2)
if (px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, cur_mag);
result = calibrate_return_error;
}
#endif
}
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI2)
// Mag device no longer needed
if (fd_mag >= 0) {
px4_close(fd_mag);
}
#endif
if (result == calibrate_return_ok) {
bool failed = false;
/* set parameters */
(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(device_ids[cur_mag])));
(void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_offset)));
(void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_offset)));
(void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_offset)));
// FIXME: scaling is not used right now on QURT
#ifndef __PX4_QURT
(void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_scale)));
(void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_scale)));
(void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_scale)));
#endif
if (failed) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, cur_mag);
result = calibrate_return_error;
} else {
calibration_log_info(mavlink_log_pub, "[cal] mag #%u off: x:%.2f y:%.2f z:%.2f Ga",
cur_mag,
(double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset);
#ifndef __PX4_QURT
calibration_log_info(mavlink_log_pub, "[cal] mag #%u scale: x:%.2f y:%.2f z:%.2f",
cur_mag,
(double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale);
#endif
usleep(200000);
}
}
}
}
// Trigger a param set on the last step so the whole
// system updates
(void)param_set(param_find("CAL_MAG_PRIME"), &(device_id_primary));
}
return result;
}
calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
int cancel_sub,
bool side_data_collected[detect_orientation_side_count],
calibration_from_orientation_worker_t calibration_worker,
void *worker_data,
bool lenient_still_position)
{
calibrate_return result = calibrate_return_ok;
// Setup subscriptions to onboard accel sensor
int sub_accel = orb_subscribe(ORB_ID(sensor_combined));
if (sub_accel < 0) {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "No onboard accel");
return calibrate_return_error;
}
unsigned orientation_failures = 0;
// Rotate through all requested orientation
while (true) {
if (calibrate_cancel_check(mavlink_log_pub, cancel_sub)) {
result = calibrate_return_cancelled;
break;
}
if (orientation_failures > 4) {
result = calibrate_return_error;
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "timeout: no motion");
break;
}
unsigned int side_complete_count = 0;
// Update the number of completed sides
for (unsigned i = 0; i < detect_orientation_side_count; i++) {
if (side_data_collected[i]) {
side_complete_count++;
}
}
if (side_complete_count == detect_orientation_side_count) {
// We have completed all sides, move on
break;
}
/* inform user which orientations are still needed */
char pendingStr[80];
pendingStr[0] = 0;
for (unsigned int cur_orientation = 0; cur_orientation < detect_orientation_side_count; cur_orientation++) {
if (!side_data_collected[cur_orientation]) {
strncat(pendingStr, " ", sizeof(pendingStr) - 1);
strncat(pendingStr, detect_orientation_str((enum detect_orientation_return)cur_orientation), sizeof(pendingStr) - 1);
}
}
calibration_log_info(mavlink_log_pub, "[cal] pending:%s", pendingStr);
usleep(20000);
calibration_log_info(mavlink_log_pub, "[cal] hold vehicle still on a pending side");
usleep(20000);
enum detect_orientation_return orient = detect_orientation(mavlink_log_pub, cancel_sub, sub_accel,
lenient_still_position);
if (orient == DETECT_ORIENTATION_ERROR) {
orientation_failures++;
calibration_log_info(mavlink_log_pub, "[cal] detected motion, hold still...");
usleep(20000);
continue;
}
/* inform user about already handled side */
if (side_data_collected[orient]) {
orientation_failures++;
set_tune(TONE_NOTIFY_NEGATIVE_TUNE);
calibration_log_info(mavlink_log_pub, "[cal] %s side already completed", detect_orientation_str(orient));
usleep(20000);
continue;
}
calibration_log_info(mavlink_log_pub, CAL_QGC_ORIENTATION_DETECTED_MSG, detect_orientation_str(orient));
usleep(20000);
calibration_log_info(mavlink_log_pub, CAL_QGC_ORIENTATION_DETECTED_MSG, detect_orientation_str(orient));
usleep(20000);
orientation_failures = 0;
// Call worker routine
result = calibration_worker(orient, cancel_sub, worker_data);
if (result != calibrate_return_ok) {
break;
}
calibration_log_info(mavlink_log_pub, CAL_QGC_SIDE_DONE_MSG, detect_orientation_str(orient));
usleep(20000);
calibration_log_info(mavlink_log_pub, CAL_QGC_SIDE_DONE_MSG, detect_orientation_str(orient));
usleep(20000);
// Note that this side is complete
side_data_collected[orient] = true;
// output neutral tune
set_tune(TONE_NOTIFY_NEUTRAL_TUNE);
// temporary priority boost for the white blinking led to come trough
rgbled_set_color_and_mode(led_control_s::COLOR_WHITE, led_control_s::MODE_BLINK_FAST, 3, 1);
usleep(200000);
}
if (sub_accel >= 0) {
px4_close(sub_accel);
}
return result;
}
static calibrate_return accel_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data)
{
const unsigned samples_num = 750;
accel_worker_data_t* worker_data = (accel_worker_data_t*)(data);
calibration_log_info(worker_data->mavlink_log_pub, "[cal] Hold still, measuring %s side", detect_orientation_str(orientation));
read_accelerometer_avg(worker_data->sensor_correction_sub, worker_data->subs, worker_data->accel_ref, orientation, samples_num);
calibration_log_info(worker_data->mavlink_log_pub, "[cal] %s side result: [%8.4f %8.4f %8.4f]", detect_orientation_str(orientation),
(double)worker_data->accel_ref[0][orientation][0],
(double)worker_data->accel_ref[0][orientation][1],
(double)worker_data->accel_ref[0][orientation][2]);
worker_data->done_count++;
calibration_log_info(worker_data->mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 17 * worker_data->done_count);
return calibrate_return_ok;
}
calibrate_return calibrate_from_orientation(int mavlink_fd,
int cancel_sub,
bool side_data_collected[detect_orientation_side_count],
calibration_from_orientation_worker_t calibration_worker,
void* worker_data,
bool lenient_still_position)
{
calibrate_return result = calibrate_return_ok;
// Setup subscriptions to onboard accel sensor
int sub_accel = orb_subscribe(ORB_ID(sensor_combined));
if (sub_accel < 0) {
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "No onboard accel");
return calibrate_return_error;
}
unsigned orientation_failures = 0;
// Rotate through all requested orientation
while (true) {
if (calibrate_cancel_check(mavlink_fd, cancel_sub)) {
result = calibrate_return_cancelled;
break;
}
if (orientation_failures > 4) {
result = calibrate_return_error;
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "timeout: no motion");
break;
}
unsigned int side_complete_count = 0;
// Update the number of completed sides
for (unsigned i = 0; i < detect_orientation_side_count; i++) {
if (side_data_collected[i]) {
side_complete_count++;
}
}
if (side_complete_count == detect_orientation_side_count) {
// We have completed all sides, move on
break;
}
/* inform user which orientations are still needed */
char pendingStr[256];
pendingStr[0] = 0;
for (unsigned int cur_orientation=0; cur_orientation<detect_orientation_side_count; cur_orientation++) {
if (!side_data_collected[cur_orientation]) {
strcat(pendingStr, " ");
strcat(pendingStr, detect_orientation_str((enum detect_orientation_return)cur_orientation));
}
}
mavlink_and_console_log_info(mavlink_fd, "[cal] pending:%s", pendingStr);
mavlink_and_console_log_info(mavlink_fd, "[cal] hold vehicle still on a pending side");
enum detect_orientation_return orient = detect_orientation(mavlink_fd, cancel_sub, sub_accel, lenient_still_position);
if (orient == DETECT_ORIENTATION_ERROR) {
orientation_failures++;
mavlink_and_console_log_info(mavlink_fd, "[cal] detected motion, hold still...");
continue;
}
/* inform user about already handled side */
if (side_data_collected[orient]) {
orientation_failures++;
mavlink_and_console_log_critical(mavlink_fd, "%s side already completed", detect_orientation_str(orient));
mavlink_and_console_log_critical(mavlink_fd, "rotate to a pending side");
continue;
}
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_ORIENTATION_DETECTED_MSG, detect_orientation_str(orient));
orientation_failures = 0;
// Call worker routine
result = calibration_worker(orient, cancel_sub, worker_data);
if (result != calibrate_return_ok ) {
break;
}
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_SIDE_DONE_MSG, detect_orientation_str(orient));
// Note that this side is complete
side_data_collected[orient] = true;
tune_neutral(true);
usleep(200000);
}
if (sub_accel >= 0) {
px4_close(sub_accel);
}
return result;
}
