From ad148af2fd5728146fb49c4d458761f2b1dd8c88 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 16 Aug 2020 21:41:35 -0400 Subject: [PATCH] commander: mag_calibration move to Magnetometer calibration helper --- .../commander/calibration_routines.cpp | 257 +----- src/modules/commander/calibration_routines.h | 31 +- src/modules/commander/mag_calibration.cpp | 787 +++++++----------- 3 files changed, 321 insertions(+), 754 deletions(-) diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 22136293bd..86a0f04a5d 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -62,225 +62,23 @@ using namespace time_literals; -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) -{ - - float x_sumplain = 0.0f; - float x_sumsq = 0.0f; - float x_sumcube = 0.0f; - - float y_sumplain = 0.0f; - float y_sumsq = 0.0f; - float y_sumcube = 0.0f; - - float z_sumplain = 0.0f; - float z_sumsq = 0.0f; - float z_sumcube = 0.0f; - - float xy_sum = 0.0f; - float xz_sum = 0.0f; - float yz_sum = 0.0f; - - float x2y_sum = 0.0f; - float x2z_sum = 0.0f; - float y2x_sum = 0.0f; - float y2z_sum = 0.0f; - float z2x_sum = 0.0f; - float z2y_sum = 0.0f; - - for (unsigned int i = 0; i < size; i++) { - - float x2 = x[i] * x[i]; - float y2 = y[i] * y[i]; - float z2 = z[i] * z[i]; - - x_sumplain += x[i]; - x_sumsq += x2; - x_sumcube += x2 * x[i]; - - y_sumplain += y[i]; - y_sumsq += y2; - y_sumcube += y2 * y[i]; - - z_sumplain += z[i]; - z_sumsq += z2; - z_sumcube += z2 * z[i]; - - xy_sum += x[i] * y[i]; - xz_sum += x[i] * z[i]; - yz_sum += y[i] * z[i]; - - x2y_sum += x2 * y[i]; - x2z_sum += x2 * z[i]; - - y2x_sum += y2 * x[i]; - y2z_sum += y2 * z[i]; - - z2x_sum += z2 * x[i]; - z2y_sum += z2 * y[i]; - } - - // - //Least Squares Fit a sphere A,B,C with radius squared Rsq to 3D data - // - // P is a structure that has been computed with the data earlier. - // P.npoints is the number of elements; the length of X,Y,Z are identical. - // P's members are logically named. - // - // X[n] is the x component of point n - // Y[n] is the y component of point n - // Z[n] is the z component of point n - // - // A is the x coordiante of the sphere - // B is the y coordiante of the sphere - // C is the z coordiante of the sphere - // Rsq is the radius squared of the sphere. - // - //This method should converge; maybe 5-100 iterations or more. - // - float x_sum = x_sumplain / size; //sum( X[n] ) - float x_sum2 = x_sumsq / size; //sum( X[n]^2 ) - float x_sum3 = x_sumcube / size; //sum( X[n]^3 ) - float y_sum = y_sumplain / size; //sum( Y[n] ) - float y_sum2 = y_sumsq / size; //sum( Y[n]^2 ) - float y_sum3 = y_sumcube / size; //sum( Y[n]^3 ) - float z_sum = z_sumplain / size; //sum( Z[n] ) - float z_sum2 = z_sumsq / size; //sum( Z[n]^2 ) - float z_sum3 = z_sumcube / size; //sum( Z[n]^3 ) - - float XY = xy_sum / size; //sum( X[n] * Y[n] ) - float XZ = xz_sum / size; //sum( X[n] * Z[n] ) - float YZ = yz_sum / size; //sum( Y[n] * Z[n] ) - float X2Y = x2y_sum / size; //sum( X[n]^2 * Y[n] ) - float X2Z = x2z_sum / size; //sum( X[n]^2 * Z[n] ) - float Y2X = y2x_sum / size; //sum( Y[n]^2 * X[n] ) - float Y2Z = y2z_sum / size; //sum( Y[n]^2 * Z[n] ) - float Z2X = z2x_sum / size; //sum( Z[n]^2 * X[n] ) - float Z2Y = z2y_sum / size; //sum( Z[n]^2 * Y[n] ) - - //Reduction of multiplications - float F0 = x_sum2 + y_sum2 + z_sum2; - float F1 = 0.5f * F0; - float F2 = -8.0f * (x_sum3 + Y2X + Z2X); - float F3 = -8.0f * (X2Y + y_sum3 + Z2Y); - float F4 = -8.0f * (X2Z + Y2Z + z_sum3); - - //Set initial conditions: - float A = x_sum; - float B = y_sum; - float C = z_sum; - - //First iteration computation: - float A2 = A * A; - float B2 = B * B; - float C2 = C * C; - float QS = A2 + B2 + C2; - float QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum); - - //Set initial conditions: - float Rsq = F0 + QB + QS; - - //First iteration computation: - float Q0 = 0.5f * (QS - Rsq); - float Q1 = F1 + Q0; - float Q2 = 8.0f * (QS - Rsq + QB + F0); - float aA, aB, aC, nA, nB, nC, dA, dB, dC; - - //Iterate N times, ignore stop condition. - unsigned int n = 0; - - while (n < max_iterations) { - n++; - - //Compute denominator: - aA = Q2 + 16.0f * (A2 - 2.0f * A * x_sum + x_sum2); - aB = Q2 + 16.0f * (B2 - 2.0f * B * y_sum + y_sum2); - aC = Q2 + 16.0f * (C2 - 2.0f * C * z_sum + z_sum2); - aA = (fabsf(aA) < FLT_EPSILON) ? 1.0f : aA; - aB = (fabsf(aB) < FLT_EPSILON) ? 1.0f : aB; - aC = (fabsf(aC) < FLT_EPSILON) ? 1.0f : aC; - - //Compute next iteration - nA = A - ((F2 + 16.0f * (B * XY + C * XZ + x_sum * (-A2 - Q0) + A * (x_sum2 + Q1 - C * z_sum - B * y_sum))) / aA); - nB = B - ((F3 + 16.0f * (A * XY + C * YZ + y_sum * (-B2 - Q0) + B * (y_sum2 + Q1 - A * x_sum - C * z_sum))) / aB); - nC = C - ((F4 + 16.0f * (A * XZ + B * YZ + z_sum * (-C2 - Q0) + C * (z_sum2 + Q1 - A * x_sum - B * y_sum))) / aC); - - //Check for stop condition - dA = (nA - A); - dB = (nB - B); - dC = (nC - C); - - if ((dA * dA + dB * dB + dC * dC) <= delta) { break; } - - //Compute next iteration's values - A = nA; - B = nB; - C = nC; - A2 = A * A; - B2 = B * B; - C2 = C * C; - QS = A2 + B2 + C2; - QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum); - Rsq = F0 + QB + QS; - Q0 = 0.5f * (QS - Rsq); - Q1 = F1 + Q0; - Q2 = 8.0f * (QS - Rsq + QB + F0); - } - - *sphere_x = A; - *sphere_y = B; - *sphere_z = C; - *sphere_radius = sqrtf(Rsq); - - return 0; -} - -int ellipsoid_fit_least_squares(const float x[], const float y[], const float 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; - - for (int i = 0; i < max_iterations; i++) { - run_lm_sphere_fit(x, y, z, _fitness, _sphere_lambda, - size, offset_x, offset_y, offset_z, - sphere_radius, diag_x, diag_y, diag_z, offdiag_x, offdiag_y, offdiag_z); - - } - - 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); - } - } - - return 0; -} - 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, + unsigned int samples_collected, 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) { - //Run Sphere Fit using Levenberg Marquardt LSq Fit - const float lma_damping = 10.0f; - float _samples_collected = size; + // Run Sphere Fit using Levenberg Marquardt LSq Fit + const float lma_damping = 10.f; float fitness = _fitness; - float fit1 = 0.0f, fit2 = 0.0f; + float fit1 = 0.f; + float fit2 = 0.f; - matrix::SquareMatrix JTJ; - matrix::SquareMatrix JTJ2; - float JTFI[4] = {}; + matrix::SquareMatrix JTJ{}; + matrix::SquareMatrix JTJ2{}; + float JTFI[4] {}; float residual = 0.0f; // Gauss Newton Part common for all kind of extensions including LM - for (uint16_t k = 0; k < _samples_collected; k++) { + for (uint16_t k = 0; k < samples_collected; k++) { float sphere_jacob[4]; //Calculate Jacobian @@ -310,7 +108,7 @@ int run_lm_sphere_fit(const float x[], const float y[], const float z[], float & //------------------------Levenberg-Marquardt-part-starts-here---------------------------------// - //refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter + // refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter float fit1_params[4] = {*sphere_radius, *offset_x, *offset_y, *offset_z}; float fit2_params[4]; memcpy(fit2_params, fit1_params, sizeof(fit1_params)); @@ -335,8 +133,8 @@ int run_lm_sphere_fit(const float x[], const float y[], const float z[], float & } } - //Calculate mean squared residuals - for (uint16_t k = 0; k < _samples_collected; k++) { + // Calculate mean squared residuals + for (uint16_t k = 0; k < samples_collected; k++) { float A = (*diag_x * (x[k] - fit1_params[1])) + (*offdiag_x * (y[k] - fit1_params[2])) + (*offdiag_y * (z[k] + fit1_params[3])); float B = (*offdiag_x * (x[k] - fit1_params[1])) + (*diag_y * (y[k] - fit1_params[2])) + (*offdiag_z * @@ -358,8 +156,8 @@ int run_lm_sphere_fit(const float x[], const float y[], const float z[], float & fit2 += residual * residual; } - fit1 = sqrtf(fit1) / _samples_collected; - fit2 = sqrtf(fit2) / _samples_collected; + fit1 = sqrtf(fit1) / samples_collected; + fit2 = sqrtf(fit2) / samples_collected; if (fit1 > _fitness && fit2 > _fitness) { _sphere_lambda *= lma_damping; @@ -389,26 +187,25 @@ int run_lm_sphere_fit(const float x[], const float y[], const float z[], float & } int run_lm_ellipsoid_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, + unsigned int samples_collected, 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) { - //Run Sphere Fit using Levenberg Marquardt LSq Fit + // Run Sphere Fit using Levenberg Marquardt LSq Fit const float lma_damping = 10.0f; - float _samples_collected = size; float fitness = _fitness; float fit1 = 0.0f; float fit2 = 0.0f; - float JTJ[81] = {}; - float JTJ2[81] = {}; - float JTFI[9] = {}; + float JTJ[81] {}; + float JTJ2[81] {}; + float JTFI[9] {}; float residual = 0.0f; float ellipsoid_jacob[9]; // Gauss Newton Part common for all kind of extensions including LM - for (uint16_t k = 0; k < _samples_collected; k++) { + for (uint16_t k = 0; k < samples_collected; k++) { - //Calculate Jacobian + // Calculate Jacobian float A = (*diag_x * (x[k] - *offset_x)) + (*offdiag_x * (y[k] - *offset_y)) + (*offdiag_y * (z[k] - *offset_z)); float B = (*offdiag_x * (x[k] - *offset_x)) + (*diag_y * (y[k] - *offset_y)) + (*offdiag_z * (z[k] - *offset_z)); float C = (*offdiag_y * (x[k] - *offset_x)) + (*offdiag_z * (y[k] - *offset_y)) + (*diag_z * (z[k] - *offset_z)); @@ -432,7 +229,7 @@ int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], floa // compute JTJ for (uint8_t j = 0; j < 9; j++) { JTJ[i * 9 + j] += ellipsoid_jacob[i] * ellipsoid_jacob[j]; - JTJ2[i * 9 + j] += ellipsoid_jacob[i] * ellipsoid_jacob[j]; //a backup JTJ for LM + JTJ2[i * 9 + j] += ellipsoid_jacob[i] * ellipsoid_jacob[j]; // a backup JTJ for LM } JTFI[i] += ellipsoid_jacob[i] * residual; @@ -441,7 +238,7 @@ int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], floa //------------------------Levenberg-Marquardt-part-starts-here---------------------------------// - //refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter + // refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter float fit1_params[9] = {*offset_x, *offset_y, *offset_z, *diag_x, *diag_y, *diag_z, *offdiag_x, *offdiag_y, *offdiag_z}; float fit2_params[9]; memcpy(fit2_params, fit1_params, sizeof(fit1_params)); @@ -467,8 +264,8 @@ int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], floa } } - //Calculate mean squared residuals - for (uint16_t k = 0; k < _samples_collected; k++) { + // Calculate mean squared residuals + for (uint16_t k = 0; k < samples_collected; k++) { float A = (fit1_params[3] * (x[k] - fit1_params[0])) + (fit1_params[6] * (y[k] - fit1_params[1])) + (fit1_params[7] * (z[k] - fit1_params[2])); float B = (fit1_params[6] * (x[k] - fit1_params[0])) + (fit1_params[4] * (y[k] - fit1_params[1])) + (fit1_params[8] * @@ -490,8 +287,8 @@ int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], floa fit2 += residual * residual; } - fit1 = sqrtf(fit1) / _samples_collected; - fit2 = sqrtf(fit2) / _samples_collected; + fit1 = sqrtf(fit1) / samples_collected; + fit2 = sqrtf(fit2) / samples_collected; if (fit1 > _fitness && fit2 > _fitness) { _sphere_lambda *= lma_damping; diff --git a/src/modules/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h index 66eca657a2..c45896491e 100644 --- a/src/modules/commander/calibration_routines.h +++ b/src/modules/commander/calibration_routines.h @@ -48,7 +48,6 @@ * @param z point coordinates on the Z axis * @param size number of points * @param max_iterations abort if maximum number of iterations have been reached. If unsure, set to 100. - * @param delta abort if error is below delta. If unsure, set to 0 to run max_iterations times. * @param sphere_x coordinate of the sphere center on the X axis * @param sphere_y coordinate of the sphere center on the Y axis * @param sphere_z coordinate of the sphere center on the Z axis @@ -56,23 +55,19 @@ * * @return 0 on success, 1 on failure */ -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 *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); -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, - float *offdiag_z); -int run_lm_ellipsoid_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, - float *offdiag_z); -bool inverse4x4(float m[], float invOut[]); -bool mat_inverse(float *A, float *inv, uint8_t n); +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, float *offdiag_z); + +int run_lm_ellipsoid_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, float *offdiag_z); // The order of these cannot change since the calibration calculations depend on them in this order enum detect_orientation_return { diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 1b97f4af41..6af8a83dd6 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -45,195 +45,63 @@ #include #include #include -#include -#include -#include -#include -#include -#include #include -#include #include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include +#include -static const char *sensor_name = "mag"; -static constexpr unsigned max_mags = 4; -static constexpr float mag_sphere_radius = 0.2f; -static unsigned int calibration_sides = 6; ///< The total number of sides -static constexpr unsigned int calibration_total_points = 240; ///< The total points per magnetometer -static constexpr unsigned int calibraton_duration_seconds = 42; ///< The total duration the routine is allowed to take +using namespace matrix; +using namespace time_literals; -static constexpr float MAG_MAX_OFFSET_LEN = - 1.3f; ///< The maximum measurement range is ~1.9 Ga, the earth field is ~0.6 Ga, so an offset larger than ~1.3 Ga means the mag will saturate in some directions. - -int32_t device_ids[max_mags]; -bool internal[max_mags]; -int device_prio_max = 0; -int32_t device_id_primary = 0; -static unsigned _last_mag_progress = 0; +static constexpr char sensor_name[] {"mag"}; +static constexpr unsigned MAX_MAGS = 4; +static constexpr float MAG_SPHERE_RADIUS_DEFAULT = 0.2f; +static constexpr unsigned int calibration_total_points = 240; ///< The total points per magnetometer +static constexpr unsigned int calibraton_duration_s = 42; ///< The total duration the routine is allowed to take calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_mask); /// Data passed to calibration worker routine -typedef struct { +struct mag_worker_data_t { orb_advert_t *mavlink_log_pub; + bool append_to_existing_calibration; + unsigned last_mag_progress; unsigned done_count; - int sub_mag[max_mags]; - unsigned int calibration_points_perside; - unsigned int calibration_interval_perside_seconds; - uint64_t calibration_interval_perside_useconds; - unsigned int calibration_counter_total[max_mags]; + unsigned calibration_sides; ///< The total number of sides bool side_data_collected[detect_orientation_side_count]; - float *x[max_mags]; - float *y[max_mags]; - float *z[max_mags]; -} mag_worker_data_t; + unsigned int calibration_points_perside; + uint64_t calibration_interval_perside_us; + unsigned int calibration_counter_total[MAX_MAGS]; + float *x[MAX_MAGS]; + float *y[MAX_MAGS]; + float *z[MAX_MAGS]; + + calibration::Magnetometer calibration[MAX_MAGS] {}; +}; int do_mag_calibration(orb_advert_t *mavlink_log_pub) { calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name); - struct mag_calibration_s mscale_null; - mscale_null.x_offset = 0.0f; - mscale_null.x_scale = 1.0f; - mscale_null.y_offset = 0.0f; - mscale_null.y_scale = 1.0f; - mscale_null.z_offset = 0.0f; - mscale_null.z_scale = 1.0f; - int result = PX4_OK; - // Determine which mags are available and reset each - - char str[30]; - - // reset the learned EKF mag in-flight bias offsets which have been learned for the previous - // sensor calibration and will be invalidated by a new sensor calibration - (void)sprintf(str, "EKF2_MAGBIAS_X"); - result = param_set_no_notification(param_find(str), &mscale_null.x_offset); - - if (result != PX4_OK) { - PX4_ERR("unable to reset %s", str); - } - - (void)sprintf(str, "EKF2_MAGBIAS_Y"); - result = param_set_no_notification(param_find(str), &mscale_null.y_offset); - - if (result != PX4_OK) { - PX4_ERR("unable to reset %s", str); - } - - (void)sprintf(str, "EKF2_MAGBIAS_Z"); - result = param_set_no_notification(param_find(str), &mscale_null.z_offset); - - if (result != PX4_OK) { - PX4_ERR("unable to reset %s", str); - } - - for (size_t i = 0; i < max_mags; i++) { - device_ids[i] = 0; // signals no mag - } - - _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++) { -#if 1 // TODO: replace all IOCTL usage - // Reset mag id to mag not available - (void)sprintf(str, "CAL_MAG%u_ID", cur_mag); - result = param_set_no_notification(param_find(str), &(device_ids[cur_mag])); - - if (result != PX4_OK) { - calibration_log_info(mavlink_log_pub, "[cal] Unable to reset CAL_MAG%u_ID", cur_mag); - break; - } - -#else - (void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag); - result = param_set_no_notification(param_find(str), &mscale_null.x_offset); - - if (result != PX4_OK) { - PX4_ERR("unable to reset %s", str); - } - - (void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag); - result = param_set_no_notification(param_find(str), &mscale_null.y_offset); - - if (result != PX4_OK) { - PX4_ERR("unable to reset %s", str); - } - - (void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag); - result = param_set_no_notification(param_find(str), &mscale_null.z_offset); - - if (result != PX4_OK) { - PX4_ERR("unable to reset %s", str); - } - - (void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag); - result = param_set_no_notification(param_find(str), &mscale_null.x_scale); - - if (result != PX4_OK) { - PX4_ERR("unable to reset %s", str); - } - - (void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag); - result = param_set_no_notification(param_find(str), &mscale_null.y_scale); - - if (result != PX4_OK) { - PX4_ERR("unable to reset %s", str); - } - - (void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag); - result = param_set_no_notification(param_find(str), &mscale_null.z_scale); - - if (result != PX4_OK) { - PX4_ERR("unable to reset %s", str); - } - -#endif - - param_notify_changes(); - -#if 1 // TODO: replace all IOCTL usage - // Attempt to open mag - (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag); - int fd = px4_open(str, O_RDONLY); - - if (fd < 0) { - continue; - } - - // Get device id for this mag - device_ids[cur_mag] = px4_ioctl(fd, DEVIOCGDEVICEID, 0); - internal[cur_mag] = (px4_ioctl(fd, MAGIOCGEXTERNAL, 0) <= 0); - - 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); - } - } - - - px4_close(fd); -#endif - } - // Calibrate all mags at the same time if (result == PX4_OK) { switch (mag_calibrate_all(mavlink_log_pub, cal_mask)) { @@ -268,7 +136,7 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub) static bool reject_sample(float sx, float sy, float sz, float x[], float y[], float z[], unsigned count, unsigned max_count) { - float min_sample_dist = fabsf(5.4f * mag_sphere_radius / sqrtf(max_count)) / 3.0f; + float min_sample_dist = fabsf(5.4f * MAG_SPHERE_RADIUS_DEFAULT / sqrtf(max_count)) / 3.0f; for (size_t i = 0; i < count; i++) { float dx = sx - x[i]; @@ -277,6 +145,9 @@ static bool reject_sample(float sx, float sy, float sz, float x[], float y[], fl float dist = sqrtf(dx * dx + dy * dy + dz * dz); if (dist < min_sample_dist) { + PX4_DEBUG("rejected X: %.3f Y: %.3f Z: %.3f (%.3f < %.3f) (%d/%d) ", (double)sx, (double)sy, (double)sz, (double)dist, + (double)min_sample_dist, count, max_count); + return true; } } @@ -286,7 +157,7 @@ static bool reject_sample(float sx, float sy, float sz, float x[], float y[], fl static unsigned progress_percentage(mag_worker_data_t *worker_data) { - return 100 * ((float)worker_data->done_count) / calibration_sides; + return 100 * ((float)worker_data->done_count) / worker_data->calibration_sides; } // Returns calibrate_return_error if any parameter is not finite @@ -295,7 +166,7 @@ static calibrate_return check_calibration_result(float offset_x, float offset_y, 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) + orb_advert_t *mavlink_log_pub, uint8_t cur_mag) { float must_be_finite[] = {offset_x, offset_y, offset_z, sphere_radius, @@ -311,21 +182,16 @@ static calibrate_return check_calibration_result(float offset_x, float offset_y, 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, #%zu)", cur_mag); + calibration_log_emergency(mavlink_log_pub, "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; - } + // earth field between 0.25 and 0.65 Gauss + if (sphere_radius < 0.2f || sphere_radius >= 0.7f) { + calibration_log_emergency(mavlink_log_pub, "Retry calibration (mag #%u sphere radius invaid %.3f)", cur_mag, + (double)sphere_radius); + return calibrate_return_error; } // Notify if a parameter which should be positive is non-positive @@ -333,8 +199,21 @@ static calibrate_return check_calibration_result(float offset_x, float offset_y, 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"); + calibration_log_emergency(mavlink_log_pub, "Retry calibration (mag #%u with non-positive scale)", 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) { + // maximum measurement range is ~1.9 Ga, the earth field is ~0.6 Ga, + // so an offset larger than ~1.3 Ga means the mag will saturate in some directions. + static constexpr float MAG_MAX_OFFSET_LEN = 1.3f; + + if (fabsf(should_be_not_huge[i]) > MAG_MAX_OFFSET_LEN) { + calibration_log_critical(mavlink_log_pub, "Warning: mag (#%u) with large offsets", cur_mag); break; } } @@ -347,16 +226,12 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta const hrt_abstime calibration_started = hrt_absolute_time(); calibrate_return result = calibrate_return_ok; - unsigned int calibration_counter_side; - mag_worker_data_t *worker_data = (mag_worker_data_t *)(data); // notify user to start rotating set_tune(TONE_SINGLE_BEEP_TUNE); - calibration_log_info(worker_data->mavlink_log_pub, "[cal] Rotate vehicle around the detected orientation"); - calibration_log_info(worker_data->mavlink_log_pub, "[cal] Continue rotation for %s %u s", - detect_orientation_str(orientation), worker_data->calibration_interval_perside_seconds); + calibration_log_info(worker_data->mavlink_log_pub, "[cal] Rotate vehicle"); /* * Detect if the system is rotating. @@ -366,15 +241,15 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta * for a good result, so we're not constraining the user more than we have to. */ - hrt_abstime detection_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds * 5; + const hrt_abstime detection_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_us * 5; hrt_abstime last_gyro = 0; float gyro_x_integral = 0.0f; float gyro_y_integral = 0.0f; float gyro_z_integral = 0.0f; - const float gyro_int_thresh_rad = 0.5f; + static constexpr float gyro_int_thresh_rad = 0.5f; - int sub_gyro = orb_subscribe(ORB_ID(sensor_gyro)); + uORB::SubscriptionBlocking gyro_sub{ORB_ID(sensor_gyro)}; while (fabsf(gyro_x_integral) < gyro_int_thresh_rad && fabsf(gyro_y_integral) < gyro_int_thresh_rad && @@ -383,29 +258,21 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta /* abort on request */ if (calibrate_cancel_check(worker_data->mavlink_log_pub, calibration_started)) { result = calibrate_return_cancelled; - px4_close(sub_gyro); return result; } /* abort with timeout */ if (hrt_absolute_time() > detection_deadline) { result = calibrate_return_error; - warnx("int: %8.4f, %8.4f, %8.4f", (double)gyro_x_integral, (double)gyro_y_integral, (double)gyro_z_integral); + PX4_ERR("gyro int: %8.4f, %8.4f, %8.4f", (double)gyro_x_integral, (double)gyro_y_integral, (double)gyro_z_integral); calibration_log_critical(worker_data->mavlink_log_pub, "Failed: This calibration requires rotation."); break; } /* Wait clocking for new data on all gyro */ - px4_pollfd_struct_t fds[1]; - fds[0].fd = sub_gyro; - fds[0].events = POLLIN; - size_t fd_count = 1; - - int poll_ret = px4_poll(fds, fd_count, 1000); + sensor_gyro_s gyro; - if (poll_ret > 0) { - sensor_gyro_s gyro{}; - orb_copy(ORB_ID(sensor_gyro), sub_gyro, &gyro); + if (gyro_sub.updateBlocking(gyro, 1000_ms)) { /* ensure we have a valid first timestamp */ if (last_gyro > 0) { @@ -421,12 +288,16 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta } } - px4_close(sub_gyro); + uORB::SubscriptionBlocking mag_sub[MAX_MAGS] { + {ORB_ID(sensor_mag), 0, 0}, + {ORB_ID(sensor_mag), 0, 1}, + {ORB_ID(sensor_mag), 0, 2}, + {ORB_ID(sensor_mag), 0, 3}, + }; - uint64_t calibration_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds; + uint64_t calibration_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_us; unsigned poll_errcount = 0; - - calibration_counter_side = 0; + unsigned calibration_counter_side = 0; while (hrt_absolute_time() < calibration_deadline && calibration_counter_side < worker_data->calibration_points_perside) { @@ -436,72 +307,79 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta break; } - // Wait clocking for new data on all mags - px4_pollfd_struct_t fds[max_mags]; - size_t fd_count = 0; - - for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { - if (worker_data->sub_mag[cur_mag] >= 0 && device_ids[cur_mag] != 0) { - fds[fd_count].fd = worker_data->sub_mag[cur_mag]; - fds[fd_count].events = POLLIN; - fd_count++; - } - } + if (mag_sub[0].updatedBlocking(1000_ms)) { + bool rejected = false; + Vector3f new_samples[MAX_MAGS] {}; - int poll_ret = px4_poll(fds, fd_count, 1000); + for (uint8_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { + sensor_mag_s mag; - if (poll_ret > 0) { + if (worker_data->calibration[cur_mag].device_id() != 0) { + if (mag_sub[cur_mag].update(&mag)) { - int prev_count[max_mags]; - bool rejected = false; + if (worker_data->append_to_existing_calibration) { + // keep and update the existing calibration when we are not doing a full 6-axis calibration + const Matrix3f &scale = worker_data->calibration[cur_mag].scale(); + const Vector3f &offset = worker_data->calibration[cur_mag].offset(); - for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { + const Vector3f m{scale *(Vector3f{mag.x, mag.y, mag.z} - offset)}; - prev_count[cur_mag] = worker_data->calibration_counter_total[cur_mag]; + mag.x = m(0); + mag.y = m(1); + mag.z = m(2); + } - if (worker_data->sub_mag[cur_mag] >= 0) { - sensor_mag_s mag{}; + // Check if this measurement is good to go in + bool reject = reject_sample(mag.x, mag.y, mag.z, + worker_data->x[cur_mag], worker_data->y[cur_mag], worker_data->z[cur_mag], + worker_data->calibration_counter_total[cur_mag], + worker_data->calibration_sides * worker_data->calibration_points_perside); - orb_copy(ORB_ID(sensor_mag), worker_data->sub_mag[cur_mag], &mag); + if (reject) { + rejected = true; - // Check if this measurement is good to go in - rejected = rejected || reject_sample(mag.x, mag.y, mag.z, - worker_data->x[cur_mag], worker_data->y[cur_mag], worker_data->z[cur_mag], - worker_data->calibration_counter_total[cur_mag], - calibration_sides * worker_data->calibration_points_perside); + } else { + new_samples[cur_mag](0) = mag.x; + new_samples[cur_mag](1) = mag.y; + new_samples[cur_mag](2) = mag.z; + } - worker_data->x[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.x; - worker_data->y[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.y; - worker_data->z[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.z; - worker_data->calibration_counter_total[cur_mag]++; + } else { + rejected = true; + } } } // Keep calibration of all mags in lockstep - if (rejected) { - // Reset counts, since one of the mags rejected the measurement - for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { - worker_data->calibration_counter_total[cur_mag] = prev_count[cur_mag]; + if (!rejected) { + for (uint8_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { + if (worker_data->calibration[cur_mag].device_id() != 0) { + worker_data->x[cur_mag][worker_data->calibration_counter_total[cur_mag]] = new_samples[cur_mag](0); + worker_data->y[cur_mag][worker_data->calibration_counter_total[cur_mag]] = new_samples[cur_mag](1); + worker_data->z[cur_mag][worker_data->calibration_counter_total[cur_mag]] = new_samples[cur_mag](2); + worker_data->calibration_counter_total[cur_mag]++; + } } - } else { calibration_counter_side++; unsigned new_progress = progress_percentage(worker_data) + - (unsigned)((100 / calibration_sides) * ((float)calibration_counter_side / (float) + (unsigned)((100 / worker_data->calibration_sides) * ((float)calibration_counter_side / (float) worker_data->calibration_points_perside)); - if (new_progress - _last_mag_progress > 3) { + if (new_progress - worker_data->last_mag_progress > 0) { // Progress indicator for side calibration_log_info(worker_data->mavlink_log_pub, "[cal] %s side calibration: progress <%u>", detect_orientation_str(orientation), new_progress); - px4_usleep(20000); + px4_usleep(10000); - _last_mag_progress = new_progress; + worker_data->last_mag_progress = new_progress; } } + PX4_DEBUG("side counter %d / %d", calibration_counter_side, worker_data->calibration_points_perside); + } else { poll_errcount++; } @@ -529,22 +407,23 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma { calibrate_return result = calibrate_return_ok; - mag_worker_data_t worker_data; + mag_worker_data_t worker_data{}; + // keep and update the existing calibration when we are not doing a full 6-axis calibration + worker_data.append_to_existing_calibration = cal_mask < ((1 << 6) - 1); worker_data.mavlink_log_pub = mavlink_log_pub; + worker_data.last_mag_progress = 0; + worker_data.calibration_sides = 0; worker_data.done_count = 0; worker_data.calibration_points_perside = calibration_total_points / detect_orientation_side_count; - 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; - - calibration_sides = 0; + worker_data.calibration_interval_perside_us = (calibraton_duration_s / detect_orientation_side_count) * 1000 * 1000; 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++; + worker_data.calibration_sides++; } else { // mark as completed from the beginning @@ -557,10 +436,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma } } - for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { - // Initialize to no subscription - worker_data.sub_mag[cur_mag] = -1; - + for (size_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { // Initialize to no memory allocated worker_data.x[cur_mag] = nullptr; worker_data.y[cur_mag] = nullptr; @@ -568,105 +444,34 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma worker_data.calibration_counter_total[cur_mag] = 0; } - const unsigned int calibration_points_maxcount = calibration_sides * worker_data.calibration_points_perside; + const unsigned int calibration_points_maxcount = worker_data.calibration_sides * worker_data.calibration_points_perside; - char str[30]; + for (uint8_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { - // Get actual mag count and alloate only as much memory as needed - const unsigned orb_mag_count = orb_group_count(ORB_ID(sensor_mag)); + uORB::SubscriptionData mag_sub{ORB_ID(sensor_mag), cur_mag}; - // Warn that we will not calibrate more than max_mags magnetometers - if (orb_mag_count > max_mags) { - calibration_log_critical(mavlink_log_pub, "Detected %u mags, but will calibrate only %u", orb_mag_count, max_mags); - } - - for (size_t cur_mag = 0; cur_mag < orb_mag_count && cur_mag < max_mags; cur_mag++) { - worker_data.x[cur_mag] = static_cast(malloc(sizeof(float) * calibration_points_maxcount)); - worker_data.y[cur_mag] = static_cast(malloc(sizeof(float) * calibration_points_maxcount)); - worker_data.z[cur_mag] = static_cast(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, "ERROR: out of memory"); - result = calibrate_return_error; + if (mag_sub.advertised() && (mag_sub.get().device_id != 0) && (mag_sub.get().timestamp > 0)) { + worker_data.calibration[cur_mag].set_device_id(mag_sub.get().device_id); + worker_data.calibration[cur_mag].set_external(mag_sub.get().is_external); } - } - - - // 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. - for (unsigned cur_mag = 0; cur_mag < orb_mag_count && cur_mag < max_mags; cur_mag++) { - - // Lock in to correct ORB instance - bool found_cur_mag = false; - - for (unsigned i = 0; i < orb_mag_count && !found_cur_mag; i++) { - worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), i); - - sensor_mag_s report{}; - orb_copy(ORB_ID(sensor_mag), worker_data.sub_mag[cur_mag], &report); -#if 1 // TODO: replace all IOCTL usage + // reset calibration index to match uORB numbering + worker_data.calibration[cur_mag].set_calibration_index(cur_mag); - // 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 (worker_data.calibration[cur_mag].device_id() != 0) { + worker_data.x[cur_mag] = static_cast(malloc(sizeof(float) * calibration_points_maxcount)); + worker_data.y[cur_mag] = static_cast(malloc(sizeof(float) * calibration_points_maxcount)); + worker_data.z[cur_mag] = static_cast(malloc(sizeof(float) * calibration_points_maxcount)); - if (report.device_id == (uint32_t)device_ids[cur_mag]) { - // Device IDs match, correct ORB instance for this mag - found_cur_mag = true; - - } else { - orb_unsubscribe(worker_data.sub_mag[cur_mag]); - } - -#else - - // For the DriverFramework drivers, we fill device ID (this is the first time) by copying one report. - device_ids[cur_mag] = report.device_id; - found_cur_mag = true; - -#endif - } - - if (!found_cur_mag) { - calibration_log_critical(mavlink_log_pub, "Mag #%u (ID %u) no matching uORB devid", cur_mag, device_ids[cur_mag]); + 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, "ERROR: out of memory"); result = calibrate_return_error; break; } - if (device_ids[cur_mag] != 0) { - // Get priority - ORB_PRIO prio = ORB_PRIO_UNINITIALIZED; - 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, "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); - } + } else { + break; } - } if (result == calibrate_return_ok) { @@ -677,60 +482,92 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma true); // true: lenient still detection } - // 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; + // calibration values for each mag + Vector3f sphere[MAX_MAGS]; + Vector3f diag[MAX_MAGS]; + Vector3f offdiag[MAX_MAGS]; + float sphere_radius[MAX_MAGS]; + + for (size_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { + sphere_radius[cur_mag] = MAG_SPHERE_RADIUS_DEFAULT; + sphere[cur_mag].zero(); + diag[cur_mag] = Vector3f{1.f, 1.f, 1.f}; + offdiag[cur_mag].zero(); } - // 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) { + + // if GPS is available use real field intensity from world magnetic model + uORB::SubscriptionData gps_sub{ORB_ID(vehicle_gps_position)}; + const vehicle_gps_position_s &gps = gps_sub.get(); + + if (hrt_elapsed_time(&gps.timestamp) < 10_s && (gps.fix_type >= 2) && (gps.eph < 1000)) { + const double lat = gps.lat / 1.e7; + const double lon = gps.lon / 1.e7; + + // magnetic field data returned by the geo library using the current GPS position + const float mag_strength_gps = get_mag_strength_gauss(lat, lon); + + PX4_INFO("[cal] using current GPS for field strength: %.4f Gauss", (double)mag_strength_gps); + + for (size_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { + sphere_radius[cur_mag] = mag_strength_gps; + } + } + + // Sphere fit the data to get calibration values + for (uint8_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { + if (worker_data.calibration[cur_mag].device_id() != 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, - &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], sphere_fit_only); - - result = check_calibration_result(sphere_x[cur_mag], sphere_y[cur_mag], sphere_z[cur_mag], + bool sphere_fit_only = worker_data.calibration_sides <= 2; + { + float fitness = 1.0e30f; + float sphere_lambda = 1.0f; + static constexpr int max_iterations = 100; + + for (int i = 0; i < max_iterations; i++) { + int ret = run_lm_sphere_fit(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag], + fitness, sphere_lambda, worker_data.calibration_counter_total[cur_mag], + &sphere[cur_mag](0), &sphere[cur_mag](1), &sphere[cur_mag](2), + &sphere_radius[cur_mag], + &diag[cur_mag](0), &diag[cur_mag](1), &diag[cur_mag](2), + &offdiag[cur_mag](0), &offdiag[cur_mag](1), &offdiag[cur_mag](2)); + + PX4_DEBUG("Mag: %d (%d/%d) sphere fit ret=%d, fitness: %.4f, lambda: %.4f", cur_mag, i, max_iterations, ret, + (double)fitness, (double)sphere_lambda); + } + + PX4_INFO("Mag: %d sphere fitness: %.4f radius: %.4f", cur_mag, (double)fitness, (double)sphere_radius[cur_mag]); + } + + if (!sphere_fit_only) { + float fitness = 1.0e30f; + float ellipsoid_lambda = 1.0f; + static constexpr int max_iterations = 100; + + for (int i = 0; i < max_iterations; i++) { + int ret = run_lm_ellipsoid_fit(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag], + fitness, ellipsoid_lambda, worker_data.calibration_counter_total[cur_mag], + &sphere[cur_mag](0), &sphere[cur_mag](1), &sphere[cur_mag](2), + &sphere_radius[cur_mag], + &diag[cur_mag](0), &diag[cur_mag](1), &diag[cur_mag](2), + &offdiag[cur_mag](0), &offdiag[cur_mag](1), &offdiag[cur_mag](2)); + + PX4_DEBUG("Mag: %d (%d/%d) ellipsoid fit ret=%d, fitness: %.4f, lambda: %.4f", cur_mag, i, max_iterations, ret, + (double)fitness, (double)ellipsoid_lambda); + } + + PX4_INFO("Mag: %d ellipsoid fitness: %.4f", cur_mag, (double)fitness); + } + + result = check_calibration_result(sphere[cur_mag](0), sphere[cur_mag](1), sphere[cur_mag](2), 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], + diag[cur_mag](0), diag[cur_mag](1), diag[cur_mag](2), + offdiag[cur_mag](0), offdiag[cur_mag](1), offdiag[cur_mag](2), mavlink_log_pub, cur_mag); if (result == calibrate_return_error) { @@ -744,158 +581,96 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma if (result == calibrate_return_ok) { // DO NOT REMOVE! Critical validation data! +#if 0 + printf("RAW DATA:\n--------------------\n"); - // printf("RAW DATA:\n--------------------\n"); - // for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { + for (uint8_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { - // if (worker_data.calibration_counter_total[cur_mag] == 0) { - // continue; - // } + 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]); + printf("RAW: MAG %u with %u samples:\n", cur_mag, 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); - // } + 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(">>>>>>>\n"); + } - // printf("CALIBRATED DATA:\n--------------------\n"); - // for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { + printf("CALIBRATED DATA:\n--------------------\n"); - // if (worker_data.calibration_counter_total[cur_mag] == 0) { - // continue; - // } + for (uint8_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { - // printf("Calibrated: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]); + if (worker_data.calibration_counter_total[cur_mag] == 0) { + continue; + } + + printf("Calibrated: MAG %u with %u samples:\n", cur_mag, 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[cur_mag](0); + float y = worker_data.y[cur_mag][i] - sphere[cur_mag](1); + float z = worker_data.z[cur_mag][i] - sphere[cur_mag](2); + printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z); + } - // 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"); + } - // printf("SPHERE RADIUS: %8.4f\n", (double)sphere_radius[cur_mag]); - // printf(">>>>>>>\n"); - // } +#endif // DO NOT REMOVE! Critical validation data! } // Data points are no longer needed - for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { + 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++) { - for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) { - if (device_ids[cur_mag] != 0) { - mag_calibration_s mscale; - -#if 1 // TODO: replace all IOCTL usage - int fd_mag = -1; - - (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, "ERROR: unable to open mag device #%u", cur_mag); - result = calibrate_return_error; - } - -#endif - - if (result == calibrate_return_ok) { - -#if 1 // TODO: replace all IOCTL usage - - // 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; - } + auto ¤t_cal = worker_data.calibration[cur_mag]; + if (current_cal.device_id() != 0) { + if (worker_data.append_to_existing_calibration) { // 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 - } + // mag_value = (mag_readout - (offset_existing + offset_new/scale_existing)) * scale_existing + Vector3f offset = current_cal.offset() + sphere[cur_mag].edivide(current_cal.scale().diag()); + current_cal.set_offset(offset); -#if 1 // TODO: replace all IOCTL usage - - // Mag device no longer needed - if (fd_mag >= 0) { - px4_close(fd_mag); + } else { + current_cal.set_offset(sphere[cur_mag]); + current_cal.set_scale(diag[cur_mag]); + current_cal.set_offdiagonal(offdiag[cur_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))); - - (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))); - - if (failed) { - calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG); - 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); + } else { + current_cal.Reset(); + } - 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); + current_cal.set_calibration_index(cur_mag); + current_cal.ParametersSave(); + } - px4_usleep(200000); - } - } - } + // reset the learned EKF mag in-flight bias offsets which have been learned for the previous + // sensor calibration and will be invalidated by a new sensor calibration + for (int axis = 0; axis < 3; axis++) { + char axis_char = 'X' + axis; + char str[20] {}; + sprintf(str, "EKF2_MAGBIAS_%c", axis_char); + float offset = 0.f; + param_set_no_notification(param_find(str), &offset); } - // Trigger a param set on the last step so the whole - // system updates - (void)param_set(param_find("CAL_MAG_PRIME"), &(device_id_primary)); + param_notify_changes(); } return result;