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;
}
calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors)
{
calibrate_return result = calibrate_return_ok;
*active_sensors = 0;
accel_worker_data_t worker_data;
worker_data.mavlink_log_pub = mavlink_log_pub;
worker_data.done_count = 0;
bool data_collected[detect_orientation_side_count] = { false, false, false, false, false, false };
// Initialise sub to sensor thermal compensation data
worker_data.sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction));
// Initialize subs to error condition so we know which ones are open and which are not
for (size_t i=0; i<max_accel_sens; i++) {
worker_data.subs[i] = -1;
}
uint64_t timestamps[max_accel_sens] = {};
// We should not try to subscribe if the topic doesn't actually exist and can be counted.
const unsigned orb_accel_count = orb_group_count(ORB_ID(sensor_accel));
// Warn that we will not calibrate more than max_accels accelerometers
if (orb_accel_count > max_accel_sens) {
calibration_log_critical(mavlink_log_pub, "Detected %u accels, but will calibrate only %u", orb_accel_count, max_accel_sens);
}
for (unsigned cur_accel = 0; cur_accel < orb_accel_count && cur_accel < max_accel_sens; cur_accel++) {
// Lock in to correct ORB instance
bool found_cur_accel = false;
for(unsigned i = 0; i < orb_accel_count && !found_cur_accel; i++) {
worker_data.subs[cur_accel] = orb_subscribe_multi(ORB_ID(sensor_accel), i);
struct accel_report report = {};
orb_copy(ORB_ID(sensor_accel), worker_data.subs[cur_accel], &report);
#ifdef __PX4_NUTTX
// For NuttX, we get the UNIQUE device ID from the sensor driver via an IOCTL
// and match it up with the one from the uORB subscription, because the
// instance ordering of uORB and the order of the FDs may not be the same.
if(report.device_id == device_id[cur_accel]) {
// Device IDs match, correct ORB instance for this accel
found_cur_accel = true;
// store initial timestamp - used to infer which sensors are active
timestamps[cur_accel] = report.timestamp;
} else {
orb_unsubscribe(worker_data.subs[cur_accel]);
}
#else
// For the DriverFramework drivers, we fill device ID (this is the first time) by copying one report.
device_id[cur_accel] = report.device_id;
found_cur_accel = true;
#endif
}
if(!found_cur_accel) {
calibration_log_critical(mavlink_log_pub, "Accel #%u (ID %u) no matching uORB devid", cur_accel, device_id[cur_accel]);
result = calibrate_return_error;
break;
}
if (device_id[cur_accel] != 0) {
// Get priority
int32_t prio;
orb_priority(worker_data.subs[cur_accel], &prio);
if (prio > device_prio_max) {
device_prio_max = prio;
device_id_primary = device_id[cur_accel];
}
} else {
calibration_log_critical(mavlink_log_pub, "Accel #%u no device id, abort", cur_accel);
result = calibrate_return_error;
break;
}
}
if (result == calibrate_return_ok) {
int cancel_sub = calibrate_cancel_subscribe();
result = calibrate_from_orientation(mavlink_log_pub, cancel_sub, data_collected, accel_calibration_worker, &worker_data, false /* normal still */);
calibrate_cancel_unsubscribe(cancel_sub);
}
/* close all subscriptions */
for (unsigned i = 0; i < max_accel_sens; i++) {
if (worker_data.subs[i] >= 0) {
/* figure out which sensors were active */
struct accel_report arp = {};
(void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp);
if (arp.timestamp != 0 && timestamps[i] != arp.timestamp) {
(*active_sensors)++;
}
px4_close(worker_data.subs[i]);
}
}
orb_unsubscribe(worker_data.sensor_correction_sub);
if (result == calibrate_return_ok) {
/* calculate offsets and transform matrix */
for (unsigned i = 0; i < (*active_sensors); i++) {
result = calculate_calibration_values(i, worker_data.accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
if (result != calibrate_return_ok) {
calibration_log_critical(mavlink_log_pub, "ERROR: calibration calculation error");
break;
}
}
}
return result;
}
calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors)
{
calibrate_return result = calibrate_return_ok;
*active_sensors = 0;
accel_worker_data_t worker_data;
worker_data.mavlink_log_pub = mavlink_log_pub;
worker_data.done_count = 0;
bool data_collected[detect_orientation_side_count] = { false, false, false, false, false, false };
// Initialize subs to error condition so we know which ones are open and which are not
for (size_t i=0; i<max_accel_sens; i++) {
worker_data.subs[i] = -1;
}
uint64_t timestamps[max_accel_sens];
// We should not try to subscribe if the topic doesn't actually exist and can be counted.
const unsigned accel_count = orb_group_count(ORB_ID(sensor_accel));
for (unsigned i = 0; i < accel_count; i++) {
worker_data.subs[i] = orb_subscribe_multi(ORB_ID(sensor_accel), i);
if (worker_data.subs[i] < 0) {
result = calibrate_return_error;
break;
}
#if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE) || defined(__PX4_POSIX_RPI)
// For QURT respectively the driver framework, we need to get the device ID by copying one report.
struct accel_report accel_report;
orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &accel_report);
device_id[i] = accel_report.device_id;
#endif
/* store initial timestamp - used to infer which sensors are active */
struct accel_report arp = {};
(void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp);
timestamps[i] = arp.timestamp;
if (device_id[i] != 0) {
// Get priority
int32_t prio;
orb_priority(worker_data.subs[i], &prio);
if (prio > device_prio_max) {
device_prio_max = prio;
device_id_primary = device_id[i];
}
} else {
calibration_log_critical(mavlink_log_pub, "[cal] Accel #%u no device id, abort", i);
result = calibrate_return_error;
break;
}
}
if (result == calibrate_return_ok) {
int cancel_sub = calibrate_cancel_subscribe();
result = calibrate_from_orientation(mavlink_log_pub, cancel_sub, data_collected, accel_calibration_worker, &worker_data, false /* normal still */);
calibrate_cancel_unsubscribe(cancel_sub);
}
/* close all subscriptions */
for (unsigned i = 0; i < max_accel_sens; i++) {
if (worker_data.subs[i] >= 0) {
/* figure out which sensors were active */
struct accel_report arp = {};
(void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp);
if (arp.timestamp != 0 && timestamps[i] != arp.timestamp) {
(*active_sensors)++;
}
px4_close(worker_data.subs[i]);
}
}
if (result == calibrate_return_ok) {
/* calculate offsets and transform matrix */
for (unsigned i = 0; i < (*active_sensors); i++) {
result = calculate_calibration_values(i, worker_data.accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
if (result != calibrate_return_ok) {
calibration_log_critical(mavlink_log_pub, "[cal] ERROR: calibration calculation error");
break;
}
}
}
return result;
}
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;
}
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;
}
