1.1 --- a/measure.c Mon Oct 14 16:22:35 2013 +0000
1.2 +++ b/measure.c Mon Oct 14 16:50:39 2013 +0000
1.3 @@ -12,6 +12,8 @@
1.4 * (at your option) any later version.
1.5 */
1.6
1.7 +#include <stdio.h>
1.8 +#include <stdlib.h>
1.9 #include <sys/time.h>
1.10 #include <unistd.h>
1.11 #include <pthread.h>
1.12 @@ -30,6 +32,10 @@
1.13 void ui_calibrate(bool using_filter, int (*print)(const char *, ...), void (*flush)())
1.14 {
1.15 vectorf tmpB[1];
1.16 + FILE *magnetcfg;
1.17 + char fieldmins[3][8], fieldmaxs[3][8], *endptr;
1.18 + double value;
1.19 + int i;
1.20
1.21 print("Calibrating...\n");
1.22 flush();
1.23 @@ -53,6 +59,33 @@
1.24
1.25 print("Calibrated using (%.4f, %.4f, %.4f).\n", acceleration0.x, acceleration0.y, acceleration0.z);
1.26 flush();
1.27 +
1.28 + /* Read magnetometer settings, if possible. */
1.29 +
1.30 + magnetcfg = fopen(IMU_MAGNET_SETTINGS_FILE, "r");
1.31 +
1.32 + if (magnetcfg != NULL)
1.33 + {
1.34 + fscanf(magnetcfg, "%7s %7s %7s %7s %7s %7s",
1.35 + fieldmins[0], fieldmins[1], fieldmins[2],
1.36 + fieldmaxs[0], fieldmaxs[1], fieldmaxs[2]);
1.37 +
1.38 + for (i = 0; i < 3; i++)
1.39 + {
1.40 + value = strtod(fieldmins[i], &endptr);
1.41 + if (endptr != fieldmins[i])
1.42 + fieldmin.axis[i] = value;
1.43 + value = strtod(fieldmaxs[i], &endptr);
1.44 + if (endptr != fieldmaxs[i])
1.45 + fieldmax.axis[i] = value;
1.46 + }
1.47 +
1.48 + print("Calibrated using (%.1f, %.1f, %.1f), (%.1f, %.1f, %.1f).\n",
1.49 + fieldmin.x, fieldmin.y, fieldmin.z,
1.50 + fieldmax.x, fieldmax.y, fieldmax.z);
1.51 +
1.52 + fclose(magnetcfg);
1.53 + }
1.54 }
1.55
1.56 void *get_measurements(void *arg)