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 <algos/mag_cal.h> 18 #include <algos/mat.h> 19 20 #include <errno.h> 21 #include <nanohub_math.h> 22 #include <string.h> 23 24 #define MAX_EIGEN_RATIO 25.0f 25 #define MAX_EIGEN_MAG 80.0f // uT 26 #define MIN_EIGEN_MAG 10.0f // uT 27 28 #define MAX_FIT_MAG 80.0f 29 #define MIN_FIT_MAG 10.0f 30 31 #define MIN_BATCH_WINDOW 1000000UL // 1 sec 32 #define MAX_BATCH_WINDOW 15000000UL // 15 sec 33 #define MIN_BATCH_SIZE 25 // samples 34 35 // eigen value magnitude and ratio test 36 static int moc_eigen_test(struct MagCal *moc) 37 { 38 // covariance matrix 39 struct Mat33 S; 40 S.elem[0][0] = moc->acc_xx - moc->acc_x * moc->acc_x; 41 S.elem[0][1] = S.elem[1][0] = moc->acc_xy - moc->acc_x * moc->acc_y; 42 S.elem[0][2] = S.elem[2][0] = moc->acc_xz - moc->acc_x * moc->acc_z; 43 S.elem[1][1] = moc->acc_yy - moc->acc_y * moc->acc_y; 44 S.elem[1][2] = S.elem[2][1] = moc->acc_yz - moc->acc_y * moc->acc_z; 45 S.elem[2][2] = moc->acc_zz - moc->acc_z * moc->acc_z; 46 47 struct Vec3 eigenvals; 48 struct Mat33 eigenvecs; 49 mat33GetEigenbasis(&S, &eigenvals, &eigenvecs); 50 51 float evmax = (eigenvals.x > eigenvals.y) ? eigenvals.x : eigenvals.y; 52 evmax = (eigenvals.z > evmax) ? eigenvals.z : evmax; 53 54 float evmin = (eigenvals.x < eigenvals.y) ? eigenvals.x : eigenvals.y; 55 evmin = (eigenvals.z < evmin) ? eigenvals.z : evmin; 56 57 float evmag = sqrtf(eigenvals.x + eigenvals.y + eigenvals.z); 58 59 int eigen_pass = (evmin * MAX_EIGEN_RATIO > evmax) 60 && (evmag > MIN_EIGEN_MAG) 61 && (evmag < MAX_EIGEN_MAG); 62 63 return eigen_pass; 64 } 65 66 //Kasa sphere fitting with normal equation 67 static int moc_fit(struct MagCal *moc, struct Vec3 *bias, float *radius) 68 { 69 // A * out = b 70 // (4 x 4) (4 x 1) (4 x 1) 71 struct Mat44 A; 72 A.elem[0][0] = moc->acc_xx; A.elem[0][1] = moc->acc_xy; 73 A.elem[0][2] = moc->acc_xz; A.elem[0][3] = moc->acc_x; 74 A.elem[1][0] = moc->acc_xy; A.elem[1][1] = moc->acc_yy; 75 A.elem[1][2] = moc->acc_yz; A.elem[1][3] = moc->acc_y; 76 A.elem[2][0] = moc->acc_xz; A.elem[2][1] = moc->acc_yz; 77 A.elem[2][2] = moc->acc_zz; A.elem[2][3] = moc->acc_z; 78 A.elem[3][0] = moc->acc_x; A.elem[3][1] = moc->acc_y; 79 A.elem[3][2] = moc->acc_z; A.elem[3][3] = 1.0f; 80 81 struct Vec4 b; 82 initVec4(&b, -moc->acc_xw, -moc->acc_yw, -moc->acc_zw, -moc->acc_w); 83 84 struct Size4 pivot; 85 mat44DecomposeLup(&A, &pivot); 86 87 struct Vec4 out; 88 mat44Solve(&A, &out, &b, &pivot); 89 90 // sphere: (x - xc)^2 + (y - yc)^2 + (z - zc)^2 = r^2 91 // 92 // xc = -out[0] / 2, yc = -out[1] / 2, zc = -out[2] / 2 93 // r = sqrt(xc^2 + yc^2 + zc^2 - out[3]) 94 95 struct Vec3 v; 96 initVec3(&v, out.x, out.y, out.z); 97 vec3ScalarMul(&v, -0.5f); 98 99 float r = sqrtf(vec3Dot(&v, &v) - out.w); 100 101 initVec3(bias, v.x, v.y, v.z); 102 *radius = r; 103 104 int success = 0; 105 if (r > MIN_FIT_MAG && r < MAX_FIT_MAG) { 106 success = 1; 107 } 108 109 return success; 110 } 111 112 static void moc_reset(struct MagCal *moc) 113 { 114 moc->acc_x = moc->acc_y = moc->acc_z = moc->acc_w = 0.0f; 115 moc->acc_xx = moc->acc_xy = moc->acc_xz = moc->acc_xw = 0.0f; 116 moc->acc_yy = moc->acc_yz = moc->acc_yw = 0.0f; 117 moc->acc_zz = moc->acc_zw = 0.0f; 118 119 moc->nsamples = 0; 120 moc->start_time = 0; 121 } 122 123 static int moc_batch_complete(struct MagCal *moc, uint64_t sample_time_us) 124 { 125 int complete = 0; 126 127 if ((sample_time_us - moc->start_time > MIN_BATCH_WINDOW) 128 && (moc->nsamples > MIN_BATCH_SIZE)) { 129 130 complete = 1; 131 132 } else if (sample_time_us - moc->start_time > MAX_BATCH_WINDOW) { 133 // not enough samples collected in MAX_BATCH_WINDOW 134 moc_reset(moc); 135 } 136 137 return complete; 138 } 139 140 void initMagCal(struct MagCal *moc, 141 float x_bias, float y_bias, float z_bias, 142 float c00, float c01, float c02, 143 float c10, float c11, float c12, 144 float c20, float c21, float c22) 145 { 146 moc_reset(moc); 147 moc->update_time = 0; 148 moc->radius = 0.0f; 149 150 moc->x_bias = x_bias; 151 moc->y_bias = y_bias; 152 moc->z_bias = z_bias; 153 154 moc->c00 = c00; moc->c01 = c01; moc->c02 = c02; 155 moc->c10 = c10; moc->c11 = c11; moc->c12 = c12; 156 moc->c20 = c20; moc->c21 = c21; moc->c22 = c22; 157 } 158 159 void destroy_mag_cal(struct MagCal *moc) 160 { 161 (void)moc; 162 } 163 164 bool magCalUpdate(struct MagCal *moc, uint64_t sample_time_us, 165 float x, float y, float z) 166 { 167 bool new_bias = false; 168 169 // 1. run accumulators 170 float w = x * x + y * y + z * z; 171 172 moc->acc_x += x; 173 moc->acc_y += y; 174 moc->acc_z += z; 175 moc->acc_w += w; 176 177 moc->acc_xx += x * x; 178 moc->acc_xy += x * y; 179 moc->acc_xz += x * z; 180 moc->acc_xw += x * w; 181 182 moc->acc_yy += y * y; 183 moc->acc_yz += y * z; 184 moc->acc_yw += y * w; 185 186 moc->acc_zz += z * z; 187 moc->acc_zw += z * w; 188 189 if (++moc->nsamples == 1) { 190 moc->start_time = sample_time_us; 191 } 192 193 // 2. batch has enough samples? 194 if (moc_batch_complete(moc, sample_time_us)) { 195 196 float inv = 1.0f / moc->nsamples; 197 198 moc->acc_x *= inv; 199 moc->acc_y *= inv; 200 moc->acc_z *= inv; 201 moc->acc_w *= inv; 202 203 moc->acc_xx *= inv; 204 moc->acc_xy *= inv; 205 moc->acc_xz *= inv; 206 moc->acc_xw *= inv; 207 208 moc->acc_yy *= inv; 209 moc->acc_yz *= inv; 210 moc->acc_yw *= inv; 211 212 moc->acc_zz *= inv; 213 moc->acc_zw *= inv; 214 215 // 3. eigen test 216 if (moc_eigen_test(moc)) { 217 218 struct Vec3 bias; 219 float radius; 220 221 // 4. Kasa sphere fitting 222 if (moc_fit(moc, &bias, &radius)) { 223 224 moc->x_bias = bias.x; 225 moc->y_bias = bias.y; 226 moc->z_bias = bias.z; 227 228 moc->radius = radius; 229 moc->update_time = sample_time_us; 230 231 new_bias = true; 232 } 233 } 234 235 // 5. reset for next batch 236 moc_reset(moc); 237 } 238 239 return new_bias; 240 } 241 242 void magCalGetBias(struct MagCal *moc, float *x, float *y, float *z) 243 { 244 *x = moc->x_bias; 245 *y = moc->y_bias; 246 *z = moc->z_bias; 247 } 248 249 void magCalAddBias(struct MagCal *moc, float x, float y, float z) 250 { 251 moc->x_bias += x; 252 moc->y_bias += y; 253 moc->z_bias += z; 254 } 255 256 void magCalRemoveBias(struct MagCal *moc, float xi, float yi, float zi, 257 float *xo, float *yo, float *zo) 258 { 259 *xo = xi - moc->x_bias; 260 *yo = yi - moc->y_bias; 261 *zo = zi - moc->z_bias; 262 } 263 264 void magCalSetSoftiron(struct MagCal *moc, 265 float c00, float c01, float c02, 266 float c10, float c11, float c12, 267 float c20, float c21, float c22) 268 { 269 moc->c00 = c00; moc->c01 = c01; moc->c02 = c02; 270 moc->c10 = c10; moc->c11 = c11; moc->c12 = c12; 271 moc->c20 = c20; moc->c21 = c21; moc->c22 = c22; 272 } 273 274 void magCalRemoveSoftiron(struct MagCal *moc, float xi, float yi, float zi, 275 float *xo, float *yo, float *zo) 276 { 277 *xo = moc->c00 * xi + moc->c01 * yi + moc->c02 * zi; 278 *yo = moc->c10 * xi + moc->c11 * yi + moc->c12 * zi; 279 *zo = moc->c20 * xi + moc->c21 * yi + moc->c22 * zi; 280 } 281 282
