Lines Matching refs:moc
36 static int moc_eigen_test(struct MagCal *moc) in moc_eigen_test() argument
40 S.elem[0][0] = moc->acc_xx - moc->acc_x * moc->acc_x; in moc_eigen_test()
41 S.elem[0][1] = S.elem[1][0] = moc->acc_xy - moc->acc_x * moc->acc_y; in moc_eigen_test()
42 S.elem[0][2] = S.elem[2][0] = moc->acc_xz - moc->acc_x * moc->acc_z; in moc_eigen_test()
43 S.elem[1][1] = moc->acc_yy - moc->acc_y * moc->acc_y; in moc_eigen_test()
44 S.elem[1][2] = S.elem[2][1] = moc->acc_yz - moc->acc_y * moc->acc_z; in moc_eigen_test()
45 S.elem[2][2] = moc->acc_zz - moc->acc_z * moc->acc_z; in moc_eigen_test()
67 static int moc_fit(struct MagCal *moc, struct Vec3 *bias, float *radius) in moc_fit() argument
72 A.elem[0][0] = moc->acc_xx; A.elem[0][1] = moc->acc_xy; in moc_fit()
73 A.elem[0][2] = moc->acc_xz; A.elem[0][3] = moc->acc_x; in moc_fit()
74 A.elem[1][0] = moc->acc_xy; A.elem[1][1] = moc->acc_yy; in moc_fit()
75 A.elem[1][2] = moc->acc_yz; A.elem[1][3] = moc->acc_y; in moc_fit()
76 A.elem[2][0] = moc->acc_xz; A.elem[2][1] = moc->acc_yz; in moc_fit()
77 A.elem[2][2] = moc->acc_zz; A.elem[2][3] = moc->acc_z; in moc_fit()
78 A.elem[3][0] = moc->acc_x; A.elem[3][1] = moc->acc_y; in moc_fit()
79 A.elem[3][2] = moc->acc_z; A.elem[3][3] = 1.0f; in moc_fit()
82 initVec4(&b, -moc->acc_xw, -moc->acc_yw, -moc->acc_zw, -moc->acc_w); in moc_fit()
112 static void moc_reset(struct MagCal *moc) in moc_reset() argument
114 moc->acc_x = moc->acc_y = moc->acc_z = moc->acc_w = 0.0f; in moc_reset()
115 moc->acc_xx = moc->acc_xy = moc->acc_xz = moc->acc_xw = 0.0f; in moc_reset()
116 moc->acc_yy = moc->acc_yz = moc->acc_yw = 0.0f; in moc_reset()
117 moc->acc_zz = moc->acc_zw = 0.0f; in moc_reset()
119 moc->nsamples = 0; in moc_reset()
120 moc->start_time = 0; in moc_reset()
123 static int moc_batch_complete(struct MagCal *moc, uint64_t sample_time_us) in moc_batch_complete() argument
127 if ((sample_time_us - moc->start_time > MIN_BATCH_WINDOW) in moc_batch_complete()
128 && (moc->nsamples > MIN_BATCH_SIZE)) { in moc_batch_complete()
132 } else if (sample_time_us - moc->start_time > MAX_BATCH_WINDOW) { in moc_batch_complete()
134 moc_reset(moc); in moc_batch_complete()
140 void initMagCal(struct MagCal *moc, in initMagCal() argument
146 moc_reset(moc); in initMagCal()
147 moc->update_time = 0; in initMagCal()
148 moc->radius = 0.0f; in initMagCal()
150 moc->x_bias = x_bias; in initMagCal()
151 moc->y_bias = y_bias; in initMagCal()
152 moc->z_bias = z_bias; in initMagCal()
154 moc->c00 = c00; moc->c01 = c01; moc->c02 = c02; in initMagCal()
155 moc->c10 = c10; moc->c11 = c11; moc->c12 = c12; in initMagCal()
156 moc->c20 = c20; moc->c21 = c21; moc->c22 = c22; in initMagCal()
159 void destroy_mag_cal(struct MagCal *moc) in destroy_mag_cal() argument
161 (void)moc; in destroy_mag_cal()
164 bool magCalUpdate(struct MagCal *moc, uint64_t sample_time_us, in magCalUpdate() argument
172 moc->acc_x += x; in magCalUpdate()
173 moc->acc_y += y; in magCalUpdate()
174 moc->acc_z += z; in magCalUpdate()
175 moc->acc_w += w; in magCalUpdate()
177 moc->acc_xx += x * x; in magCalUpdate()
178 moc->acc_xy += x * y; in magCalUpdate()
179 moc->acc_xz += x * z; in magCalUpdate()
180 moc->acc_xw += x * w; in magCalUpdate()
182 moc->acc_yy += y * y; in magCalUpdate()
183 moc->acc_yz += y * z; in magCalUpdate()
184 moc->acc_yw += y * w; in magCalUpdate()
186 moc->acc_zz += z * z; in magCalUpdate()
187 moc->acc_zw += z * w; in magCalUpdate()
189 if (++moc->nsamples == 1) { in magCalUpdate()
190 moc->start_time = sample_time_us; in magCalUpdate()
194 if (moc_batch_complete(moc, sample_time_us)) { in magCalUpdate()
196 float inv = 1.0f / moc->nsamples; in magCalUpdate()
198 moc->acc_x *= inv; in magCalUpdate()
199 moc->acc_y *= inv; in magCalUpdate()
200 moc->acc_z *= inv; in magCalUpdate()
201 moc->acc_w *= inv; in magCalUpdate()
203 moc->acc_xx *= inv; in magCalUpdate()
204 moc->acc_xy *= inv; in magCalUpdate()
205 moc->acc_xz *= inv; in magCalUpdate()
206 moc->acc_xw *= inv; in magCalUpdate()
208 moc->acc_yy *= inv; in magCalUpdate()
209 moc->acc_yz *= inv; in magCalUpdate()
210 moc->acc_yw *= inv; in magCalUpdate()
212 moc->acc_zz *= inv; in magCalUpdate()
213 moc->acc_zw *= inv; in magCalUpdate()
216 if (moc_eigen_test(moc)) { in magCalUpdate()
222 if (moc_fit(moc, &bias, &radius)) { in magCalUpdate()
224 moc->x_bias = bias.x; in magCalUpdate()
225 moc->y_bias = bias.y; in magCalUpdate()
226 moc->z_bias = bias.z; in magCalUpdate()
228 moc->radius = radius; in magCalUpdate()
229 moc->update_time = sample_time_us; in magCalUpdate()
236 moc_reset(moc); in magCalUpdate()
242 void magCalGetBias(struct MagCal *moc, float *x, float *y, float *z) in magCalGetBias() argument
244 *x = moc->x_bias; in magCalGetBias()
245 *y = moc->y_bias; in magCalGetBias()
246 *z = moc->z_bias; in magCalGetBias()
249 void magCalAddBias(struct MagCal *moc, float x, float y, float z) in magCalAddBias() argument
251 moc->x_bias += x; in magCalAddBias()
252 moc->y_bias += y; in magCalAddBias()
253 moc->z_bias += z; in magCalAddBias()
256 void magCalRemoveBias(struct MagCal *moc, float xi, float yi, float zi, in magCalRemoveBias() argument
259 *xo = xi - moc->x_bias; in magCalRemoveBias()
260 *yo = yi - moc->y_bias; in magCalRemoveBias()
261 *zo = zi - moc->z_bias; in magCalRemoveBias()
264 void magCalSetSoftiron(struct MagCal *moc, in magCalSetSoftiron() argument
269 moc->c00 = c00; moc->c01 = c01; moc->c02 = c02; in magCalSetSoftiron()
270 moc->c10 = c10; moc->c11 = c11; moc->c12 = c12; in magCalSetSoftiron()
271 moc->c20 = c20; moc->c21 = c21; moc->c22 = c22; in magCalSetSoftiron()
274 void magCalRemoveSoftiron(struct MagCal *moc, float xi, float yi, float zi, in magCalRemoveSoftiron() argument
277 *xo = moc->c00 * xi + moc->c01 * yi + moc->c02 * zi; in magCalRemoveSoftiron()
278 *yo = moc->c10 * xi + moc->c11 * yi + moc->c12 * zi; in magCalRemoveSoftiron()
279 *zo = moc->c20 * xi + moc->c21 * yi + moc->c22 * zi; in magCalRemoveSoftiron()