TemperatureCalibrationBaro::TemperatureCalibrationBaro(float min_temperature_rise)
: TemperatureCalibrationCommon(min_temperature_rise)
{
//init subscriptions
_num_sensor_instances = orb_group_count(ORB_ID(sensor_baro));
if (_num_sensor_instances > SENSOR_COUNT_MAX) {
_num_sensor_instances = SENSOR_COUNT_MAX;
}
for (unsigned i = 0; i < _num_sensor_instances; i++) {
_sensor_subs[i] = orb_subscribe_multi(ORB_ID(sensor_baro), i);
}
}
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;
}
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;
}
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] = nullptr;
worker_data.y[cur_mag] = nullptr;
worker_data.z[cur_mag] = nullptr;
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] == nullptr || worker_data.y[cur_mag] == nullptr || worker_data.z[cur_mag] == nullptr) {
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));
// Warn that we will not calibrate more than max_mags magnetometers
if (mag_count > max_mags) {
calibration_log_critical(mavlink_log_pub, "[cal] Detected %u mags, but will calibrate only %u", mag_count, max_mags);
}
for (unsigned cur_mag = 0; cur_mag < mag_count && cur_mag < max_mags; 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_RPI) || defined(__PX4_POSIX_BEBOP)
// 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];
float diag_x[max_mags];
float diag_y[max_mags];
float diag_z[max_mags];
float offdiag_x[max_mags];
float offdiag_y[max_mags];
float offdiag_z[max_mags];
for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) {
sphere_x[cur_mag] = 0.0f;
sphere_y[cur_mag] = 0.0f;
sphere_z[cur_mag] = 0.0f;
sphere_radius[cur_mag] = 0.2f;
diag_x[cur_mag] = 1.0f;
diag_y[cur_mag] = 1.0f;
diag_z[cur_mag] = 1.0f;
offdiag_x[cur_mag] = 0.0f;
offdiag_y[cur_mag] = 0.0f;
offdiag_z[cur_mag] = 0.0f;
}
// 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
ellipsoid_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],
&diag_x[cur_mag], &diag_y[cur_mag], &diag_z[cur_mag],
&offdiag_x[cur_mag], &offdiag_y[cur_mag], &offdiag_z[cur_mag]);
result = check_calibration_result(sphere_x[cur_mag], sphere_y[cur_mag], sphere_z[cur_mag],
sphere_radius[cur_mag],
diag_x[cur_mag], diag_y[cur_mag], diag_z[cur_mag],
offdiag_x[cur_mag], offdiag_y[cur_mag], offdiag_z[cur_mag],
mavlink_log_pub, cur_mag);
if (result == calibrate_return_error) {
break;
}
}
}
}
// 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;
mscale.x_scale = 1.0;
mscale.y_scale = 1.0;
mscale.z_scale = 1.0;
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
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) != PX4_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];
mscale.x_scale = diag_x[cur_mag];
mscale.y_scale = diag_y[cur_mag];
mscale.z_scale = diag_z[cur_mag];
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
if (px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != PX4_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_RPI) && !defined(__PX4_POSIX_BEBOP)
// 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 |= (PX4_OK != param_set_no_notification(param_find(str), &(device_ids[cur_mag])));
(void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(mscale.x_offset)));
(void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(mscale.y_offset)));
(void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag);
failed |= (PX4_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 |= (PX4_OK != param_set_no_notification(param_find(str), &(mscale.x_scale)));
(void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(mscale.y_scale)));
(void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag);
failed |= (PX4_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;
}
void
MulticopterAttitudeControl::task_main()
{
/*
* do subscriptions
*/
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits));
_battery_status_sub = orb_subscribe(ORB_ID(battery_status));
_gyro_count = math::min(orb_group_count(ORB_ID(sensor_gyro)), MAX_GYRO_COUNT);
for (unsigned s = 0; s < _gyro_count; s++) {
_sensor_gyro_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
}
_sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction));
/* initialize parameters cache */
parameters_update();
/* wakeup source: gyro data from sensor selected by the sensor app */
px4_pollfd_struct_t poll_fds = {};
poll_fds.fd = _sensor_gyro_sub[_selected_gyro];
poll_fds.events = POLLIN;
while (!_task_should_exit) {
/* wait for up to 100ms for data */
int pret = px4_poll(&poll_fds, 1, 100);
/* timed out - periodic check for _task_should_exit */
if (pret == 0) {
continue;
}
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
warn("mc att ctrl: poll error %d, %d", pret, errno);
/* sleep a bit before next try */
usleep(100000);
continue;
}
perf_begin(_loop_perf);
/* run controller on gyro changes */
if (poll_fds.revents & POLLIN) {
static uint64_t last_run = 0;
float dt = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
/* guard against too small (< 2ms) and too large (> 20ms) dt's */
if (dt < 0.002f) {
dt = 0.002f;
} else if (dt > 0.02f) {
dt = 0.02f;
}
/* copy gyro data */
orb_copy(ORB_ID(sensor_gyro), _sensor_gyro_sub[_selected_gyro], &_sensor_gyro);
/* check for updates in other topics */
parameter_update_poll();
vehicle_control_mode_poll();
arming_status_poll();
vehicle_manual_poll();
vehicle_status_poll();
vehicle_motor_limits_poll();
battery_status_poll();
control_state_poll();
sensor_correction_poll();
/* Check if we are in rattitude mode and the pilot is above the threshold on pitch
* or roll (yaw can rotate 360 in normal att control). If both are true don't
* even bother running the attitude controllers */
if (_v_control_mode.flag_control_rattitude_enabled) {
if (fabsf(_manual_control_sp.y) > _params.rattitude_thres ||
fabsf(_manual_control_sp.x) > _params.rattitude_thres) {
_v_control_mode.flag_control_attitude_enabled = false;
}
}
if (_v_control_mode.flag_control_attitude_enabled) {
if (_ts_opt_recovery == nullptr) {
// the tailsitter recovery instance has not been created, thus, the vehicle
// is not a tailsitter, do normal attitude control
control_attitude(dt);
} else {
vehicle_attitude_setpoint_poll();
_thrust_sp = _v_att_sp.thrust;
math::Quaternion q(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
math::Quaternion q_sp(&_v_att_sp.q_d[0]);
_ts_opt_recovery->setAttGains(_params.att_p, _params.yaw_ff);
_ts_opt_recovery->calcOptimalRates(q, q_sp, _v_att_sp.yaw_sp_move_rate, _rates_sp);
/* limit rates */
for (int i = 0; i < 3; i++) {
_rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i));
}
}
/* publish attitude rates setpoint */
_v_rates_sp.roll = _rates_sp(0);
_v_rates_sp.pitch = _rates_sp(1);
_v_rates_sp.yaw = _rates_sp(2);
_v_rates_sp.thrust = _thrust_sp;
_v_rates_sp.timestamp = hrt_absolute_time();
if (_v_rates_sp_pub != nullptr) {
orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
} else if (_rates_sp_id) {
_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
}
//}
} else {
/* attitude controller disabled, poll rates setpoint topic */
if (_v_control_mode.flag_control_manual_enabled) {
/* manual rates control - ACRO mode */
_rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x,
_manual_control_sp.r).emult(_params.acro_rate_max);
_thrust_sp = math::min(_manual_control_sp.z, MANUAL_THROTTLE_MAX_MULTICOPTER);
/* publish attitude rates setpoint */
_v_rates_sp.roll = _rates_sp(0);
_v_rates_sp.pitch = _rates_sp(1);
_v_rates_sp.yaw = _rates_sp(2);
_v_rates_sp.thrust = _thrust_sp;
_v_rates_sp.timestamp = hrt_absolute_time();
if (_v_rates_sp_pub != nullptr) {
orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
} else if (_rates_sp_id) {
_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
}
} else {
/* attitude controller disabled, poll rates setpoint topic */
vehicle_rates_setpoint_poll();
_rates_sp(0) = _v_rates_sp.roll;
_rates_sp(1) = _v_rates_sp.pitch;
_rates_sp(2) = _v_rates_sp.yaw;
_thrust_sp = _v_rates_sp.thrust;
}
}
if (_v_control_mode.flag_control_rates_enabled) {
control_attitude_rates(dt);
/* publish actuator controls */
_actuators.control[0] = (PX4_ISFINITE(_att_control(0))) ? _att_control(0) : 0.0f;
_actuators.control[1] = (PX4_ISFINITE(_att_control(1))) ? _att_control(1) : 0.0f;
_actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f;
_actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f;
_actuators.control[7] = _v_att_sp.landing_gear;
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = _ctrl_state.timestamp;
/* scale effort by battery status */
if (_params.bat_scale_en && _battery_status.scale > 0.0f) {
for (int i = 0; i < 4; i++) {
_actuators.control[i] *= _battery_status.scale;
}
}
_controller_status.roll_rate_integ = _rates_int(0);
_controller_status.pitch_rate_integ = _rates_int(1);
_controller_status.yaw_rate_integ = _rates_int(2);
_controller_status.timestamp = hrt_absolute_time();
if (!_actuators_0_circuit_breaker_enabled) {
if (_actuators_0_pub != nullptr) {
orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
perf_end(_controller_latency_perf);
} else if (_actuators_id) {
_actuators_0_pub = orb_advertise(_actuators_id, &_actuators);
}
}
/* publish controller status */
if (_controller_status_pub != nullptr) {
orb_publish(ORB_ID(mc_att_ctrl_status), _controller_status_pub, &_controller_status);
} else {
_controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status);
}
}
if (_v_control_mode.flag_control_termination_enabled) {
if (!_vehicle_status.is_vtol) {
_rates_sp.zero();
_rates_int.zero();
_thrust_sp = 0.0f;
_att_control.zero();
/* publish actuator controls */
_actuators.control[0] = 0.0f;
_actuators.control[1] = 0.0f;
_actuators.control[2] = 0.0f;
_actuators.control[3] = 0.0f;
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = _ctrl_state.timestamp;
if (!_actuators_0_circuit_breaker_enabled) {
if (_actuators_0_pub != nullptr) {
orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
perf_end(_controller_latency_perf);
} else if (_actuators_id) {
_actuators_0_pub = orb_advertise(_actuators_id, &_actuators);
}
}
_controller_status.roll_rate_integ = _rates_int(0);
_controller_status.pitch_rate_integ = _rates_int(1);
_controller_status.yaw_rate_integ = _rates_int(2);
_controller_status.timestamp = hrt_absolute_time();
/* publish controller status */
if (_controller_status_pub != nullptr) {
orb_publish(ORB_ID(mc_att_ctrl_status), _controller_status_pub, &_controller_status);
} else {
_controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status);
}
/* publish attitude rates setpoint */
_v_rates_sp.roll = _rates_sp(0);
_v_rates_sp.pitch = _rates_sp(1);
_v_rates_sp.yaw = _rates_sp(2);
_v_rates_sp.thrust = _thrust_sp;
_v_rates_sp.timestamp = hrt_absolute_time();
if (_v_rates_sp_pub != nullptr) {
orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
} else if (_rates_sp_id) {
_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
}
}
}
}
perf_end(_loop_perf);
}
_control_task = -1;
return;
}
