// Returns calibrate_return_error if any parameter is not finite
// Logs if parameters are out of range
static calibrate_return check_calibration_result(float offset_x, float offset_y, float offset_z,
float sphere_radius,
float diag_x, float diag_y, float diag_z,
float offdiag_x, float offdiag_y, float offdiag_z,
orb_advert_t *mavlink_log_pub, size_t cur_mag)
{
float must_be_finite[] = {offset_x, offset_y, offset_z,
sphere_radius,
diag_x, diag_y, diag_z,
offdiag_x, offdiag_y, offdiag_z};
float should_be_not_huge[] = {offset_x, offset_y, offset_z};
float should_be_positive[] = {sphere_radius, diag_x, diag_y, diag_z};
// Make sure every parameter is finite
const int num_finite = sizeof(must_be_finite) / sizeof(*must_be_finite);
for (unsigned i = 0; i < num_finite; ++i) {
if (!PX4_ISFINITE(must_be_finite[i])) {
calibration_log_emergency(mavlink_log_pub,
"ERROR: Retry calibration (sphere NaN, #%u)", cur_mag);
return calibrate_return_error;
}
}
// Notify if offsets are too large
const int num_not_huge = sizeof(should_be_not_huge) / sizeof(*should_be_not_huge);
for (unsigned i = 0; i < num_not_huge; ++i) {
if (fabsf(should_be_not_huge[i]) > MAG_MAX_OFFSET_LEN) {
calibration_log_critical(mavlink_log_pub, "Warning: %s mag with large offsets",
(internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external");
break;
}
}
// Notify if a parameter which should be positive is non-positive
const int num_positive = sizeof(should_be_positive) / sizeof(*should_be_positive);
for (unsigned i = 0; i < num_positive; ++i) {
if (should_be_positive[i] <= 0.0f) {
calibration_log_critical(mavlink_log_pub, "Warning: %s mag with non-positive scale",
(internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external");
break;
}
}
return calibrate_return_ok;
}
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: Right-side up, Left Side, Nose down
worker_data.side_data_collected[DETECT_ORIENTATION_RIGHTSIDE_UP] = false;
worker_data.side_data_collected[DETECT_ORIENTATION_LEFT] = false;
worker_data.side_data_collected[DETECT_ORIENTATION_NOSE_DOWN] = false;
worker_data.side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = true;
worker_data.side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = true;
worker_data.side_data_collected[DETECT_ORIENTATION_RIGHT] = true;
calibration_log_info(mavlink_log_pub,
"[cal] %s side done, rotate to a different side",
detect_orientation_str(DETECT_ORIENTATION_TAIL_DOWN));
usleep(100000);
calibration_log_info(mavlink_log_pub,
"[cal] %s side done, rotate to a different side",
detect_orientation_str(DETECT_ORIENTATION_TAIL_DOWN));
usleep(100000);
calibration_log_info(mavlink_log_pub,
"[cal] %s side done, rotate to a different side",
detect_orientation_str(DETECT_ORIENTATION_UPSIDE_DOWN));
usleep(100000);
calibration_log_info(mavlink_log_pub,
"[cal] %s side done, rotate to a different side",
detect_orientation_str(DETECT_ORIENTATION_UPSIDE_DOWN));
usleep(100000);
calibration_log_info(mavlink_log_pub,
"[cal] %s side done, rotate to a different side",
detect_orientation_str(DETECT_ORIENTATION_RIGHT));
usleep(100000);
calibration_log_info(mavlink_log_pub,
"[cal] %s side done, rotate to a different side",
detect_orientation_str(DETECT_ORIENTATION_RIGHT));
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);
#ifdef __PX4_QURT
// 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_emergency(mavlink_log_pub, "ERROR: Replace %s mag fault", (internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external");
calibration_log_info(mavlink_log_pub, "Excessive offsets: %8.4f, %8.4f, %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) {
(void)param_set_no_notification(param_find("CAL_MAG_PRIME"), &(device_id_primary));
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {
struct mag_calibration_s mscale;
#ifndef __PX4_QURT
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];
#ifndef __PX4_QURT
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
}
#ifndef __PX4_QURT
// 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)));
(void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag);
// FIXME: scaling is not used right now on QURT
#ifndef __PX4_QURT
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);
}
}
}
}
}
return result;
}
