mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-06 16:49:51 +08:00
Simplified magnetometer calibration routine
This commit is contained in:
Julian Oes
+47
-132
@@ -307,30 +307,10 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
|
||||
const uint64_t calibration_interval_us = 45 * 1000000;
|
||||
unsigned int calibration_counter = 0;
|
||||
|
||||
const int peak_samples = 2000;
|
||||
/* Get rid of 10% */
|
||||
const int outlier_margin = (peak_samples) / 10;
|
||||
const int peak_samples = 500;
|
||||
|
||||
float *mag_maxima[3];
|
||||
mag_maxima[0] = (float*)malloc(peak_samples * sizeof(float));
|
||||
mag_maxima[1] = (float*)malloc(peak_samples * sizeof(float));
|
||||
mag_maxima[2] = (float*)malloc(peak_samples * sizeof(float));
|
||||
|
||||
float *mag_minima[3];
|
||||
mag_minima[0] = (float*)malloc(peak_samples * sizeof(float));
|
||||
mag_minima[1] = (float*)malloc(peak_samples * sizeof(float));
|
||||
mag_minima[2] = (float*)malloc(peak_samples * sizeof(float));
|
||||
|
||||
/* initialize data table */
|
||||
for (int i = 0; i < peak_samples; i++) {
|
||||
mag_maxima[0][i] = FLT_MIN;
|
||||
mag_maxima[1][i] = FLT_MIN;
|
||||
mag_maxima[2][i] = FLT_MIN;
|
||||
|
||||
mag_minima[0][i] = FLT_MAX;
|
||||
mag_minima[1][i] = FLT_MAX;
|
||||
mag_minima[2][i] = FLT_MAX;
|
||||
}
|
||||
float mag_max[3] = {0, 0, 0};
|
||||
float mag_min[3] = {0, 0, 0};
|
||||
|
||||
int fd = open(MAG_DEVICE_PATH, 0);
|
||||
struct mag_scale mscale_null = {
|
||||
@@ -357,27 +337,25 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
|
||||
orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
|
||||
/* get min/max values */
|
||||
|
||||
/* iterate through full list */
|
||||
for (int i = 0; i < peak_samples; i++) {
|
||||
/* x minimum */
|
||||
if (raw.magnetometer_raw[0] < mag_minima[0][i])
|
||||
mag_minima[0][i] = raw.magnetometer_ga[0];
|
||||
/* y minimum */
|
||||
if (raw.magnetometer_raw[1] < mag_minima[1][i])
|
||||
mag_minima[1][i] = raw.magnetometer_ga[1];
|
||||
/* z minimum */
|
||||
if (raw.magnetometer_raw[2] < mag_minima[2][i])
|
||||
mag_minima[2][i] = raw.magnetometer_ga[2];
|
||||
if (raw.magnetometer_raw[0] < mag_min[0]) {
|
||||
mag_min[0] = raw.magnetometer_raw[0];
|
||||
}
|
||||
else if (raw.magnetometer_raw[0] > mag_max[0]) {
|
||||
mag_max[0] = raw.magnetometer_raw[0];
|
||||
}
|
||||
|
||||
/* x maximum */
|
||||
if (raw.magnetometer_raw[0] > mag_maxima[0][i])
|
||||
mag_maxima[0][i] = raw.magnetometer_ga[0];
|
||||
/* y maximum */
|
||||
if (raw.magnetometer_raw[1] > mag_maxima[1][i])
|
||||
mag_maxima[1][i] = raw.magnetometer_ga[1];
|
||||
/* z maximum */
|
||||
if (raw.magnetometer_raw[2] > mag_maxima[2][i])
|
||||
mag_maxima[2][i] = raw.magnetometer_ga[2];
|
||||
if (raw.magnetometer_raw[1] < mag_min[1]) {
|
||||
mag_min[1] = raw.magnetometer_raw[1];
|
||||
}
|
||||
else if (raw.magnetometer_raw[1] > mag_max[1]) {
|
||||
mag_max[1] = raw.magnetometer_raw[1];
|
||||
}
|
||||
|
||||
if (raw.magnetometer_raw[2] < mag_min[2]) {
|
||||
mag_min[2] = raw.magnetometer_raw[2];
|
||||
}
|
||||
else if (raw.magnetometer_raw[2] > mag_max[2]) {
|
||||
mag_max[2] = raw.magnetometer_raw[2];
|
||||
}
|
||||
|
||||
calibration_counter++;
|
||||
@@ -392,67 +370,6 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
|
||||
status->flag_preflight_mag_calibration = false;
|
||||
state_machine_publish(status_pub, status, mavlink_fd);
|
||||
|
||||
/* sort values */
|
||||
cal_bsort(mag_minima[0], peak_samples);
|
||||
cal_bsort(mag_minima[1], peak_samples);
|
||||
cal_bsort(mag_minima[2], peak_samples);
|
||||
|
||||
cal_bsort(mag_maxima[0], peak_samples);
|
||||
cal_bsort(mag_maxima[1], peak_samples);
|
||||
cal_bsort(mag_maxima[2], peak_samples);
|
||||
|
||||
float min_avg[3] = { 0.0f, 0.0f, 0.0f };
|
||||
float max_avg[3] = { 0.0f, 0.0f, 0.0f };
|
||||
|
||||
// printf("start:\n");
|
||||
|
||||
// for (int i = 0; i < 10; i++) {
|
||||
// printf("mag min: %8.4f\t%8.4f\t%8.4f\tmax: %8.4f\t%8.4f\t%8.4f\n",
|
||||
// mag_minima[0][i],
|
||||
// mag_minima[1][i],
|
||||
// mag_minima[2][i],
|
||||
// mag_maxima[0][i],
|
||||
// mag_maxima[1][i],
|
||||
// mag_maxima[2][i]);
|
||||
// usleep(10000);
|
||||
// }
|
||||
// printf("-----\n");
|
||||
|
||||
// for (int i = (peak_samples - outlier_margin)-10; i < (peak_samples - outlier_margin); i++) {
|
||||
// printf("mag min: %8.4f\t%8.4f\t%8.4f\tmax: %8.4f\t%8.4f\t%8.4f\n",
|
||||
// mag_minima[0][i],
|
||||
// mag_minima[1][i],
|
||||
// mag_minima[2][i],
|
||||
// mag_maxima[0][i],
|
||||
// mag_maxima[1][i],
|
||||
// mag_maxima[2][i]);
|
||||
// usleep(10000);
|
||||
// }
|
||||
|
||||
// printf("end\n");
|
||||
|
||||
/* take average of center value group */
|
||||
for (int i = 0; i < (peak_samples - outlier_margin); i++) {
|
||||
min_avg[0] += mag_minima[0][i+outlier_margin];
|
||||
min_avg[1] += mag_minima[1][i+outlier_margin];
|
||||
min_avg[2] += mag_minima[2][i+outlier_margin];
|
||||
|
||||
max_avg[0] += mag_maxima[0][i];
|
||||
max_avg[1] += mag_maxima[1][i];
|
||||
max_avg[2] += mag_maxima[2][i];
|
||||
}
|
||||
|
||||
min_avg[0] /= (peak_samples - outlier_margin);
|
||||
min_avg[1] /= (peak_samples - outlier_margin);
|
||||
min_avg[2] /= (peak_samples - outlier_margin);
|
||||
|
||||
max_avg[0] /= (peak_samples - outlier_margin);
|
||||
max_avg[1] /= (peak_samples - outlier_margin);
|
||||
max_avg[2] /= (peak_samples - outlier_margin);
|
||||
|
||||
// printf("\nFINAL:\nmag min: %8.4f\t%8.4f\t%8.4f\nmax: %8.4f\t%8.4f\t%8.4f\n", (double)min_avg[0],
|
||||
// (double)min_avg[1], (double)min_avg[2], (double)max_avg[0], (double)max_avg[1], (double)max_avg[2]);
|
||||
|
||||
float mag_offset[3];
|
||||
|
||||
/**
|
||||
@@ -467,31 +384,37 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
|
||||
* offset = (max + min) / 2.0f
|
||||
*/
|
||||
|
||||
mag_offset[0] = (max_avg[0] + min_avg[0]) / 2.0f;
|
||||
mag_offset[1] = (max_avg[1] + min_avg[1]) / 2.0f;
|
||||
mag_offset[2] = (max_avg[2] + min_avg[2]) / 2.0f;
|
||||
printf("max 0: %f\n",mag_max[0]);
|
||||
printf("max 1: %f\n",mag_max[1]);
|
||||
printf("max 2: %f\n",mag_max[2]);
|
||||
printf("min 0: %f\n",mag_min[0]);
|
||||
printf("min 1: %f\n",mag_min[1]);
|
||||
printf("min 2: %f\n",mag_min[2]);
|
||||
|
||||
if (!isfinite(mag_offset[1]) || !isfinite(mag_offset[1]) || !isfinite(mag_offset[2])) {
|
||||
mag_offset[0] = (mag_max[0] + mag_min[0]) / 2.0f;
|
||||
mag_offset[1] = (mag_max[1] + mag_min[1]) / 2.0f;
|
||||
mag_offset[2] = (mag_max[2] + mag_min[2]) / 2.0f;
|
||||
|
||||
mavlink_log_critical(mavlink_fd, "[commander] MAG calibration failed (INF/NAN)");
|
||||
} else {
|
||||
/* announce and set new offset */
|
||||
printf("mag off 0: %f\n",mag_offset[0]);
|
||||
printf("mag off 1: %f\n",mag_offset[1]);
|
||||
printf("mag off 2: %f\n",mag_offset[2]);
|
||||
|
||||
// char offset_output[50];
|
||||
// sprintf(offset_output, "[commander] mag cal: %8.4f %8.4f %8.4f", (double)mag_offset[0], (double)mag_offset[1], (double)mag_offset[2]);
|
||||
// mavlink_log_info(mavlink_fd, offset_output);
|
||||
/* announce and set new offset */
|
||||
|
||||
if (param_set(param_find("SENS_MAG_XOFF"), &(mag_offset[0]))) {
|
||||
fprintf(stderr, "[commander] Setting X mag offset failed!\n");
|
||||
}
|
||||
|
||||
if (param_set(param_find("SENS_MAG_YOFF"), &(mag_offset[1]))) {
|
||||
fprintf(stderr, "[commander] Setting Y mag offset failed!\n");
|
||||
}
|
||||
// char offset_output[50];
|
||||
// sprintf(offset_output, "[commander] mag cal: %8.4f %8.4f %8.4f", (double)mag_offset[0], (double)mag_offset[1], (double)mag_offset[2]);
|
||||
// mavlink_log_info(mavlink_fd, offset_output);
|
||||
|
||||
if (param_set(param_find("SENS_MAG_ZOFF"), &(mag_offset[2]))) {
|
||||
fprintf(stderr, "[commander] Setting Z mag offset failed!\n");
|
||||
}
|
||||
if (param_set(param_find("SENS_MAG_XOFF"), &(mag_offset[0]))) {
|
||||
fprintf(stderr, "[commander] Setting X mag offset failed!\n");
|
||||
}
|
||||
|
||||
if (param_set(param_find("SENS_MAG_YOFF"), &(mag_offset[1]))) {
|
||||
fprintf(stderr, "[commander] Setting Y mag offset failed!\n");
|
||||
}
|
||||
|
||||
if (param_set(param_find("SENS_MAG_ZOFF"), &(mag_offset[2]))) {
|
||||
fprintf(stderr, "[commander] Setting Z mag offset failed!\n");
|
||||
}
|
||||
|
||||
fd = open(MAG_DEVICE_PATH, 0);
|
||||
@@ -507,14 +430,6 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
|
||||
warn("WARNING: failed to set scale / offsets for mag");
|
||||
close(fd);
|
||||
|
||||
free(mag_maxima[0]);
|
||||
free(mag_maxima[1]);
|
||||
free(mag_maxima[2]);
|
||||
|
||||
free(mag_minima[0]);
|
||||
free(mag_minima[1]);
|
||||
free(mag_minima[2]);
|
||||
|
||||
/* auto-save to EEPROM */
|
||||
int save_ret = pm_save_eeprom(false);
|
||||
if(save_ret != 0) {
|
||||
|
||||
@@ -484,7 +484,7 @@ param_export(int fd, bool only_unsaved)
|
||||
|
||||
s->unsaved = false;
|
||||
|
||||
/* append the appripriate BSON type object */
|
||||
/* append the appropriate BSON type object */
|
||||
switch (param_type(s->param)) {
|
||||
case PARAM_TYPE_INT32:
|
||||
param_get(s->param, &i);
|
||||
@@ -688,4 +688,4 @@ param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_chang
|
||||
|
||||
func(arg, param);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user