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