From: Adriana Reus Date: Wed, 14 May 2014 12:59:45 +0000 (+0300) Subject: Magnetometer calibration X-Git-Tag: android-x86-7.1-r1~74^2 X-Git-Url: http://git.osdn.net/view?a=commitdiff_plain;h=4a088e67ef7a1556ad0ab066d2fbe3116631a0a6;p=android-x86%2Fhardware-intel-libsensors.git Magnetometer calibration Implementation ported from the MCG HAL. Algorithm gathers a sample set and computes the hard iron bias offset and the soft iron bias matrix inversion. Once the calibration data is set it's used for the following events until a better calibration data is generated. Samples are continuously gathered. Change-Id: Ia170190713bca7d54acb385c005ef4394242f24f Signed-off-by: Adriana Reus --- diff --git a/Android.mk b/Android.mk index c45af74..0ca7ab3 100644 --- a/Android.mk +++ b/Android.mk @@ -15,6 +15,8 @@ src_files := $(src_path)/entry.c \ $(src_path)/description.c \ $(src_path)/utils.c \ $(src_path)/transform.c \ + $(src_path)/compass-calibration.c \ + $(src_path)/matrix-ops.c \ LOCAL_C_INCLUDES += $(LOCAL_PATH) vendor/intel/hardware/iio-sensors LOCAL_MODULE := sensors.gmin diff --git a/calibration.h b/calibration.h new file mode 100644 index 0000000..3da2156 --- /dev/null +++ b/calibration.h @@ -0,0 +1,48 @@ +/* + * Copyright (C) 2014 Intel Corporation. + */ + +#ifndef __CALIBRATION_H__ +#define __CALIBRATION_H__ + +#define COMPASS_CALIBRATION_PATH "/data/compass.conf" +#define DS_SIZE 48 +#define EPSILON 0.000000001 + + +/* If no cal data is present - first calibration will + use a more relaxed set of values to get an initial + calibration faster */ +#define FIRST_MIN_DIFF 1.0f +#define FIRST_MAX_SQR_ERR 3.5f +#define FIRST_LOOKBACK_COUNT 4 + +#define MIN_DIFF 1.5f +#define MAX_SQR_ERR 2.5f +#define LOOKBACK_COUNT 6 + + +#ifdef DBG_RAW_DATA +#define RAW_DATA_FULL_PATH "/data/raw_compass_data_full_%d.txt" +#define RAW_DATA_SELECTED_PATH "/data/raw_compass_data_selected_%d.txt" +#endif + +typedef struct { + /* hard iron offsets */ + double offset[3][1]; + + /* soft iron matrix */ + double w_invert[3][3]; + + /* geomagnetic strength */ + double bfield; + +} calibration_data; + +typedef double mat_input_t[DS_SIZE][3]; + +void calibrate_compass (struct sensors_event_t* event, int64_t time); +void compass_read_data (const char* config_path); +void compass_store_data (const char* config_path); + +#endif diff --git a/compass-calibration.c b/compass-calibration.c new file mode 100644 index 0000000..4ac4248 --- /dev/null +++ b/compass-calibration.c @@ -0,0 +1,458 @@ +#include +#include +#include +#include +#include +#include +#include +#include "calibration.h" +#include "matrix-ops.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 + +static float select_points[DS_SIZE][3]; +static int select_point_count = 0; + +static int calibrated = 0; +static calibration_data cal_data; + +/* reset calibration algorithm */ +static void reset_calibration () +{ + int i,j; + select_point_count = 0; + for (i = 0; i < DS_SIZE; i++) + for (j=0; j < 3; j++) + select_points[i][j] = 0; +} + +static double calc_square_err (calibration_data data) +{ + double err = 0; + double raw[3][1], result[3][1], mat_diff[3][1]; + int i; + + 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]; + + 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; + + err += diff * diff; + } + err /= 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) +{ + double p = mat[0][1] * mat[0][1] + mat[0][2] * mat[0][2] + mat[1][2] * mat[1][2]; + + if (p < EPSILON) { + *eig1 = mat[0][0]; + *eig2 = mat[1][1]; + *eig3 = mat[2][2]; + return; + } + + double q = (mat[0][0] + mat[1][1] + mat[2][2]) / 3; + double temp1 = mat[0][0] - q; + double temp2 = mat[1][1] - q; + double temp3 = mat[2][2] - q; + + p = temp1 * temp1 + temp2 * temp2 + temp3 * temp3 + 2 * p; + p = sqrt(p / 6); + + double mat2[3][3]; + assign(3, 3, mat, mat2); + mat2[0][0] -= q; + mat2[1][1] -= q; + mat2[2][2] -= q; + multiply_scalar_inplace(3, 3, mat2, 1/p); + + double r = (mat2[0][0] * mat2[1][1] * mat2[2][2] + mat2[0][1] * mat2[1][2] * mat2[2][0] + + mat2[0][2] * mat2[1][0] * mat2[2][1] - mat2[0][2] * mat2[1][1] * mat2[2][0] + - mat2[0][0] * mat2[1][2] * mat2[2][1] - mat2[0][1] * mat2[1][0] * mat2[2][2]) / 2; + + double phi; + if (r <= -1.0) + phi = M_PI/3; + else if (r >= 1.0) + 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]) +{ + double h[3][3]; + double x_tmp[2][2]; + assign(3, 3, mat, h); + h[0][0] -= eig; + h[1][1] -= eig; + h[2][2] -= eig; + + double x[2][2]; + x[0][0] = h[1][1]; + x[0][1] = h[1][2]; + x[1][0] = h[2][1]; + x[1][1] = h[2][2]; + invert(2, x, x_tmp); + assign(2, 2, x_tmp, x); + + double temp1 = x[0][0] * (-h[1][0]) + x[0][1] * (-h[2][0]); + double temp2 = x[1][0] * (-h[1][0]) + x[1][1] * (-h[2][0]); + double norm = sqrt(1 + temp1 * temp1 + temp2 * temp2); + + vec[0][0] = 1.0 / norm; + vec[1][0] = temp1 / norm; + 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]; + double p_temp1[9][9]; + double p_temp2[9][DS_SIZE]; + double temp1[3][3], temp[3][3]; + double temp1_inv[3][3]; + double temp2[3][1]; + double result[9][9]; + double p[9][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++) { + w[i][0] = m[i][0] * m[i][0]; + h[i][0] = m[i][0]; + h[i][1] = m[i][1]; + h[i][2] = m[i][2]; + h[i][3] = -1 * m[i][0] * m[i][1]; + h[i][4] = -1 * m[i][0] * m[i][2]; + h[i][5] = -1 * m[i][1] * m[i][2]; + h[i][6] = -1 * m[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); + 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); + + temp1[0][0] = 2; + temp1[0][1] = p[3][0]; + temp1[0][2] = p[4][0]; + temp1[1][0] = p[3][0]; + temp1[1][1] = 2 * p[6][0]; + temp1[1][2] = p[5][0]; + temp1[2][0] = p[4][0]; + temp1[2][1] = p[5][0]; + temp1[2][2] = 2 * p[7][0]; + + temp2[0][0] = p[0][0]; + temp2[1][0] = p[1][0]; + temp2[2][0] = p[2][0]; + + invert(3, temp1, temp1_inv); + multiply(3, 3, 1, temp1_inv, temp2, offset); + double off_x = offset[0][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); + + a[0][1] = p[3][0] * a[0][0] / 2; + a[0][2] = p[4][0] * a[0][0] / 2; + a[1][2] = p[5][0] * a[0][0] / 2; + a[1][1] = p[6][0] * a[0][0]; + a[2][2] = p[7][0] * a[0][0]; + a[2][1] = a[1][2]; + a[1][0] = a[0][1]; + a[2][0] = a[0][2]; + + double eig1 = 0, eig2 = 0, eig3 = 0; + compute_eigenvalues(a, &eig1, &eig2, &eig3); + + sqrt_evals[0][0] = sqrt(eig1); + sqrt_evals[1][0] = 0; + sqrt_evals[2][0] = 0; + sqrt_evals[0][1] = 0; + sqrt_evals[1][1] = sqrt(eig2); + sqrt_evals[2][1] = 0; + sqrt_evals[0][2] = 0; + sqrt_evals[1][2] = 0; + sqrt_evals[2][2] = sqrt(eig3); + + calc_evector(a, eig1, evec1); + calc_evector(a, eig2, evec2); + calc_evector(a, eig3, evec3); + + evecs[0][0] = evec1[0][0]; + evecs[1][0] = evec1[1][0]; + evecs[2][0] = evec1[2][0]; + evecs[0][1] = evec2[0][0]; + evecs[1][1] = evec2[1][0]; + evecs[2][1] = evec2[2][0]; + evecs[0][2] = evec3[0][0]; + evecs[1][2] = evec3[1][0]; + evecs[2][2] = evec3[2][0]; + + multiply (3, 3, 3, evecs, sqrt_evals, temp1); + 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); + multiply_scalar_inplace(3, 3, w_invert, *bfield); + + return 1; +} + +static void compass_cal_init (FILE* data_file) +{ + +#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 + + 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 (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); + + } 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; + } + +} + +static void compass_store_result(FILE* data_file) +{ + if (data_file == 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); + + if (ret < 0) + ALOGE ("compass calibration - store data failed!"); +} + +static int compass_collect (struct sensors_event_t* event, int64_t current_time) +{ + float data[3] = {event->magnetic.x, event->magnetic.y, event->magnetic.z}; + int index,j; + 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++; + } + + 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 (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 + } + return 1; +} + +static void compass_compute_cal (struct sensors_event_t* event) +{ + if (!calibrated) + return; + + double result[3][1], raw[3][1], diff[3][1]; + + 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); + + 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]; +} + +static int compass_ready () +{ + mat_input_t mat; + int i; + float max_sqr_err; + + if (select_point_count < DS_SIZE) + return calibrated; + + max_sqr_err = calibrated ? MAX_SQR_ERR : FIRST_MAX_SQR_ERR; + + /* 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]; + } + + /* check if result is good */ + calibration_data new_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); + 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; + 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); + } + } + } + reset_calibration(); + return calibrated; +} + +void calibrate_compass (struct sensors_event_t* event, int64_t current_time) +{ + 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; + } +} + +void compass_read_data (const char* config_file) +{ + FILE* data_file = fopen (config_file, "r"); + + compass_cal_init(data_file); + if (data_file) + fclose(data_file); +} + +void compass_store_data (const char* config_file) +{ + FILE* data_file = fopen (config_file, "w"); + + compass_store_result(data_file); + if (data_file) + fclose(data_file); + +} diff --git a/control.c b/control.c index 9b6d2fd..e204cbe 100644 --- a/control.c +++ b/control.c @@ -13,6 +13,7 @@ #include "enumeration.h" #include "utils.h" #include "transform.h" +#include "calibration.h" /* Currently active sensors count, per device */ static int poll_sensors_per_dev[MAX_DEVICES]; /* poll-mode sensors */ @@ -211,6 +212,8 @@ int adjust_counters (int s, int enabled) */ int dev_num = sensor_info[s].dev_num; + int catalog_index = sensor_info[s].catalog_index; + int sensor_type = sensor_catalog[catalog_index].type; /* Refcount per sensor, in terms of enable count */ if (enabled) { @@ -219,8 +222,12 @@ int adjust_counters (int s, int enabled) sensor_info[s].enable_count++; - if (sensor_info[s].enable_count != 1) + if (sensor_info[s].enable_count != 1) { return 0; /* The sensor was, and remains, in use */ + } else { + if (sensor_type == SENSOR_TYPE_MAGNETIC_FIELD) + compass_read_data(COMPASS_CALIBRATION_PATH); + } } else { if (sensor_info[s].enable_count == 0) return -1; /* Spurious disable call */ @@ -228,6 +235,9 @@ int adjust_counters (int s, int enabled) ALOGI("Disabling sensor %d (iio device %d: %s)\n", s, dev_num, sensor_info[s].friendly_name); + if (sensor_type == SENSOR_TYPE_MAGNETIC_FIELD) + compass_store_data(COMPASS_CALIBRATION_PATH); + sensor_info[s].enable_count--; if (sensor_info[s].enable_count > 0) diff --git a/matrix-ops.c b/matrix-ops.c new file mode 100644 index 0000000..c845c95 --- /dev/null +++ b/matrix-ops.c @@ -0,0 +1,102 @@ +#include "matrix-ops.h" +#include +#include + +void transpose (int rows, int cols, double m[rows][cols], double m_trans[cols][rows]) +{ + int i,j; + + for (i = 0; i < rows; i++) + for (j = 0; j < cols; j++) + m_trans[j][i] = m[i][j]; +} + + +void multiply (int m, int n, int p, double m1[m][n], double m2[n][p], double result[m][p]) +{ + int i,j,k; + + for (i = 0; i < m; i++) + for (k = 0; k < p; k++) { + result [i][k] = 0; + for (j = 0; j < n; j++) + result [i][k] += m1[i][j] * m2 [j][k]; + } +} +void invert (int s, double m[s][s], double m_inv[s][s]) +{ + double t; + int swap,i,j,k; + double tmp[s][s]; + + for (i = 0; i < s; i++) + for (j = 0; j < s; j++) + m_inv[i][j] = 0; + for (i = 0; i < s; i++) + m_inv[i][i] = 1; + + assign(s,s,m,tmp); + + for (i = 0; i < s; i++) { + swap = i; + for (j = i+1; j < s; j++) { + if (fabs(tmp[i][j]) > fabs(tmp[i][i])) + swap = j; + } + + if (swap != i) { + /* swap rows */ + for (k = 0; k < s; k++) { + t = tmp[k][i]; + tmp[k][i] = tmp[k][swap]; + tmp[k][swap] = t; + + t = m_inv[k][i]; + m_inv[k][i] = m_inv[k][swap]; + m_inv[k][swap] = t; + } + } + + t = 1 / tmp[i][i]; + for (k = 0 ; k < s ; k++) { + tmp[k][i] *= t; + m_inv[k][i] *= t; + } + + for (j = 0 ; j < s ; j++) { + if (j != i) { + t = tmp[i][j]; + for (k = 0 ; k < s; k++) { + tmp[k][j] -= tmp[k][i] * t; + m_inv[k][j] -= m_inv[k][i] * t; + } + } + } + + } +} +void multiply_scalar_inplace(int rows, int cols, double m[rows][cols], double scalar) +{ + int i,j; + + for (i = 0; i < rows; i++) + for (j = 0; j < cols; j++) + m[i][j] = m[i][j] * scalar; +} +void assign (int rows, int cols, double m[rows][cols], double m1[rows][cols]) +{ + int i,j; + + for (i = 0; i < rows; i++) + for (j = 0; j < cols; j++) + m1[i][j] = m[i][j]; +} + +void substract (int rows, int cols, double m1[rows][cols], double m2[rows][cols], double res[rows][cols]) +{ + int i,j; + + for (i = 0; i < rows; i++) + for (j = 0; j < cols; j++) + res[i][j] = m1[i][j] - m2[i][j]; +} diff --git a/matrix-ops.h b/matrix-ops.h new file mode 100644 index 0000000..dc72247 --- /dev/null +++ b/matrix-ops.h @@ -0,0 +1,11 @@ +#ifndef __MATRIX_OPS__ +#define __MATRIX_OPS__ + +void transpose (int rows, int cols, double m[rows][cols], double m_trans[cols][rows]); +void multiply (int m, int n, int p, double m1[m][n], double m2[n][p], double result[m][p]); +void invert (int s, double m[s][s], double m_inv[s][s]); +void multiply_scalar_inplace(int rows, int cols, double m[rows][cols], double scalar); +void assign (int rows, int cols, double m[rows][cols], double m1[rows][cols]); +void substract (int rows, int cols, double m1[rows][cols], double m2[rows][cols], double res[rows][cols]); + +#endif diff --git a/transform.c b/transform.c index 1c281c3..705d5de 100644 --- a/transform.c +++ b/transform.c @@ -10,6 +10,7 @@ #include "common.h" #include "transform.h" #include "utils.h" +#include "calibration.h" /*----------------------------------------------------------------------------*/ @@ -201,6 +202,9 @@ static void finalize_sample_default(int s, struct sensors_event_t* data) data->data[0] = x; data->data[1] = y; data->data[2] = z; + + /* Calibrate compass */ + calibrate_compass (data, get_timestamp()); break; case SENSOR_TYPE_GYROSCOPE: @@ -222,6 +226,7 @@ static void finalize_sample_default(int s, struct sensors_event_t* data) /* Only keep two decimals for temperature readings */ data->data[0] = 0.01 * ((int) (data->data[0] * 100)); break; + } }