1 #include "calibration/common/calibration_data.h"
2
3 #include <string.h>
4
5 #include "common/math/vec.h"
6
7 // FUNCTION IMPLEMENTATIONS
8 //////////////////////////////////////////////////////////////////////////////
9
10 // Set calibration data to identity scale factors, zero skew and
11 // zero bias.
calDataReset(struct ThreeAxisCalData * calstruct)12 void calDataReset(struct ThreeAxisCalData *calstruct) {
13 memset(calstruct, 0, sizeof(struct ThreeAxisCalData));
14 calstruct->scale_factor_x = 1.0f;
15 calstruct->scale_factor_y = 1.0f;
16 calstruct->scale_factor_z = 1.0f;
17 }
18
calDataCorrectData(const struct ThreeAxisCalData * calstruct,const float x_impaired[THREE_AXIS_DIM],float * x_corrected)19 void calDataCorrectData(const struct ThreeAxisCalData* calstruct,
20 const float x_impaired[THREE_AXIS_DIM],
21 float* x_corrected) {
22 // x_temp = (x_impaired - bias).
23 float x_temp[THREE_AXIS_DIM];
24 vecSub(x_temp, x_impaired, calstruct->bias, THREE_AXIS_DIM);
25
26 // x_corrected = scale_skew_mat * x_temp, where:
27 // scale_skew_mat = [scale_factor_x 0 0
28 // skew_yx scale_factor_y 0
29 // skew_zx skew_zy scale_factor_z].
30 x_corrected[0] = calstruct->scale_factor_x * x_temp[0];
31 x_corrected[1] = calstruct->skew_yx * x_temp[0] +
32 calstruct->scale_factor_y * x_temp[1];
33 x_corrected[2] = calstruct->skew_zx * x_temp[0] +
34 calstruct->skew_zy * x_temp[1] +
35 calstruct->scale_factor_z * x_temp[2];
36 }
37