From 6b0ed71ae02a56692a80e47bd11baa7e14d9284e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 25 Sep 2012 16:36:33 +0200 Subject: Simplified magnetometer calibration routine --- apps/commander/commander.c | 183 ++++++++++++--------------------------------- 1 file changed, 49 insertions(+), 134 deletions(-) (limited to 'apps/commander/commander.c') diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 7535b9046..4c20a2f71 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -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; - - 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; - } + const int peak_samples = 500; + + 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]; - - /* 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[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]; + } + + 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) { -- cgit v1.2.3