BMX055
Dependents: 9Dsensor_test LINE_TRACE_CAR
BMX055.cpp
00001 #include "mbed.h" 00002 #include "BMX055.h" 00003 #include <math.h> 00004 00005 const float G = 9.80665; 00006 const float PI = 3.14159; 00007 00008 static int num_of_points = 0; 00009 static float points[1024][3]; 00010 00011 00012 BMX055::BMX055(PinName SDA, PinName SCL) : 00013 bmx055(SDA, SCL), 00014 accel(), 00015 gyroscope(), 00016 magnet() 00017 { 00018 bmx055.frequency(100000); 00019 bmx_init(); 00020 } 00021 00022 void BMX055::bmx_init(void) 00023 { 00024 char buf[2] = { 0 }; 00025 printf("accel setting\n"); 00026 buf[0] = 0x14; 00027 buf[1] = 0xB6; 00028 bmx055.write(ACC, buf, 2); 00029 wait_ms(2); 00030 buf[0] = 0x0F; 00031 buf[1] = 0x05; 00032 buf[0] = 0x10; 00033 buf[1] = 0x08; 00034 bmx055.write(ACC, buf, 2); 00035 buf[0] = 0x11; 00036 buf[1] = 0x00; 00037 bmx055.write(ACC, buf, 2); 00038 wait_ms(2); 00039 00040 printf("gyroscope setting\n"); 00041 buf[0] = 0x0F; 00042 buf[1] = 0x02; 00043 bmx055.write(GYRO, buf, 2); 00044 wait(0.1); 00045 buf[0] = 0x10; 00046 buf[1] = 0x00; 00047 bmx055.write(GYRO, buf, 2); 00048 wait(0.1); 00049 buf[0] = 0x11; 00050 buf[1] = 0x00; 00051 bmx055.write(GYRO, buf, 2); 00052 wait(0.1); 00053 00054 printf("magnet setting\n"); 00055 buf[0] = 0x4B; 00056 buf[1] = 0x82; 00057 bmx055.write(MAG, buf, 2); 00058 wait(0.1); 00059 buf[0] = 0x4B; 00060 buf[1] = 0x01; 00061 bmx055.write(MAG, buf, 2); 00062 wait(0.1); 00063 buf[0] = 0x4C; 00064 buf[1] = 0x00; 00065 bmx055.write(MAG, buf, 2); 00066 buf[0] = 0x4E; 00067 buf[1] = 0x84; 00068 bmx055.write(MAG, buf, 2); 00069 buf[0] = 0x51; 00070 buf[1] = 0x04; 00071 bmx055.write(MAG, buf, 2); 00072 buf[0] = 0x52; 00073 buf[1] = 0x16; 00074 bmx055.write(MAG, buf, 2); 00075 wait(0.1); 00076 00077 buf[0] = 0x00; 00078 bmx055.write(MAG, buf, 1, 1); 00079 bmx055.read(MAG, buf, 1); 00080 printf("read:0x%02x\n", buf[0]); 00081 00082 timer.start(); 00083 before_time = -1; 00084 } 00085 00086 void BMX055::Get_acc(void) 00087 { 00088 uint8_t data[6] = { 0 }; 00089 char send[1], get[1]; 00090 char temp; 00091 00092 send[0] = (char)(2); 00093 bmx055.write(ACC, send, 1, true); 00094 bmx055.read(ACC, (char*)data, 6); 00095 00096 for (int i = 0; i < 3; i++) { 00097 accel[i] = (int16_t)(((int16_t)data[i * 2 + 1] << 8) | data[i * 2]) >> 4; 00098 if (accel[i] > 2047)accel[i] -= 4096; 00099 accel[i] = (accel[i] / 1024 * G - acc_biases[i]) * acc_gains[i]; 00100 } 00101 } 00102 00103 void BMX055::Get_gyro(void) 00104 { 00105 int data[6] = { 0 }; 00106 char send[1], get[1]; 00107 char temp; 00108 00109 for (int i = 0; i < 6; i++) { 00110 send[0] = (char)(2 + i); 00111 bmx055.write(GYRO, send, 1); 00112 bmx055.read(GYRO, get, 1); 00113 temp = get[0]; 00114 data[i] = temp; 00115 } 00116 00117 for (int i = 0; i < 3; i++) { 00118 gyroscope[i] = (int16_t)(((int16_t)data[i * 2 + 1] << 8) | data[i * 2]) >> 4; 00119 if (gyroscope[i] > 32767)gyroscope[i] -= 65536; 00120 gyroscope[i] = (gyroscope[i] * (8000.0 / 32768) * (PI / 180) - gyro_biases[i]) * gyro_gains[i]; 00121 } 00122 } 00123 00124 void BMX055::Get_mag(void) 00125 { 00126 int data[8] = { 0 }; 00127 char send[1], get[1]; 00128 char temp; 00129 00130 for (int i = 0; i < 8; i++) { 00131 send[0] = (char)(0x42 + i); 00132 bmx055.write(MAG, send, 1); 00133 bmx055.read(MAG, get, 1); 00134 temp = get[0]; 00135 data[i] = temp; 00136 } 00137 00138 for (int i = 0; i < 3; i++) { 00139 if (i != 2)magnet[i] = (int16_t)(((int16_t)data[i * 2 + 1] << 8) | data[i * 2]) >> 3; 00140 else magnet[i] = (int16_t)(((int16_t)data[i * 2 + 1] << 8) | data[i * 2]) >> 1; 00141 if (i == 2 && magnet[i] > 16383)magnet[i] -= 32768; 00142 else if (i != 2 && magnet[i] > 4095)magnet[i] -= 8192; 00143 magnet[i] -= mag_biases[i]; 00144 magnet[i] /= mag_gains[i]; 00145 } 00146 } 00147 00148 void BMX055::Update_posture() 00149 { 00150 double dt = 0; 00151 double after_time = 0; 00152 00153 after_time = timer.read(); 00154 00155 Get_gyro(); 00156 00157 dt = after_time - before_time; 00158 00159 before_time = after_time; 00160 00161 Quaternion omega(0, gyroscope[0], gyroscope[1], gyroscope[2]); 00162 00163 Quaternion dq = (posture * 0.5f) * omega; 00164 00165 posture += (dq * dt);///////////////////////////////////////////////////// 00166 00167 posture /= posture.norm();//////////////////////////////////////////////// 00168 00169 00170 00171 00172 Quaternion acc_rotation_vector; 00173 00174 Get_acc(); 00175 float acc_weight=0.0; 00176 00177 Quaternion acc_vector_observed(0, accel[0], accel[1], accel[2]); 00178 acc_weight = ( 0.41 - fabs((9.797 - acc_vector_observed.norm()) / 9.797) ) * 0.5; 00179 00180 if(acc_weight<0) { 00181 acc_weight = 0.0; 00182 } 00183 00184 acc_vector_observed /= acc_vector_observed.norm(); 00185 00186 Quaternion acc_vector_estimated; 00187 00188 acc_vector_estimated = posture.conjugate() * initial_acc * posture; 00189 00190 acc_vector_estimated /= acc_vector_estimated.norm(); 00191 00192 acc_rotation_vector.w = 0; 00193 acc_rotation_vector.x = acc_vector_observed.y * acc_vector_estimated.z - acc_vector_observed.z * acc_vector_estimated.y; 00194 acc_rotation_vector.y = acc_vector_observed.z * acc_vector_estimated.x - acc_vector_observed.x * acc_vector_estimated.z; 00195 acc_rotation_vector.z = acc_vector_observed.x * acc_vector_estimated.y - acc_vector_observed.y * acc_vector_estimated.x; 00196 00197 acc_rotation_vector *= acos((acc_vector_estimated.x * acc_vector_observed.x) + (acc_vector_estimated.y * acc_vector_observed.y) + (acc_vector_estimated.z * acc_vector_observed.z)); 00198 00199 00200 acc_rotation_vector *= acc_weight; 00201 00202 Quaternion mag_rotation_vector; 00203 00204 Get_mag(); 00205 00206 float copy=0.0f; 00207 copy = magnet[0]; 00208 magnet[0] = -magnet[1]; 00209 magnet[1] = copy; 00210 00211 Quaternion mag_vector_observed(0, magnet[0], magnet[1], magnet[2]); 00212 00213 mag_vector_observed /= mag_vector_observed.norm(); 00214 00215 Quaternion mag_vector_estimated; 00216 00217 mag_vector_estimated = posture.conjugate() * initial_mag * posture; 00218 00219 mag_vector_estimated /= mag_vector_estimated.norm(); 00220 00221 00222 00223 00224 mag_rotation_vector.w = 0; 00225 mag_rotation_vector.x = mag_vector_observed.y * mag_vector_estimated.z - mag_vector_observed.z * mag_vector_estimated.y; 00226 mag_rotation_vector.y = mag_vector_observed.z * mag_vector_estimated.x - mag_vector_observed.x * mag_vector_estimated.z; 00227 mag_rotation_vector.z = mag_vector_observed.x * mag_vector_estimated.y - mag_vector_observed.y * mag_vector_estimated.x; 00228 00229 mag_rotation_vector *= acos((mag_vector_estimated.x * mag_vector_observed.x) + (mag_vector_estimated.y * mag_vector_observed.y) + (mag_vector_estimated.z * mag_vector_observed.z)); 00230 00231 mag_rotation_vector *= 0.2f;/////////////////////////////////////////////////////////////////////////////// 00232 00233 00234 00235 00236 00237 00238 00239 00240 Quaternion rotation_vector; 00241 Quaternion rotation_quaternion; 00242 00243 rotation_vector=(acc_rotation_vector+mag_rotation_vector)/2.0; 00244 00245 rotation_quaternion.w = cos(rotation_vector.norm() / 2); 00246 rotation_quaternion.x = (rotation_vector.x / rotation_vector.norm()) * sin(rotation_vector.norm() / 2); 00247 rotation_quaternion.y = (rotation_vector.y / rotation_vector.norm()) * sin(rotation_vector.norm() / 2); 00248 rotation_quaternion.z = (rotation_vector.z / rotation_vector.norm()) * sin(rotation_vector.norm() / 2); 00249 00250 rotation_quaternion=posture*rotation_quaternion*posture.conjugate(); 00251 00252 posture=rotation_quaternion*posture; 00253 00254 posture /= posture.norm(); 00255 00256 } 00257 00258 void BMX055::mag_calibration(DigitalIn button) 00259 { 00260 00261 printf("たくさん回したらもう一度青いボタンを押せ\n"); 00262 00263 for (int i = 0; i < 1024; i++) { 00264 Get_mag(); 00265 for (int j = 0; j < 3; j++) { 00266 points[i][j] = magnet[j]; 00267 } 00268 num_of_points++; 00269 if (button == 0) { 00270 break; 00271 } 00272 wait(0.01); 00273 } 00274 00275 float Mat[6][7]; 00276 00277 for (int i = 0; i < 3; i++) { 00278 for (int j = 0; j < 3; j++) { 00279 Mat[i][j] = 0; 00280 for (int k = 0; k < num_of_points; k++) { 00281 Mat[i][j] += 2.0f * (points[k][i] * points[k][i]) * (points[k][j] * points[k][j]); 00282 } 00283 } 00284 00285 for (int j = 3; j < 6; j++) { 00286 Mat[i][j] = 0; 00287 for (int k = 0; k < num_of_points; k++) { 00288 Mat[i][j] += 2.0f * (points[k][i] * points[k][i]) * points[k][j - 3]; 00289 } 00290 } 00291 } 00292 00293 for (int i = 3; i < 6; i++) { 00294 for (int j = 0; j < 3; j++) { 00295 Mat[i][j] = 0; 00296 for (int k = 0; k < num_of_points; k++) { 00297 Mat[i][j] += 2.0f * points[k][i - 3] * (points[k][j] * points[k][j]); 00298 } 00299 } 00300 00301 for (int j = 3; j < 6; j++) { 00302 Mat[i][j] = 0; 00303 for (int k = 0; k < num_of_points; k++) { 00304 Mat[i][j] += 2.0f * points[k][i - 3] * points[k][j - 3]; 00305 } 00306 } 00307 } 00308 00309 for (int i = 0; i < 3; i++) { 00310 Mat[i][6] = 0; 00311 for (int k = 0; k < num_of_points; k++) { 00312 Mat[i][6] += 2.0f * points[k][i] * points[k][i]; 00313 } 00314 } 00315 00316 for (int i = 3; i < 6; i++) { 00317 Mat[i][6] = 0; 00318 for (int k = 0; k < num_of_points; k++) { 00319 Mat[i][6] += 2.0f * points[k][i - 3]; 00320 } 00321 } 00322 00323 00324 00325 for (int i = 0; i < 5; i++) { 00326 int max = i; 00327 00328 for (int j = i; j < 6; j++) { 00329 if (fabs(Mat[j][i]) > fabs(Mat[i][max])) { 00330 max = j; 00331 } 00332 } 00333 00334 if (max != i) { 00335 float element; 00336 for (int j = 0; j < 7; j++) { 00337 element = Mat[i][j]; 00338 Mat[i][j] = Mat[max][j]; 00339 Mat[max][j] = element; 00340 } 00341 } 00342 00343 00344 00345 float coeff = 0; 00346 00347 for (int j = i + 1; j < 6; j++) { 00348 coeff = Mat[j][i] / Mat[i][i]; 00349 for (int k = i; k < 7; k++) { 00350 Mat[j][k] -= Mat[i][k] * coeff; 00351 } 00352 } 00353 } 00354 00355 00356 for (int i = 5; i > 0; i--) { 00357 float coeff = 0; 00358 for (int j = i - 1; j > -1; j--) { 00359 coeff = Mat[j][i] / Mat[i][i]; 00360 for (int k = i; k < 7; k++) { 00361 Mat[j][k] -= Mat[i][k] * coeff; 00362 } 00363 } 00364 } 00365 00366 for (int i = 0; i < 6; i++) { 00367 Mat[i][6] /= Mat[i][i]; 00368 Mat[i][i] /= Mat[i][i]; 00369 } 00370 00371 00372 00373 00374 00375 float r = 1; 00376 for (int i = 0; i < 3; i++) { 00377 r += (Mat[i + 3][6] * Mat[i + 3][6]) / (4 * Mat[i][6]); 00378 } 00379 00380 for (int i = 0; i < 3; i++) { 00381 mag_gains[i] = sqrt(r / Mat[i][6]); 00382 } 00383 00384 for (int i = 0; i < 3; i++) { 00385 mag_biases[i] = -Mat[i + 3][6] / (2 * Mat[i][6]); 00386 } 00387 00388 for (int a = 0; a < 3; a++) { 00389 printf("%f,%f\n", mag_gains[a], mag_biases[a]); 00390 } 00391 00392 00393 } 00394 00395 void BMX055::set_initial_mag() 00396 { 00397 Get_mag(); 00398 00399 float copy=0.0f; 00400 copy = magnet[0]; 00401 magnet[0] = -magnet[1]; 00402 magnet[1] = copy; 00403 00404 initial_mag.w = 0; 00405 initial_mag.x = magnet[0]; 00406 initial_mag.y = magnet[1]; 00407 initial_mag.z = magnet[2]; 00408 00409 initial_mag /= initial_mag.norm(); 00410 } 00411 00412 void BMX055::set_initial_acc() 00413 { 00414 Get_acc(); 00415 00416 initial_acc.w = 0; 00417 initial_acc.x = accel[0]; 00418 initial_acc.y = accel[1]; 00419 initial_acc.z = accel[2]; 00420 00421 initial_acc /= initial_acc.norm(); 00422 } 00423 00424 float BMX055::get_azimuth_machineframe() 00425 { 00426 00427 float sin_azimuth = -2.0 * posture.z * posture.w; 00428 float cos_azimuth = (posture.w * posture.w) - (posture.z * posture.z); 00429 00430 if (sin_azimuth > 0) { 00431 return acos(cos_azimuth); 00432 } else { 00433 return 2.0f * PI - acos(cos_azimuth); 00434 } 00435 }
Generated on Sat Jul 16 2022 06:27:02 by
1.7.2