diff --git a/src/modules/commander/calibration_messages.h b/src/modules/commander/calibration_messages.h index 0c7cda1417..8afcf7927e 100644 --- a/src/modules/commander/calibration_messages.h +++ b/src/modules/commander/calibration_messages.h @@ -61,6 +61,7 @@ #define CAL_ERROR_SENSOR_MSG "[cal] calibration failed: reading sensor" #define CAL_ERROR_RESET_CAL_MSG "[cal] calibration failed: to reset, sensor %u" +#define CAL_ERROR_READ_CAL_MSG "[cal] calibration failed: to read calibration" #define CAL_ERROR_APPLY_CAL_MSG "[cal] calibration failed: to apply calibration" #define CAL_ERROR_SET_PARAMS_MSG "[cal] calibration failed: to set parameters" diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 8a0cfa7b34..cafe5e84e8 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -238,8 +238,9 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[], } int ellipsoid_fit_least_squares(const float x[], const float y[], const float z[], - unsigned int size, int max_iterations, float delta, 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) + unsigned int size, int max_iterations, 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, bool sphere_fit_only) { float _fitness = 1.0e30f, _sphere_lambda = 1.0f, _ellipsoid_lambda = 1.0f; @@ -250,12 +251,14 @@ int ellipsoid_fit_least_squares(const float x[], const float y[], const float z[ } - _fitness = 1.0e30f; + if (!sphere_fit_only) { + _fitness = 1.0e30f; - for (int i = 0; i < max_iterations; i++) { - run_lm_ellipsoid_fit(x, y, z, _fitness, _ellipsoid_lambda, - size, offset_x, offset_y, offset_z, - sphere_radius, diag_x, diag_y, diag_z, offdiag_x, offdiag_y, offdiag_z); + for (int i = 0; i < max_iterations; i++) { + run_lm_ellipsoid_fit(x, y, z, _fitness, _ellipsoid_lambda, + size, offset_x, offset_y, offset_z, + sphere_radius, diag_x, diag_y, diag_z, offdiag_x, offdiag_y, offdiag_z); + } } return 0; diff --git a/src/modules/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h index 08c9308f11..ff94941eeb 100644 --- a/src/modules/commander/calibration_routines.h +++ b/src/modules/commander/calibration_routines.h @@ -58,9 +58,9 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[], unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius); int ellipsoid_fit_least_squares(const float x[], const float y[], const float z[], - unsigned int size, int max_iterations, float delta, float *offset_x, float *offset_y, float *offset_z, + unsigned int size, int max_iterations, 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); + float *offdiag_z, bool sphere_fit_only); int run_lm_sphere_fit(const float x[], const float y[], const float z[], float &_fitness, float &_sphere_lambda, unsigned int size, 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, diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 676fae3718..bb2781db8d 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -78,7 +78,7 @@ int device_prio_max = 0; int32_t device_id_primary = 0; static unsigned _last_mag_progress = 0; -calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub); +calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_mask); /// Data passed to calibration worker routine typedef struct { @@ -143,6 +143,15 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub) _last_mag_progress = 0; + // Collect: As defined by configuration + // start with a full mask, all six bits set + int32_t cal_mask = (1 << 6) - 1; + param_get(param_find("CAL_MAG_SIDES"), &cal_mask); + + // keep and update the existing calibration when we are not doing a full 6-axis calibration + const bool append_to_existing_calibration = cal_mask < ((1 << 6) - 1); + (void)append_to_existing_calibration; + for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) { #ifdef __PX4_NUTTX // Reset mag id to mag not available @@ -214,11 +223,13 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub) device_ids[cur_mag] = px4_ioctl(fd, DEVIOCGDEVICEID, 0); internal[cur_mag] = (px4_ioctl(fd, MAGIOCGEXTERNAL, 0) <= 0); - // Reset mag scale & offset - result = px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null); + if (!append_to_existing_calibration) { + // Reset mag scale & offset + result = px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null); - if (result != PX4_OK) { - calibration_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, cur_mag); + if (result != PX4_OK) { + calibration_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, cur_mag); + } } @@ -228,7 +239,7 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub) // Calibrate all mags at the same time if (result == PX4_OK) { - switch (mag_calibrate_all(mavlink_log_pub)) { + switch (mag_calibrate_all(mavlink_log_pub, cal_mask)) { case calibrate_return_cancelled: // Cancel message already displayed, we're done here result = PX4_ERROR; @@ -516,7 +527,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta return result; } -calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) +calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_mask) { calibrate_return result = calibrate_return_ok; @@ -528,11 +539,6 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) worker_data.calibration_interval_perside_seconds = calibraton_duration_seconds / detect_orientation_side_count; 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 - int32_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++) { @@ -716,13 +722,16 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) if (device_ids[cur_mag] != 0) { // Mag in this slot is available and we should have values for it to calibrate + // Estimate only the offsets if two-sided calibration is selected, as the problem is not constrained + // enough to reliably estimate both scales and offsets with 2 sides only (even if the existing calibration + // is already close) + bool sphere_fit_only = calibration_sides <= 2; 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, + worker_data.calibration_counter_total[cur_mag], 100, &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]); + &offdiag_x[cur_mag], &offdiag_y[cur_mag], &offdiag_z[cur_mag], sphere_fit_only); result = check_calibration_result(sphere_x[cur_mag], sphere_y[cur_mag], sphere_z[cur_mag], sphere_radius[cur_mag], @@ -793,15 +802,11 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) 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; + mag_calibration_s mscale; #ifdef __PX4_NUTTX int fd_mag = -1; - // Set new scale (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag); fd_mag = px4_open(str, 0); @@ -810,30 +815,40 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) 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, "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]; #ifdef __PX4_NUTTX + // Read existing calibration + if (px4_ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale) != PX4_OK) { + calibration_log_critical(mavlink_log_pub, CAL_ERROR_READ_CAL_MSG); + result = calibrate_return_error; + } + + // Update calibration + // The formula for applying the calibration is: + // mag_value = (mag_readout - (offset_existing + offset_new/scale_existing)) * scale_existing * scale_new + mscale.x_offset = mscale.x_offset + sphere_x[cur_mag] / mscale.x_scale; + mscale.y_offset = mscale.y_offset + sphere_y[cur_mag] / mscale.y_scale; + mscale.z_offset = mscale.z_offset + sphere_z[cur_mag] / mscale.z_scale; + mscale.x_scale = mscale.x_scale * diag_x[cur_mag]; + mscale.y_scale = mscale.y_scale * diag_y[cur_mag]; + mscale.z_scale = mscale.z_scale * diag_z[cur_mag]; + if (px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != PX4_OK) { calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG); result = calibrate_return_error; } +#else + 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]; #endif } diff --git a/src/modules/sensors/sensor_params_mag.c b/src/modules/sensors/sensor_params_mag.c index f63c07b3c6..4cc5eb9e22 100644 --- a/src/modules/sensors/sensor_params_mag.c +++ b/src/modules/sensors/sensor_params_mag.c @@ -42,6 +42,11 @@ PARAM_DEFINE_INT32(CAL_MAG_PRIME, 0); /** * Bitfield selecting mag sides for calibration * + * If set to two side calibration, only the offsets are estimated, the scale + * calibration is left unchanged. Thus an initial six side calibration is + * recommended. + * + * Bits: * DETECT_ORIENTATION_TAIL_DOWN = 1 * DETECT_ORIENTATION_NOSE_DOWN = 2 * DETECT_ORIENTATION_LEFT = 4