Thanks to visit codestin.com
Credit goes to github.com

Skip to content

Commit b973b2c

Browse files
bkuengdagar
bkueng
authored and
dagar
committed
mag calibration: improve 4 and 2 side calibration
- if less than 6 sides are calibrated, keep the existing calibration and update the offsets and scales - if 2 sides are calibrated, estimate the offsets only (as this is enough if a full calibration was done already, and the problem is not constrained enough to estimate scales and offsets)
1 parent 1d78f02 commit b973b2c

File tree

5 files changed

+65
-41
lines changed

5 files changed

+65
-41
lines changed

‎src/modules/commander/calibration_messages.h

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -61,6 +61,7 @@
6161

6262
#define CAL_ERROR_SENSOR_MSG "[cal] calibration failed: reading sensor"
6363
#define CAL_ERROR_RESET_CAL_MSG "[cal] calibration failed: to reset, sensor %u"
64+
#define CAL_ERROR_READ_CAL_MSG "[cal] calibration failed: to read calibration"
6465
#define CAL_ERROR_APPLY_CAL_MSG "[cal] calibration failed: to apply calibration"
6566
#define CAL_ERROR_SET_PARAMS_MSG "[cal] calibration failed: to set parameters"
6667

‎src/modules/commander/calibration_routines.cpp

Lines changed: 10 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -238,8 +238,9 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[],
238238
}
239239

240240
int ellipsoid_fit_least_squares(const float x[], const float y[], const float z[],
241-
unsigned int size, int max_iterations, float delta, float *offset_x, float *offset_y, float *offset_z,
242-
float *sphere_radius, float *diag_x, float *diag_y, float *diag_z, float *offdiag_x, float *offdiag_y, float *offdiag_z)
241+
unsigned int size, int max_iterations, float *offset_x, float *offset_y, float *offset_z,
242+
float *sphere_radius, float *diag_x, float *diag_y, float *diag_z,
243+
float *offdiag_x, float *offdiag_y, float *offdiag_z, bool sphere_fit_only)
243244
{
244245
float _fitness = 1.0e30f, _sphere_lambda = 1.0f, _ellipsoid_lambda = 1.0f;
245246

@@ -250,12 +251,14 @@ int ellipsoid_fit_least_squares(const float x[], const float y[], const float z[
250251

251252
}
252253

253-
_fitness = 1.0e30f;
254+
if (!sphere_fit_only) {
255+
_fitness = 1.0e30f;
254256

255-
for (int i = 0; i < max_iterations; i++) {
256-
run_lm_ellipsoid_fit(x, y, z, _fitness, _ellipsoid_lambda,
257-
size, offset_x, offset_y, offset_z,
258-
sphere_radius, diag_x, diag_y, diag_z, offdiag_x, offdiag_y, offdiag_z);
257+
for (int i = 0; i < max_iterations; i++) {
258+
run_lm_ellipsoid_fit(x, y, z, _fitness, _ellipsoid_lambda,
259+
size, offset_x, offset_y, offset_z,
260+
sphere_radius, diag_x, diag_y, diag_z, offdiag_x, offdiag_y, offdiag_z);
261+
}
259262
}
260263

261264
return 0;

‎src/modules/commander/calibration_routines.h

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -58,9 +58,9 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[],
5858
unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z,
5959
float *sphere_radius);
6060
int ellipsoid_fit_least_squares(const float x[], const float y[], const float z[],
61-
unsigned int size, int max_iterations, float delta, float *offset_x, float *offset_y, float *offset_z,
61+
unsigned int size, int max_iterations, float *offset_x, float *offset_y, float *offset_z,
6262
float *sphere_radius, float *diag_x, float *diag_y, float *diag_z, float *offdiag_x, float *offdiag_y,
63-
float *offdiag_z);
63+
float *offdiag_z, bool sphere_fit_only);
6464
int run_lm_sphere_fit(const float x[], const float y[], const float z[], float &_fitness, float &_sphere_lambda,
6565
unsigned int size, float *offset_x, float *offset_y, float *offset_z,
6666
float *sphere_radius, float *diag_x, float *diag_y, float *diag_z, float *offdiag_x, float *offdiag_y,

‎src/modules/commander/mag_calibration.cpp

Lines changed: 47 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -78,7 +78,7 @@ int device_prio_max = 0;
7878
int32_t device_id_primary = 0;
7979
static unsigned _last_mag_progress = 0;
8080

81-
calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub);
81+
calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_mask);
8282

8383
/// Data passed to calibration worker routine
8484
typedef struct {
@@ -143,6 +143,15 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub)
143143

144144
_last_mag_progress = 0;
145145

146+
// Collect: As defined by configuration
147+
// start with a full mask, all six bits set
148+
int32_t cal_mask = (1 << 6) - 1;
149+
param_get(param_find("CAL_MAG_SIDES"), &cal_mask);
150+
151+
// keep and update the existing calibration when we are not doing a full 6-axis calibration
152+
const bool append_to_existing_calibration = cal_mask < ((1 << 6) - 1);
153+
(void)append_to_existing_calibration;
154+
146155
for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) {
147156
#ifdef __PX4_NUTTX
148157
// Reset mag id to mag not available
@@ -214,11 +223,13 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub)
214223
device_ids[cur_mag] = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
215224
internal[cur_mag] = (px4_ioctl(fd, MAGIOCGEXTERNAL, 0) <= 0);
216225

217-
// Reset mag scale & offset
218-
result = px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);
226+
if (!append_to_existing_calibration) {
227+
// Reset mag scale & offset
228+
result = px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);
219229

220-
if (result != PX4_OK) {
221-
calibration_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, cur_mag);
230+
if (result != PX4_OK) {
231+
calibration_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, cur_mag);
232+
}
222233
}
223234

224235

