X-Git-Url: http://git.osdn.net/view?a=blobdiff_plain;f=compass-calibration.c;h=602f4e2b2af1221f7dc4fee9dea1a59e18864e4c;hb=refs%2Ftags%2Fandroid-x86-8.1-r1;hp=2bbdce6157fb71eec79d9d6729db6cd2890ad000;hpb=cae87f2ed02011f3850bc890d115ddf5e539abb9;p=android-x86%2Fhardware-intel-libsensors.git diff --git a/compass-calibration.c b/compass-calibration.c index 2bbdce6..602f4e2 100644 --- a/compass-calibration.c +++ b/compass-calibration.c @@ -1,41 +1,40 @@ /* - * 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 -#define RAW_DATA_FULL_PATH "/data/raw_compass_data_full_%d.txt" -#define RAW_DATA_SELECTED_PATH "/data/raw_compass_data_selected_%d.txt" -static FILE *raw_data = NULL; -static FILE *raw_data_selected = 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 - -#define MAGNETIC_LOW 960 /* 31 micro tesla squared */ -#define CAL_STEPS 5 - -/* 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 }; + +/* Compass defines */ +#define COMPASS_CALIBRATION_PATH "/data/compass.conf" +#define EPSILON 0.000000001 + +#define MAGNETIC_LOW 960 /* 31 micro tesla squared */ +#define CAL_STEPS 5 +#define CAL_VERSION 1.0 + +/* 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 */ @@ -57,6 +56,7 @@ static double calc_square_err (compass_cal_t* data) 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 < MAGN_DS_SIZE; i++) { raw[0][0] = data->sample[i][0]; @@ -70,8 +70,7 @@ static double calc_square_err (compass_cal_t* data) substract (3, 1, raw, data->offset, mat_diff); multiply(3, 3, 1, data->w_invert, mat_diff, result); - double diff = sqrt(result[0][0] * result[0][0] + result[1][0] * result[1][0] - + result[2][0] * result[2][0]) - data->bfield; + 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; } @@ -80,10 +79,7 @@ static double calc_square_err (compass_cal_t* data) 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. - */ + /* 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]; @@ -127,9 +123,9 @@ static void compute_eigenvalues (double mat[3][3], double* eig1, double* eig2, d 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); @@ -192,6 +188,7 @@ 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 (MAGN_DS_SIZE, 9, h, h_trans); multiply (9, MAGN_DS_SIZE, 9, h_trans, h, result); invert (9, result, p_temp1); @@ -218,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); @@ -266,6 +262,7 @@ 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) @@ -279,57 +276,35 @@ static int ellipsoid_fit (mat_input_t m, double offset[3][1], double w_invert[3] static void compass_cal_init (FILE* data_file, sensor_info_t* info) { - -#ifdef DBG_RAW_DATA - if (raw_data) { - fclose(raw_data); - raw_data = NULL; - } - - if (raw_data_selected) { - fclose(raw_data_selected); - raw_data_selected = NULL; - } - - 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 - 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; + int cal_steps = (info->max_cal_level && info->max_cal_level <= CAL_STEPS) ? info->max_cal_level : CAL_STEPS; + float version; + if (cal_data == NULL) return; - int data_count = 14; + int data_count = 15; reset_sample(cal_data); if (!info->cal_level && data_file != NULL) { - int ret = fscanf(data_file, "%d %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf", - &info->cal_level, &cal_data->offset[0][0], &cal_data->offset[1][0], &cal_data->offset[2][0], + 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 (ret != data_count || info->cal_level >= cal_steps) { + 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; @@ -347,7 +322,6 @@ static void compass_cal_init (FILE* data_file, sensor_info_t* info) cal_data->bfield = 0; } - } @@ -358,15 +332,16 @@ static void compass_store_result (FILE* data_file, sensor_info_t* info) 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", - info->cal_level, cal_data->offset[0][0], cal_data->offset[1][0], cal_data->offset[2][0], + 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!"); } @@ -386,38 +361,18 @@ static int compass_collect (sensors_event_t* event, sensor_info_t* info) if (data[0] == 0 || data[1] == 0 || data[2] == 0) return -1; -#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++; - } - - if (raw_data && raw_data_count >= MAX_RAW_DATA_COUNT) { - fclose(raw_data); - raw_data = NULL; - } -#endif - 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. - */ + /* 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++) { + 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; + ALOGV("CompassCalibration:point reject: [%f,%f,%f], selected_count=%d", data[0], data[1], data[2], cal_data->sample_count); + return 0; } - } - } } if (cal_data->sample_count < MAGN_DS_SIZE) { @@ -426,13 +381,7 @@ static int compass_collect (sensors_event_t* event, sensor_info_t* info) 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); -#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 + 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; } @@ -494,12 +443,10 @@ static int compass_ready (sensor_info_t* info) 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. + * 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; + int cal_steps = (info->max_cal_level && info->max_cal_level <= CAL_STEPS) ? info->max_cal_level : CAL_STEPS; if (cal_data->sample_count < MAGN_DS_SIZE) return info->cal_level; @@ -528,7 +475,7 @@ static int compass_ready (sensor_info_t* info) 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 */ + /* 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; @@ -547,55 +494,55 @@ static int compass_ready (sensor_info_t* info) } -void calibrate_compass (sensors_event_t* event, sensor_info_t* info) +void calibrate_compass (int s, sensors_event_t* event) { int cal_level; /* Calibration is continuous */ - compass_collect (event, info); + compass_collect (event, &sensor[s]); - cal_level = compass_ready(info); + 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, info); + compass_compute_cal (event, &sensor[s]); event->magnetic.status = SENSOR_STATUS_ACCURACY_LOW; break; case 2: - compass_compute_cal (event, info); + compass_compute_cal (event, &sensor[s]); event->magnetic.status = SENSOR_STATUS_ACCURACY_MEDIUM; break; default: - compass_compute_cal (event, info); + compass_compute_cal (event, &sensor[s]); event->magnetic.status = SENSOR_STATUS_ACCURACY_HIGH; break; } } -void compass_read_data (sensor_info_t* info) +void compass_read_data (int s) { FILE* data_file = fopen (COMPASS_CALIBRATION_PATH, "r"); - compass_cal_init(data_file, info); + compass_cal_init(data_file, &sensor[s]); + if (data_file) fclose(data_file); } -void compass_store_data (sensor_info_t* info) +void compass_store_data (int s) { FILE* data_file = fopen (COMPASS_CALIBRATION_PATH, "w"); - compass_store_result(data_file, info); + compass_store_result(data_file, &sensor[s]); + if (data_file) fclose(data_file); - }