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; }