Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Mag calibration improvements and fixes #13623

Merged
merged 4 commits into from
Nov 29, 2019
Merged

Mag calibration improvements and fixes #13623

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
15d96f0
calibration: shorten too long messages
bkueng Nov 28, 2019
9249c70
fix mag calibration: remove px4_ioctl(fd, MAGIOCCALIBRATE, fd); call
bkueng Nov 28, 2019
3be28f7
mag calibration: keep time and number of points per side constant
bkueng Nov 28, 2019
3aab27a
mag calibration: improve 4 and 2 side calibration
bkueng Nov 28, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions src/modules/commander/accelerometer_calibration.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -414,7 +414,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(device_id[uorb_index])));

if (failed) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, uorb_index);
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG);
return PX4_ERROR;
}

Expand All @@ -432,7 +432,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
}

if (res != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, uorb_index);
calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG);
}

#endif
Expand Down
6 changes: 3 additions & 3 deletions src/modules/commander/airspeed_calibration.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -115,7 +115,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)

/* set scaling offset parameter */
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, 1);
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG);
goto error_return;
}
}
Expand Down Expand Up @@ -186,7 +186,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
}

if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, 1);
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG);
goto error_return;
}

Expand Down Expand Up @@ -237,7 +237,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
diff_pres_offset = 0.0f;

if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, 1);
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG);
goto error_return;
}

Expand Down
5 changes: 3 additions & 2 deletions src/modules/commander/calibration_messages.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -61,7 +61,8 @@

#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_APPLY_CAL_MSG "[cal] calibration failed: to apply calibration, sensor %u"
#define CAL_ERROR_SET_PARAMS_MSG "[cal] calibration failed: to set parameters, 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"

#endif /* CALIBRATION_MESSAGES_H_ */
17 changes: 10 additions & 7 deletions src/modules/commander/calibration_routines.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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;
Expand Down
4 changes: 2 additions & 2 deletions src/modules/commander/calibration_routines.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
2 changes: 1 addition & 1 deletion src/modules/commander/gyro_calibration.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -522,7 +522,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
px4_close(fd);

if (res != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, 1);
calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG);
}

#endif
Expand Down
93 changes: 49 additions & 44 deletions src/modules/commander/mag_calibration.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -214,31 +223,23 @@ 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
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);
}

/* calibrate range */
if (result == PX4_OK) {
result = px4_ioctl(fd, MAGIOCCALIBRATE, fd);
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_info(mavlink_log_pub, "[cal] Skipped scale calibration, sensor %u", cur_mag);
/* this is non-fatal - mark it accordingly */
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)) {
switch (mag_calibrate_all(mavlink_log_pub, cal_mask)) {
case calibrate_return_cancelled:
// Cancel message already displayed, we're done here
result = PX4_ERROR;
Expand Down Expand Up @@ -526,23 +527,18 @@ 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;

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

// 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++) {
Expand Down Expand Up @@ -726,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],
Expand Down Expand Up @@ -803,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);

Expand All @@ -820,30 +815,40 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
result = calibrate_return_error;
}

#endif

if (result == calibrate_return_ok) {

#ifdef __PX4_NUTTX

// Read existing calibration
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);
calibration_log_critical(mavlink_log_pub, CAL_ERROR_READ_CAL_MSG);
result = calibrate_return_error;
}
}

#endif
// 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 (result == calibrate_return_ok) {
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];

#ifdef __PX4_NUTTX

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
}

Expand Down Expand Up @@ -881,7 +886,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
#endif

if (failed) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, cur_mag);
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG);
result = calibrate_return_error;

} else {
Expand Down
5 changes: 5 additions & 0 deletions src/modules/sensors/sensor_params_mag.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down