aboutsummaryrefslogtreecommitdiff
path: root/apps
diff options
context:
space:
mode:
Diffstat (limited to 'apps')
-rw-r--r--apps/commander/commander.c183
-rw-r--r--apps/systemlib/param/param.c4
2 files changed, 51 insertions, 136 deletions
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) {
diff --git a/apps/systemlib/param/param.c b/apps/systemlib/param/param.c
index a01c170a6..0ab7c0ea3 100644
--- a/apps/systemlib/param/param.c
+++ b/apps/systemlib/param/param.c
@@ -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);
}
-} \ No newline at end of file
+}
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-11 18:51:39 +0000