fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

mag calibration: improve 4 and 2 side calibration

Browse Source 
- 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)
This commit is contained in:
Beat Küng 2019-11-28 10:50:36 +01:00 committed by Daniel Agar
parent 1d78f02733
commit b973b2ca64
5 changed files with 66 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/modules/commander/calibration_messages.h
View File

@ -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"

17
src/modules/commander/calibration_routines.cpp
View File

@ -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;

4
src/modules/commander/calibration_routines.h
View File

@ -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,

81
src/modules/commander/mag_calibration.cpp
View File

@ -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
}

5
src/modules/sensors/sensor_params_mag.c
View File

@ -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