/*
- * Copyright (C) 2014 Intel Corporation.
+ * Copyright (C) 2014-2015 Intel Corporation.
*/
-#include <errno.h>
-#include <fcntl.h>
#include <math.h>
#include <hardware/sensors.h>
-#include <sys/stat.h>
-#include <string.h>
+#include <stdio.h>
#include <utils/Log.h>
#include "calibration.h"
#include "matrix-ops.h"
+#include "description.h"
-#ifdef DBG_RAW_DATA
-#define MAX_RAW_DATA_COUNT 2000
-static FILE *raw_data = NULL;
-static FILE *raw_data_selected = NULL;
-static FILE *compensation_data = NULL;
-static int raw_data_count = 0;
-int file_no = 0;
-#endif
+/* Compass defines */
+#define COMPASS_CALIBRATION_PATH "/data/compass.conf"
+#define EPSILON 0.000000001
-static float select_points[DS_SIZE][3];
-static int select_point_count = 0;
+#define MAGNETIC_LOW 960 /* 31 micro tesla squared */
+#define CAL_STEPS 5
+#define CAL_VERSION 1.0
-static int calibrated = 0;
-static calibration_data cal_data;
+/* We'll have multiple calibration levels so that we can provide an estimation as fast as possible */
+static const float min_diffs [CAL_STEPS] = {0.2, 0.25, 0.4, 0.6, 1.0};
+static const float max_sqr_errs [CAL_STEPS] = {10.0, 10.0, 8.0, 5.0, 3.5};
+static const unsigned int lookback_counts [CAL_STEPS] = {2, 3, 4, 5, 6 };
-/* reset calibration algorithm */
-static void reset_calibration ()
+
+/* Reset calibration algorithm */
+static void reset_sample (compass_cal_t* data)
{
int i,j;
- select_point_count = 0;
- for (i = 0; i < DS_SIZE; i++)
+ data->sample_count = 0;
+ for (i = 0; i < MAGN_DS_SIZE; i++)
for (j=0; j < 3; j++)
- select_points[i][j] = 0;
+ data->sample[i][j] = 0;
+
+ data->average[0] = data->average[1] = data->average[2] = 0;
}
-static double calc_square_err (calibration_data data)
+
+static double calc_square_err (compass_cal_t* data)
{
double err = 0;
double raw[3][1], result[3][1], mat_diff[3][1];
int i;
+ float stdev[3] = {0,0,0};
+ double diff;
- for (i = 0; i < DS_SIZE; i++) {
- raw[0][0] = select_points[i][0];
- raw[1][0] = select_points[i][1];
- raw[2][0] = select_points[i][2];
+ for (i = 0; i < MAGN_DS_SIZE; i++) {
+ raw[0][0] = data->sample[i][0];
+ raw[1][0] = data->sample[i][1];
+ raw[2][0] = data->sample[i][2];
- substract (3, 1, raw, data.offset, mat_diff);
- multiply(3, 3, 1, data.w_invert, mat_diff, result);
+ stdev[0] += (raw[0][0] - data->average[0]) * (raw[0][0] - data->average[0]);
+ stdev[1] += (raw[1][0] - data->average[1]) * (raw[1][0] - data->average[1]);
+ stdev[2] += (raw[2][0] - data->average[2]) * (raw[2][0] - data->average[2]);
- double diff = sqrt(result[0][0] * result[0][0] + result[1][0] * result[1][0]
- + result[2][0] * result[2][0]) - data.bfield;
+ substract (3, 1, raw, data->offset, mat_diff);
+ multiply(3, 3, 1, data->w_invert, mat_diff, result);
+
+ diff = sqrt(result[0][0] * result[0][0] + result[1][0] * result[1][0] + result[2][0] * result[2][0]) - data->bfield;
err += diff * diff;
}
- err /= DS_SIZE;
+
+ stdev[0] = sqrt(stdev[0] / MAGN_DS_SIZE);
+ stdev[1] = sqrt(stdev[1] / MAGN_DS_SIZE);
+ stdev[2] = sqrt(stdev[2] / MAGN_DS_SIZE);
+
+ /* A sanity check - if we have too little variation for an axis it's best to reject the calibration than risking a wrong calibration */
+ if (stdev[0] <= 1 || stdev[1] <= 1 || stdev[2] <= 1)
+ return max_sqr_errs[0];
+
+ err /= MAGN_DS_SIZE;
return err;
}
-// Given an real symmetric 3x3 matrix A, compute the eigenvalues
-static void compute_eigenvalues(double mat[3][3], double* eig1, double* eig2, double* eig3)
+
+/* Given an real symmetric 3x3 matrix A, compute the eigenvalues */
+static void compute_eigenvalues (double mat[3][3], double* eig1, double* eig2, double* eig3)
{
double p = mat[0][1] * mat[0][1] + mat[0][2] * mat[0][2] + mat[1][2] * mat[1][2];
if (r <= -1.0)
phi = M_PI/3;
else if (r >= 1.0)
- phi = 0;
- else
- phi = acos(r) / 3;
+ phi = 0;
+ else
+ phi = acos(r) / 3;
*eig3 = q + 2 * p * cos(phi);
*eig1 = q + 2 * p * cos(phi + 2 * M_PI / 3);
*eig2 = 3 * q - *eig1 - *eig3;
}
-static void calc_evector(double mat[3][3], double eig, double vec[3][1])
+
+static void calc_evector (double mat[3][3], double eig, double vec[3][1])
{
double h[3][3];
double x_tmp[2][2];
vec[2][0] = temp2 / norm;
}
+
static int ellipsoid_fit (mat_input_t m, double offset[3][1], double w_invert[3][3], double* bfield)
{
- int i,j;
- double h[DS_SIZE][9];
- double w[DS_SIZE][1];
- double h_trans[9][DS_SIZE];
+ int i;
+ double h[MAGN_DS_SIZE][9];
+ double w[MAGN_DS_SIZE][1];
+ double h_trans[9][MAGN_DS_SIZE];
double p_temp1[9][9];
- double p_temp2[9][DS_SIZE];
+ double p_temp2[9][MAGN_DS_SIZE];
double temp1[3][3], temp[3][3];
double temp1_inv[3][3];
double temp2[3][1];
double a[3][3], sqrt_evals[3][3], evecs[3][3], evecs_trans[3][3];
double evec1[3][1], evec2[3][1], evec3[3][1];
- for (i = 0; i < DS_SIZE; i++) {
+ for (i = 0; i < MAGN_DS_SIZE; i++) {
w[i][0] = m[i][0] * m[i][0];
h[i][0] = m[i][0];
h[i][1] = m[i][1];
h[i][7] = -1 * m[i][2] * m[i][2];
h[i][8] = 1;
}
- transpose (DS_SIZE, 9, h, h_trans);
- multiply (9, DS_SIZE, 9, h_trans, h, result);
+
+ transpose (MAGN_DS_SIZE, 9, h, h_trans);
+ multiply (9, MAGN_DS_SIZE, 9, h_trans, h, result);
invert (9, result, p_temp1);
- multiply (9, 9, DS_SIZE, p_temp1, h_trans, p_temp2);
- multiply (9, DS_SIZE, 1, p_temp2, w, p);
+ multiply (9, 9, MAGN_DS_SIZE, p_temp1, h_trans, p_temp2);
+ multiply (9, MAGN_DS_SIZE, 1, p_temp2, w, p);
temp1[0][0] = 2;
temp1[0][1] = p[3][0];
double off_y = offset[1][0];
double off_z = offset[2][0];
-
a[0][0] = 1.0 / (p[8][0] + off_x * off_x + p[6][0] * off_y * off_y
+ p[7][0] * off_z * off_z + p[3][0] * off_x * off_y
+ p[4][0] * off_x * off_z + p[5][0] * off_y * off_z);
double eig1 = 0, eig2 = 0, eig3 = 0;
compute_eigenvalues(a, &eig1, &eig2, &eig3);
+ if (eig1 <=0 || eig2 <= 0 || eig3 <= 0)
+ return 0;
+
sqrt_evals[0][0] = sqrt(eig1);
sqrt_evals[1][0] = 0;
sqrt_evals[2][0] = 0;
transpose(3, 3, evecs, evecs_trans);
multiply (3, 3, 3, temp1, evecs_trans, temp);
transpose (3, 3, temp, w_invert);
+
*bfield = pow(sqrt(1/eig1) * sqrt(1/eig2) * sqrt(1/eig3), 1.0/3.0);
+
+ if (*bfield < 0)
+ return 0;
+
multiply_scalar_inplace(3, 3, w_invert, *bfield);
return 1;
}
-static void compass_cal_init (FILE* data_file)
+
+static void compass_cal_init (FILE* data_file, sensor_info_t* info)
{
+ compass_cal_t* cal_data = (compass_cal_t*) info->cal_data;
+ int cal_steps = (info->max_cal_level && info->max_cal_level <= CAL_STEPS) ? info->max_cal_level : CAL_STEPS;
+ float version;
-#ifdef DBG_RAW_DATA
- if (raw_data) {
- fclose(raw_data);
- raw_data = NULL;
- }
+ if (cal_data == NULL)
+ return;
- if (raw_data_selected) {
- fclose(raw_data_selected);
- raw_data_selected = NULL;
- }
+ int data_count = 15;
+ reset_sample(cal_data);
- char path[64];
- snprintf(path, 64, RAW_DATA_FULL_PATH, file_no);
- raw_data = fopen(path,"w+");
- snprintf(path, 64, RAW_DATA_SELECTED_PATH, file_no);
- raw_data_selected = fopen(path,"w+");
- file_no++;
- raw_data_count = 0;
-#endif
-
- int data_count = 14;
- reset_calibration();
- calibrated = 0;
-
- if (data_file != NULL) {
- int ret = fscanf(data_file, "%d %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
- &calibrated, &cal_data.offset[0][0], &cal_data.offset[1][0], &cal_data.offset[2][0],
- &cal_data.w_invert[0][0], &cal_data.w_invert[0][1], &cal_data.w_invert[0][2],
- &cal_data.w_invert[1][0], &cal_data.w_invert[1][1], &cal_data.w_invert[1][2],
- &cal_data.w_invert[2][0], &cal_data.w_invert[2][1], &cal_data.w_invert[2][2],
- &cal_data.bfield);
-
- if (ret != data_count) {
- calibrated = 0;
- }
- }
+ if (!info->cal_level && data_file != NULL) {
+ int ret = fscanf(data_file, "%f %d %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
+ &version, &info->cal_level,
+ &cal_data->offset[0][0], &cal_data->offset[1][0], &cal_data->offset[2][0],
+ &cal_data->w_invert[0][0], &cal_data->w_invert[0][1], &cal_data->w_invert[0][2],
+ &cal_data->w_invert[1][0], &cal_data->w_invert[1][1], &cal_data->w_invert[1][2],
+ &cal_data->w_invert[2][0], &cal_data->w_invert[2][1], &cal_data->w_invert[2][2],
+ &cal_data->bfield);
- if (calibrated) {
- ALOGI("CompassCalibration: load old data, caldata: %f %f %f %f %f %f %f %f %f %f %f %f %f",
- cal_data.offset[0][0], cal_data.offset[1][0], cal_data.offset[2][0],
- cal_data.w_invert[0][0], cal_data.w_invert[0][1],cal_data.w_invert[0][2],cal_data.w_invert[1][0],
- cal_data.w_invert[1][1], cal_data.w_invert[1][2],cal_data.w_invert[2][0],cal_data.w_invert[2][1],
- cal_data.w_invert[2][2], cal_data.bfield);
+ if (ret != data_count || info->cal_level >= cal_steps || version != CAL_VERSION)
+ info->cal_level = 0;
+ }
+ if (info->cal_level) {
+ ALOGV("CompassCalibration: load old data, caldata: %f %f %f %f %f %f %f %f %f %f %f %f %f",
+ cal_data->offset[0][0], cal_data->offset[1][0], cal_data->offset[2][0],
+ cal_data->w_invert[0][0], cal_data->w_invert[0][1], cal_data->w_invert[0][2], cal_data->w_invert[1][0],
+ cal_data->w_invert[1][1], cal_data->w_invert[1][2], cal_data->w_invert[2][0], cal_data->w_invert[2][1],
+ cal_data->w_invert[2][2], cal_data->bfield);
} else {
- cal_data.offset[0][0] = 0;
- cal_data.offset[1][0] = 0;
- cal_data.offset[2][0] = 0;
-
- cal_data.w_invert[0][0] = 1;
- cal_data.w_invert[1][0] = 0;
- cal_data.w_invert[2][0] = 0;
- cal_data.w_invert[0][1] = 0;
- cal_data.w_invert[1][1] = 1;
- cal_data.w_invert[2][1] = 0;
- cal_data.w_invert[0][2] = 0;
- cal_data.w_invert[1][2] = 0;
- cal_data.w_invert[2][2] = 1;
-
- cal_data.bfield = 0;
+ cal_data->offset[0][0] = 0;
+ cal_data->offset[1][0] = 0;
+ cal_data->offset[2][0] = 0;
+
+ cal_data->w_invert[0][0] = 1;
+ cal_data->w_invert[1][0] = 0;
+ cal_data->w_invert[2][0] = 0;
+ cal_data->w_invert[0][1] = 0;
+ cal_data->w_invert[1][1] = 1;
+ cal_data->w_invert[2][1] = 0;
+ cal_data->w_invert[0][2] = 0;
+ cal_data->w_invert[1][2] = 0;
+ cal_data->w_invert[2][2] = 1;
+
+ cal_data->bfield = 0;
}
-
}
-static void compass_store_result(FILE* data_file)
+
+static void compass_store_result (FILE* data_file, sensor_info_t* info)
{
- if (data_file == NULL)
+ compass_cal_t* cal_data = (compass_cal_t*) info->cal_data;
+
+ if (data_file == NULL || cal_data == NULL)
return;
- int ret = fprintf(data_file, "%d %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
- calibrated, cal_data.offset[0][0], cal_data.offset[1][0], cal_data.offset[2][0],
- cal_data.w_invert[0][0], cal_data.w_invert[0][1], cal_data.w_invert[0][2],
- cal_data.w_invert[1][0], cal_data.w_invert[1][1], cal_data.w_invert[1][2],
- cal_data.w_invert[2][0], cal_data.w_invert[2][1], cal_data.w_invert[2][2],
- cal_data.bfield);
+
+ int ret = fprintf(data_file, "%f %d %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
+ CAL_VERSION, info->cal_level,
+ cal_data->offset[0][0], cal_data->offset[1][0], cal_data->offset[2][0],
+ cal_data->w_invert[0][0], cal_data->w_invert[0][1], cal_data->w_invert[0][2],
+ cal_data->w_invert[1][0], cal_data->w_invert[1][1], cal_data->w_invert[1][2],
+ cal_data->w_invert[2][0], cal_data->w_invert[2][1], cal_data->w_invert[2][2],
+ cal_data->bfield);
if (ret < 0)
- ALOGE ("compass calibration - store data failed!");
+ ALOGE ("Compass calibration - store data failed!");
}
-static int compass_collect (struct sensors_event_t* event, int64_t current_time)
+
+static int compass_collect (sensors_event_t* event, sensor_info_t* info)
{
float data[3] = {event->magnetic.x, event->magnetic.y, event->magnetic.z};
- int index,j;
- int lookback_count;
+ unsigned int index,j;
+ unsigned int lookback_count;
float min_diff;
-#ifdef DBG_RAW_DATA
- if (raw_data && raw_data_count < MAX_RAW_DATA_COUNT) {
- fprintf(raw_data, "%f %f %f\n", (double)data[0], (double)data[1],
- (double)data[2]);
- raw_data_count++;
- }
+ compass_cal_t* cal_data = (compass_cal_t*) info->cal_data;
- if (raw_data && raw_data_count >= MAX_RAW_DATA_COUNT) {
- fclose(raw_data);
- raw_data = NULL;
- }
-#endif
-
- lookback_count = calibrated ? LOOKBACK_COUNT : FIRST_LOOKBACK_COUNT;
- min_diff = calibrated ? MIN_DIFF : FIRST_MIN_DIFF;
-
- // For the current point to be accepted, each x/y/z value must be different enough
- // to the last several collected points
- if (select_point_count > 0 && select_point_count < DS_SIZE) {
- int lookback = lookback_count < select_point_count ? lookback_count : select_point_count;
- for (index = 0; index < lookback; index++){
- for (j = 0; j < 3; j++) {
- if (fabsf(data[j] - select_points[select_point_count-1-index][j]) < min_diff) {
- ALOGV("CompassCalibration:point reject: [%f,%f,%f], selected_count=%d",
- data[0], data[1], data[2], select_point_count);
- return 0;
+ if (cal_data == NULL)
+ return -1;
+
+ /* Discard the point if not valid */
+ if (data[0] == 0 || data[1] == 0 || data[2] == 0)
+ return -1;
+
+ lookback_count = lookback_counts[info->cal_level];
+ min_diff = min_diffs[info->cal_level];
+
+ /* For the current point to be accepted, each x/y/z value must be different enough to the last several collected points */
+ if (cal_data->sample_count > 0 && cal_data->sample_count < MAGN_DS_SIZE) {
+ unsigned int lookback = lookback_count < cal_data->sample_count ? lookback_count : cal_data->sample_count;
+ for (index = 0; index < lookback; index++)
+ for (j = 0; j < 3; j++)
+ if (fabsf(data[j] - cal_data->sample[cal_data->sample_count-1-index][j]) < min_diff) {
+ ALOGV("CompassCalibration:point reject: [%f,%f,%f], selected_count=%d", data[0], data[1], data[2], cal_data->sample_count);
+ return 0;
}
- }
- }
}
- if (select_point_count < DS_SIZE) {
- memcpy(select_points[select_point_count], data, sizeof(float) * 3);
- select_point_count++;
- ALOGV("CompassCalibration:point collected [%f,%f,%f], selected_count=%d",
- (double)data[0], (double)data[1], (double)data[2], select_point_count);
-#ifdef DBG_RAW_DATA
- if (raw_data_selected) {
- fprintf(raw_data_selected, "%f %f %f\n", (double)data[0], (double)data[1], (double)data[2]);
- }
-#endif
+ if (cal_data->sample_count < MAGN_DS_SIZE) {
+ memcpy(cal_data->sample[cal_data->sample_count], data, sizeof(float) * 3);
+ cal_data->sample_count++;
+ cal_data->average[0] += data[0];
+ cal_data->average[1] += data[1];
+ cal_data->average[2] += data[2];
+ ALOGV("CompassCalibration:point collected [%f,%f,%f], selected_count=%d", (double)data[0], (double)data[1], (double)data[2], cal_data->sample_count);
}
return 1;
}
-static void compass_compute_cal (struct sensors_event_t* event)
+
+static void scale_event (sensors_event_t* event)
{
- if (!calibrated)
- return;
+ float sqr_norm = 0;
+ float sanity_norm = 0;
+ float scale = 1;
+
+ sqr_norm = (event->magnetic.x * event->magnetic.x +
+ event->magnetic.y * event->magnetic.y +
+ event->magnetic.z * event->magnetic.z);
+
+ if (sqr_norm < MAGNETIC_LOW)
+ sanity_norm = MAGNETIC_LOW;
+
+ if (sanity_norm && sqr_norm) {
+ scale = sanity_norm / sqr_norm;
+ scale = sqrt(scale);
+ event->magnetic.x = event->magnetic.x * scale;
+ event->magnetic.y = event->magnetic.y * scale;
+ event->magnetic.z = event->magnetic.z * scale;
+ }
+}
+
+static void compass_compute_cal (sensors_event_t* event, sensor_info_t* info)
+{
+ compass_cal_t* cal_data = (compass_cal_t*) info->cal_data;
double result[3][1], raw[3][1], diff[3][1];
+ if (!info->cal_level || cal_data == NULL)
+ return;
+
raw[0][0] = event->magnetic.x;
raw[1][0] = event->magnetic.y;
raw[2][0] = event->magnetic.z;
- substract(3, 1, raw, cal_data.offset, diff);
- multiply (3, 3, 1, cal_data.w_invert, diff, result);
+ substract(3, 1, raw, cal_data->offset, diff);
+ multiply (3, 3, 1, cal_data->w_invert, diff, result);
event->magnetic.x = event->data[0] = result[0][0];
event->magnetic.y = event->data[1] = result[1][0];
event->magnetic.z = event->data[2] = result[2][0];
+
+ scale_event(event);
}
-static int compass_ready ()
+
+static int compass_ready (sensor_info_t* info)
{
mat_input_t mat;
int i;
float max_sqr_err;
- if (select_point_count < DS_SIZE)
- return calibrated;
+ compass_cal_t* cal_data = (compass_cal_t*) info->cal_data;
+ compass_cal_t new_cal_data;
+
+ /*
+ * Some sensors take unrealistically long to calibrate at higher levels. We'll use a max_cal_level if we have such a property setup,
+ * or go with the default settings if not.
+ */
+ int cal_steps = (info->max_cal_level && info->max_cal_level <= CAL_STEPS) ? info->max_cal_level : CAL_STEPS;
- max_sqr_err = calibrated ? MAX_SQR_ERR : FIRST_MAX_SQR_ERR;
+ if (cal_data->sample_count < MAGN_DS_SIZE)
+ return info->cal_level;
- /* enough points have been collected, do the ellipsoid calibration */
- for (i = 0; i < DS_SIZE; i++) {
- mat[i][0] = select_points[i][0];
- mat[i][1] = select_points[i][1];
- mat[i][2] = select_points[i][2];
+ max_sqr_err = max_sqr_errs[info->cal_level];
+
+ /* Enough points have been collected, do the ellipsoid calibration */
+
+ /* Compute average per axis */
+ cal_data->average[0] /= MAGN_DS_SIZE;
+ cal_data->average[1] /= MAGN_DS_SIZE;
+ cal_data->average[2] /= MAGN_DS_SIZE;
+
+ for (i = 0; i < MAGN_DS_SIZE; i++) {
+ mat[i][0] = cal_data->sample[i][0];
+ mat[i][1] = cal_data->sample[i][1];
+ mat[i][2] = cal_data->sample[i][2];
}
- /* check if result is good */
- calibration_data new_cal_data;
+ /* Check if result is good. The sample data must remain the same */
+ new_cal_data = *cal_data;
+
if (ellipsoid_fit(mat, new_cal_data.offset, new_cal_data.w_invert, &new_cal_data.bfield)) {
- double new_err = calc_square_err (new_cal_data);
- ALOGV("new err is %f, max sqr err id %f", new_err,max_sqr_err);
+ double new_err = calc_square_err (&new_cal_data);
+ ALOGI("new err is %f, max sqr err id %f", new_err,max_sqr_err);
if (new_err < max_sqr_err) {
double err = calc_square_err(cal_data);
if (new_err < err) {
- /* new cal data is better, so we switch to the new */
- cal_data = new_cal_data;
- calibrated = 1;
+ /* New cal data is better, so we switch to the new */
+ memcpy(cal_data->offset, new_cal_data.offset, sizeof(cal_data->offset));
+ memcpy(cal_data->w_invert, new_cal_data.w_invert, sizeof(cal_data->w_invert));
+ cal_data->bfield = new_cal_data.bfield;
+ if (info->cal_level < (cal_steps - 1))
+ info->cal_level++;
ALOGV("CompassCalibration: ready check success, caldata: %f %f %f %f %f %f %f %f %f %f %f %f %f, err %f",
- cal_data.offset[0][0], cal_data.offset[1][0], cal_data.offset[2][0], cal_data.w_invert[0][0],
- cal_data.w_invert[0][1], cal_data.w_invert[0][2], cal_data.w_invert[1][0],cal_data.w_invert[1][1],
- cal_data.w_invert[1][2], cal_data.w_invert[2][0], cal_data.w_invert[2][1], cal_data.w_invert[2][2],
- cal_data.bfield, new_err);
+ cal_data->offset[0][0], cal_data->offset[1][0], cal_data->offset[2][0], cal_data->w_invert[0][0],
+ cal_data->w_invert[0][1], cal_data->w_invert[0][2], cal_data->w_invert[1][0], cal_data->w_invert[1][1],
+ cal_data->w_invert[1][2], cal_data->w_invert[2][0], cal_data->w_invert[2][1], cal_data->w_invert[2][2],
+ cal_data->bfield, new_err);
}
}
}
- reset_calibration();
- return calibrated;
+ reset_sample(cal_data);
+ return info->cal_level;
}
-void calibrate_compass (struct sensors_event_t* event, int64_t current_time)
+
+void calibrate_compass (int s, sensors_event_t* event)
{
- long current_time_ms = current_time / 1000000;
- compass_collect (event, current_time_ms);
- if (compass_ready()) {
- compass_compute_cal (event);
- event->magnetic.status = SENSOR_STATUS_ACCURACY_HIGH;
- } else {
- event->magnetic.status = SENSOR_STATUS_ACCURACY_LOW;
+ int cal_level;
+
+ /* Calibration is continuous */
+ compass_collect (event, &sensor[s]);
+
+ cal_level = compass_ready(&sensor[s]);
+
+ switch (cal_level) {
+ case 0:
+ scale_event(event);
+ event->magnetic.status = SENSOR_STATUS_UNRELIABLE;
+ break;
+
+ case 1:
+ compass_compute_cal (event, &sensor[s]);
+ event->magnetic.status = SENSOR_STATUS_ACCURACY_LOW;
+ break;
+
+ case 2:
+ compass_compute_cal (event, &sensor[s]);
+ event->magnetic.status = SENSOR_STATUS_ACCURACY_MEDIUM;
+ break;
+
+ default:
+ compass_compute_cal (event, &sensor[s]);
+ event->magnetic.status = SENSOR_STATUS_ACCURACY_HIGH;
+ break;
}
}
-void compass_read_data (const char* config_file)
+void compass_read_data (int s)
{
- FILE* data_file = fopen (config_file, "r");
+ FILE* data_file = fopen (COMPASS_CALIBRATION_PATH, "r");
+
+ compass_cal_init(data_file, &sensor[s]);
- compass_cal_init(data_file);
if (data_file)
fclose(data_file);
}
-void compass_store_data (const char* config_file)
+
+void compass_store_data (int s)
{
- FILE* data_file = fopen (config_file, "w");
+ FILE* data_file = fopen (COMPASS_CALIBRATION_PATH, "w");
+
+ compass_store_result(data_file, &sensor[s]);
- compass_store_result(data_file);
if (data_file)
fclose(data_file);
-
}