int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_sens][3], float (&accel_T)[max_sens][3][3], unsigned *active_sensors) { const unsigned samples_num = 3000; *active_sensors = 0; float accel_ref[max_sens][6][3]; bool data_collected[6] = { false, false, false, false, false, false }; const char *orientation_strs[6] = { "back", "front", "left", "right", "up", "down" }; int subs[max_sens]; uint64_t timestamps[max_sens]; for (unsigned i = 0; i < max_sens; i++) { subs[i] = orb_subscribe_multi(ORB_ID(sensor_accel), i); /* store initial timestamp - used to infer which sensors are active */ struct accel_report arp = {}; (void)orb_copy(ORB_ID(sensor_accel), subs[i], &arp); timestamps[i] = arp.timestamp; } unsigned done_count = 0; int res = OK; while (true) { bool done = true; unsigned old_done_count = done_count; done_count = 0; for (int i = 0; i < 6; i++) { if (data_collected[i]) { done_count++; } else { done = false; } } if (old_done_count != done_count) { mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * done_count); } if (done) { break; } /* inform user which axes are still needed */ mavlink_and_console_log_info(mavlink_fd, "pending: %s%s%s%s%s%s", (!data_collected[5]) ? "down " : "", (!data_collected[0]) ? "back " : "", (!data_collected[1]) ? "front " : "", (!data_collected[2]) ? "left " : "", (!data_collected[3]) ? "right " : "", (!data_collected[4]) ? "up " : ""); /* allow user enough time to read the message */ sleep(3); int orient = detect_orientation(mavlink_fd, subs); if (orient < 0) { mavlink_and_console_log_info(mavlink_fd, "invalid motion, hold still..."); sleep(3); continue; } /* inform user about already handled side */ if (data_collected[orient]) { mavlink_and_console_log_info(mavlink_fd, "%s side done, rotate to a different side", orientation_strs[orient]); sleep(3); continue; } mavlink_and_console_log_info(mavlink_fd, "Hold still, starting to measure %s side", orientation_strs[orient]); sleep(1); read_accelerometer_avg(subs, accel_ref, orient, samples_num); mavlink_and_console_log_info(mavlink_fd, "result for %s side: [ %.2f %.2f %.2f ]", orientation_strs[orient], (double)accel_ref[0][orient][0], (double)accel_ref[0][orient][1], (double)accel_ref[0][orient][2]); data_collected[orient] = true; tune_neutral(true); } /* close all subscriptions */ for (unsigned i = 0; i < max_sens; i++) { /* figure out which sensors were active */ struct accel_report arp = {}; (void)orb_copy(ORB_ID(sensor_accel), subs[i], &arp); if (arp.timestamp != 0 && timestamps[i] != arp.timestamp) { (*active_sensors)++; } close(subs[i]); } if (res == OK) { /* calculate offsets and transform matrix */ for (unsigned i = 0; i < (*active_sensors); i++) { res = calculate_calibration_values(i, accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); /* verbose output on the console */ printf("accel_T transformation matrix:\n"); for (unsigned j = 0; j < 3; j++) { printf(" %8.4f %8.4f %8.4f\n", (double)accel_T[i][j][0], (double)accel_T[i][j][1], (double)accel_T[i][j][2]); } printf("\n"); if (res != OK) { mavlink_and_console_log_critical(mavlink_fd, "ERROR: calibration values calculation error"); break; } } } return res; }
int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) { const int samples_num = 2500; float accel_ref[6][3]; bool data_collected[6] = { false, false, false, false, false, false }; const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" }; /* reset existing calibration */ int fd = open(ACCEL_DEVICE_PATH, 0); struct accel_scale ascale_null = { 0.0f, 1.0f, 0.0f, 1.0f, 0.0f, 1.0f, }; int ioctl_res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null); close(fd); if (OK != ioctl_res) { warn("ERROR: failed to set scale / offsets for accel"); return ERROR; } int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); while (true) { bool done = true; char str[80]; int str_ptr; str_ptr = sprintf(str, "keep vehicle still:"); for (int i = 0; i < 6; i++) { if (!data_collected[i]) { str_ptr += sprintf(&(str[str_ptr]), " %s", orientation_strs[i]); done = false; } } if (done) break; mavlink_log_info(mavlink_fd, str); int orient = detect_orientation(mavlink_fd, sensor_combined_sub); if (orient < 0) return ERROR; if (data_collected[orient]) { sprintf(str, "%s direction already measured, please rotate", orientation_strs[orient]); mavlink_log_info(mavlink_fd, str); continue; } sprintf(str, "meas started: %s", orientation_strs[orient]); mavlink_log_info(mavlink_fd, str); read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], accel_ref[orient][0], accel_ref[orient][1], accel_ref[orient][2]); mavlink_log_info(mavlink_fd, str); data_collected[orient] = true; tune_confirm(); } close(sensor_combined_sub); /* calculate offsets and rotation+scale matrix */ float accel_T[3][3]; int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); if (res != 0) { mavlink_log_info(mavlink_fd, "ERROR: calibration values calc error"); return ERROR; } /* convert accel transform matrix to scales, * rotation part of transform matrix is not used by now */ for (int i = 0; i < 3; i++) { accel_scale[i] = accel_T[i][i]; } return OK; }
int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3]) { const int samples_num = 2500; float accel_ref[6][3]; bool data_collected[6] = { false, false, false, false, false, false }; const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" }; int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); unsigned done_count = 0; int res = OK; while (true) { bool done = true; unsigned old_done_count = done_count; done_count = 0; for (int i = 0; i < 6; i++) { if (data_collected[i]) { done_count++; } else { done = false; } } if (old_done_count != done_count) { mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * done_count); } if (done) { break; } mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s", (!data_collected[0]) ? "x+ " : "", (!data_collected[1]) ? "x- " : "", (!data_collected[2]) ? "y+ " : "", (!data_collected[3]) ? "y- " : "", (!data_collected[4]) ? "z+ " : "", (!data_collected[5]) ? "z- " : ""); int orient = detect_orientation(mavlink_fd, sensor_combined_sub); if (orient < 0) { res = ERROR; break; } if (data_collected[orient]) { mavlink_log_info(mavlink_fd, "%s done, rotate to a different axis", orientation_strs[orient]); continue; } mavlink_log_info(mavlink_fd, "accel measurement started: %s axis", orientation_strs[orient]); read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); mavlink_log_info(mavlink_fd, "result for %s axis: [ %.2f %.2f %.2f ]", orientation_strs[orient], (double)accel_ref[orient][0], (double)accel_ref[orient][1], (double)accel_ref[orient][2]); data_collected[orient] = true; tune_neutral(true); } close(sensor_combined_sub); if (res == OK) { /* calculate offsets and transform matrix */ res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); if (res != OK) { mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error"); } } return res; }
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; }
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; }
int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) { const int samples_num = 2500; float accel_ref[6][3]; bool data_collected[6] = { false, false, false, false, false, false }; const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" }; /* reset existing calibration */ int fd = open(ACCEL_DEVICE_PATH, 0); struct accel_scale ascale_null = { 0.0f, 1.0f, 0.0f, 1.0f, 0.0f, 1.0f, }; int ioctl_res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null); close(fd); if (OK != ioctl_res) { warn("ERROR: failed to set scale / offsets for accel"); return ERROR; } int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); unsigned done_count = 0; while (true) { bool done = true; unsigned old_done_count = done_count; done_count = 0; for (int i = 0; i < 6; i++) { if (!data_collected[i]) { done = false; } } mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s", (!data_collected[0]) ? "x+ " : "", (!data_collected[1]) ? "x- " : "", (!data_collected[2]) ? "y+ " : "", (!data_collected[3]) ? "y- " : "", (!data_collected[4]) ? "z+ " : "", (!data_collected[5]) ? "z- " : ""); if (old_done_count != done_count) mavlink_log_info(mavlink_fd, "accel cal progress <%u> percent", 17 * done_count); if (done) break; int orient = detect_orientation(mavlink_fd, sensor_combined_sub); if (orient < 0) { close(sensor_combined_sub); return ERROR; } if (data_collected[orient]) { mavlink_log_info(mavlink_fd, "%s done, please rotate to a different axis", orientation_strs[orient]); continue; } mavlink_log_info(mavlink_fd, "accel measurement started: %s axis", orientation_strs[orient]); read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); mavlink_log_info(mavlink_fd, "result for %s axis: [ %.2f %.2f %.2f ]", orientation_strs[orient], (double)accel_ref[orient][0], (double)accel_ref[orient][1], (double)accel_ref[orient][2]); data_collected[orient] = true; tune_neutral(); } close(sensor_combined_sub); /* calculate offsets and rotation+scale matrix */ float accel_T[3][3]; int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); if (res != 0) { mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error"); return ERROR; } /* convert accel transform matrix to scales, * rotation part of transform matrix is not used by now */ for (int i = 0; i < 3; i++) { accel_scale[i] = accel_T[i][i]; } return OK; }