BMX055

Dependents:   9Dsensor_test LINE_TRACE_CAR

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers BMX055.cpp Source File

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 }