X-Git-Url: http://git.osdn.net/view?p=android-x86%2Fhardware-intel-libsensors.git;a=blobdiff_plain;f=compass-calibration.c;h=602f4e2b2af1221f7dc4fee9dea1a59e18864e4c;hp=e3e915391359b59db6c562e915cad4f1dc4e3a8b;hb=refs%2Fheads%2Fnougat-x86;hpb=71c0448a7783ed631da508e5d535398db2d26311 diff --git a/compass-calibration.c b/compass-calibration.c index e3e9153..602f4e2 100644 --- a/compass-calibration.c +++ b/compass-calibration.c @@ -1,68 +1,95 @@ /* - * Copyright (C) 2014 Intel Corporation. - */ +// Copyright (c) 2015 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +*/ -#include -#include #include #include -#include -#include +#include #include #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]; @@ -96,16 +123,17 @@ static void compute_eigenvalues(double mat[3][3], double* eig1, double* eig2, do 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]; @@ -131,14 +159,15 @@ static void calc_evector(double mat[3][3], double eig, double vec[3][1]) 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]; @@ -147,7 +176,7 @@ static int ellipsoid_fit (mat_input_t m, double offset[3][1], double w_invert[3] 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]; @@ -159,11 +188,12 @@ static int ellipsoid_fit (mat_input_t m, double offset[3][1], double w_invert[3] 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]; @@ -185,7 +215,6 @@ static int ellipsoid_fit (mat_input_t m, double offset[3][1], double w_invert[3] 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); @@ -202,6 +231,9 @@ static int ellipsoid_fit (mat_input_t m, double offset[3][1], double w_invert[3] 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; @@ -230,233 +262,287 @@ static int ellipsoid_fit (mat_input_t m, double offset[3][1], double w_invert[3] 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); - }