@@ -228,7 +239,7 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub)
228239

229240
// Calibrate all mags at the same time
230241
if (result == PX4_OK) {
231-
switch (mag_calibrate_all(mavlink_log_pub)) {
242+
switch (mag_calibrate_all(mavlink_log_pub, cal_mask)) {
232243
case calibrate_return_cancelled:
233244
// Cancel message already displayed, we're done here
234245
result = PX4_ERROR;
@@ -516,7 +527,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
516527
return result;
517528
}
518529

519-
calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
530+
calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_mask)
520531
{
521532
calibrate_return result = calibrate_return_ok;
522533

@@ -528,11 +539,6 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
528539
worker_data.calibration_interval_perside_seconds = calibraton_duration_seconds / detect_orientation_side_count;
529540
worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000;
530541

531-
// Collect: As defined by configuration
532-
// start with a full mask, all six bits set
533-
int32_t cal_mask = (1 << 6) - 1;
534-
param_get(param_find("CAL_MAG_SIDES"), &cal_mask);
535-
536542
calibration_sides = 0;
537543

538544
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)
716722
if (device_ids[cur_mag] != 0) {
717723
// Mag in this slot is available and we should have values for it to calibrate
718724

725+
// Estimate only the offsets if two-sided calibration is selected, as the problem is not constrained
726+
// enough to reliably estimate both scales and offsets with 2 sides only (even if the existing calibration
727+
// is already close)
728+
bool sphere_fit_only = calibration_sides <= 2;
719729
ellipsoid_fit_least_squares(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag],
720-
worker_data.calibration_counter_total[cur_mag],
721-
100, 0.0f,
730+
worker_data.calibration_counter_total[cur_mag], 100,
722731
&sphere_x[cur_mag], &sphere_y[cur_mag], &sphere_z[cur_mag],
723732
&sphere_radius[cur_mag],
724733
&diag_x[cur_mag], &diag_y[cur_mag], &diag_z[cur_mag],
725-
&offdiag_x[cur_mag], &offdiag_y[cur_mag], &offdiag_z[cur_mag]);
734+
&offdiag_x[cur_mag], &offdiag_y[cur_mag], &offdiag_z[cur_mag], sphere_fit_only);
726735

727736
result = check_calibration_result(sphere_x[cur_mag], sphere_y[cur_mag], sphere_z[cur_mag],
728737
sphere_radius[cur_mag],
@@ -793,15 +802,11 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
793802

794803
for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) {
795804
if (device_ids[cur_mag] != 0) {
796-
struct mag_calibration_s mscale;
797-
mscale.x_scale = 1.0;
798-
mscale.y_scale = 1.0;
799-
mscale.z_scale = 1.0;
805+
mag_calibration_s mscale;
800806

801807
#ifdef __PX4_NUTTX
802808
int fd_mag = -1;
803809

804-
// Set new scale
805810
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
806811
fd_mag = px4_open(str, 0);
807812

@@ -810,30 +815,40 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
810815
result = calibrate_return_error;
811816
}
812817

818+
#endif
819+
813820
if (result == calibrate_return_ok) {
821+
822+
#ifdef __PX4_NUTTX
823+
824+
// Read existing calibration
814825
if (px4_ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale) != PX4_OK) {
815-
calibration_log_critical(mavlink_log_pub, "ERROR: failed to get current calibration #%u", cur_mag);
826+
calibration_log_critical(mavlink_log_pub, CAL_ERROR_READ_CAL_MSG);
816827
result = calibrate_return_error;
817828
}
818-
}
819829

820-
#endif
830+
// Update calibration
831+
// The formula for applying the calibration is:
832+
// mag_value = (mag_readout - (offset_existing + offset_new/scale_existing)) * scale_existing * scale_new
833+
mscale.x_offset = mscale.x_offset + sphere_x[cur_mag] / mscale.x_scale;
834+
mscale.y_offset = mscale.y_offset + sphere_y[cur_mag] / mscale.y_scale;
835+
mscale.z_offset = mscale.z_offset + sphere_z[cur_mag] / mscale.z_scale;
836+
mscale.x_scale = mscale.x_scale * diag_x[cur_mag];
837+
mscale.y_scale = mscale.y_scale * diag_y[cur_mag];
838+
mscale.z_scale = mscale.z_scale * diag_z[cur_mag];
821839

822-
if (result == calibrate_return_ok) {
840+
if (px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != PX4_OK) {
841+
calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG);
842+
result = calibrate_return_error;
843+
}
844+
845+
#else
823846
mscale.x_offset = sphere_x[cur_mag];
824847
mscale.y_offset = sphere_y[cur_mag];
825848
mscale.z_offset = sphere_z[cur_mag];
826849
mscale.x_scale = diag_x[cur_mag];
827850
mscale.y_scale = diag_y[cur_mag];
828851
mscale.z_scale = diag_z[cur_mag];
829-
830-
#ifdef __PX4_NUTTX
831-
832-
if (px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != PX4_OK) {
833-
calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG);
834-
result = calibrate_return_error;
835-
}
836-
837852
#endif
838853
}
839854

‎src/modules/sensors/sensor_params_mag.c

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -42,6 +42,11 @@ PARAM_DEFINE_INT32(CAL_MAG_PRIME, 0);
4242
/**
4343
* Bitfield selecting mag sides for calibration
4444
*
45+
* If set to two side calibration, only the offsets are estimated, the scale
46+
* calibration is left unchanged. Thus an initial six side calibration is
47+
* recommended.
48+
*
49+
* Bits:
4550
* DETECT_ORIENTATION_TAIL_DOWN = 1
4651
* DETECT_ORIENTATION_NOSE_DOWN = 2
4752
* DETECT_ORIENTATION_LEFT = 4

0 commit comments

Comments
 (0)