1 /*
2 * Copyright (C) 2016 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 #include "calibration/sphere_fit/calibration_data.h"
18
19 #include <string.h>
20
21 #include "common/math/vec.h"
22
23 // FUNCTION IMPLEMENTATIONS
24 //////////////////////////////////////////////////////////////////////////////
25
26 // Set calibration data to identity scale factors, zero skew and
27 // zero bias.
calDataReset(struct ThreeAxisCalData * calstruct)28 void calDataReset(struct ThreeAxisCalData* calstruct) {
29 memset(calstruct, 0, sizeof(struct ThreeAxisCalData));
30 calstruct->scale_factor_x = 1.0f;
31 calstruct->scale_factor_y = 1.0f;
32 calstruct->scale_factor_z = 1.0f;
33 }
34
calDataCorrectData(const struct ThreeAxisCalData * calstruct,const float x_impaired[THREE_AXIS_DIM],float * x_corrected)35 void calDataCorrectData(const struct ThreeAxisCalData* calstruct,
36 const float x_impaired[THREE_AXIS_DIM],
37 float* x_corrected) {
38 // x_temp = (x_impaired - bias).
39 float x_temp[THREE_AXIS_DIM];
40 vecSub(x_temp, x_impaired, calstruct->bias, THREE_AXIS_DIM);
41
42 // x_corrected = scale_skew_mat * x_temp, where:
43 // scale_skew_mat = [scale_factor_x 0 0
44 // skew_yx scale_factor_y 0
45 // skew_zx skew_zy scale_factor_z].
46 x_corrected[0] = calstruct->scale_factor_x * x_temp[0];
47 x_corrected[1] =
48 calstruct->skew_yx * x_temp[0] + calstruct->scale_factor_y * x_temp[1];
49 x_corrected[2] = calstruct->skew_zx * x_temp[0] +
50 calstruct->skew_zy * x_temp[1] +
51 calstruct->scale_factor_z * x_temp[2];
52 }
53