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. 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 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
