Akira Heya / Mbed 2 deprecated MPU9250_I2C_test

test

Dependencies:   mbed

MPU9250 I2C通信 注意：プルアップ抵抗を入れるのを忘れないように

main.cpp@3:e7be5e23013d, 2022-07-25 (annotated)

Committer:
AkiraHeya
Date:
Mon Jul 25 05:54:41 2022 +0000
Revision:
3:e7be5e23013d
Parent:
1:71c319f03fda 
IMU-MPU9250-TestCode

Who changed what in which revision?

UserRevisionLine numberNew contents of line
AkiraHeya 3:e7be5e23013d 1 //------------------------------------------------------------------------------
AkiraHeya 3:e7be5e23013d 2 // Attitude measurement using IMU(MPU-9250)
AkiraHeya 3:e7be5e23013d 3 //------------------------------------------------------------------------------
AkiraHeya 3:e7be5e23013d 4 //----include
onehorse 0:2e5e65a6fb30 5 #include "mbed.h"
onehorse 0:2e5e65a6fb30 6 #include "MPU9250.h"
AkiraHeya 3:e7be5e23013d 7 //#include "EKF.h"
AkiraHeya 3:e7be5e23013d 8 //----variable
onehorse 0:2e5e65a6fb30 9 char buffer[14];
AkiraHeya 3:e7be5e23013d 10 //----Instance
AkiraHeya 3:e7be5e23013d 11 MPU9250 mpu9250;
AkiraHeya 3:e7be5e23013d 12 Timer t;
AkiraHeya 3:e7be5e23013d 13 Serial pc(USBTX, USBRX);
AkiraHeya 3:e7be5e23013d 14 //****MAIN****
onehorse 0:2e5e65a6fb30 15 int main()
onehorse 0:2e5e65a6fb30 16 {
AkiraHeya 3:e7be5e23013d 17 //----Serial baud rate
AkiraHeya 3:e7be5e23013d 18 pc.baud(921600);
AkiraHeya 3:e7be5e23013d 19 //----I2C clock rate
AkiraHeya 3:e7be5e23013d 20 i2c.frequency(400000);
AkiraHeya 3:e7be5e23013d 21 //----System clock
AkiraHeya 3:e7be5e23013d 22 //pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
AkiraHeya 3:e7be5e23013d 23 //----Timer start
AkiraHeya 3:e7be5e23013d 24 t.start();
onehorse 0:2e5e65a6fb30 25 // Read the WHO_AM_I register, this is a good test of communication
AkiraHeya 3:e7be5e23013d 26 uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);
AkiraHeya 3:e7be5e23013d 27 pc.printf("I2C check : 0x%x\n\r", whoami);
AkiraHeya 3:e7be5e23013d 28 //----Self check
AkiraHeya 3:e7be5e23013d 29 if (whoami == 0x71)
AkiraHeya 3:e7be5e23013d 30 {
AkiraHeya 3:e7be5e23013d 31 pc.printf("MPU9250 is online\n\r");
AkiraHeya 3:e7be5e23013d 32 sprintf(buffer, "0x%x", whoami);
AkiraHeya 3:e7be5e23013d 33 wait(3);
AkiraHeya 3:e7be5e23013d 34 //----Reset registers to default in preparation for device calibration
AkiraHeya 3:e7be5e23013d 35 mpu9250.resetMPU9250();
AkiraHeya 3:e7be5e23013d 36 //----Start by performing self test and reporting values
AkiraHeya 3:e7be5e23013d 37 mpu9250.MPU9250SelfTest(SelfTest);
AkiraHeya 3:e7be5e23013d 38 pc.printf("x-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[0]);
AkiraHeya 3:e7be5e23013d 39 pc.printf("y-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[1]);
AkiraHeya 3:e7be5e23013d 40 pc.printf("z-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[2]);
AkiraHeya 3:e7be5e23013d 41 pc.printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]);
AkiraHeya 3:e7be5e23013d 42 pc.printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]);
AkiraHeya 3:e7be5e23013d 43 pc.printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]);
AkiraHeya 3:e7be5e23013d 44 // Calibrate gyro and accelerometers, load biases in bias registers
AkiraHeya 3:e7be5e23013d 45 mpu9250.calibrateMPU9250(gyroBias, accelBias);
AkiraHeya 3:e7be5e23013d 46 pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
AkiraHeya 3:e7be5e23013d 47 pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
AkiraHeya 3:e7be5e23013d 48 pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
AkiraHeya 3:e7be5e23013d 49 pc.printf("x accel bias = %f\n\r", accelBias[0]);
AkiraHeya 3:e7be5e23013d 50 pc.printf("y accel bias = %f\n\r", accelBias[1]);
AkiraHeya 3:e7be5e23013d 51 pc.printf("z accel bias = %f\n\r", accelBias[2]);
AkiraHeya 3:e7be5e23013d 52 wait(3);
AkiraHeya 3:e7be5e23013d 53 //----Initialize device for active mode read of acclerometer, gyroscope, and temperature
AkiraHeya 3:e7be5e23013d 54 mpu9250.initMPU9250();
AkiraHeya 3:e7be5e23013d 55 pc.printf("MPU9250 initialized for active data mode....\n\r");
AkiraHeya 3:e7be5e23013d 56 pc.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale));
AkiraHeya 3:e7be5e23013d 57 pc.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale));
AkiraHeya 3:e7be5e23013d 58 wait(3);
onehorse 0:2e5e65a6fb30 59 }
AkiraHeya 3:e7be5e23013d 60 else
AkiraHeya 3:e7be5e23013d 61 {
AkiraHeya 3:e7be5e23013d 62 pc.printf("Could not connect to MPU9250: \n\r");
AkiraHeya 3:e7be5e23013d 63 pc.printf("%#x \n", whoami);
AkiraHeya 3:e7be5e23013d 64 sprintf(buffer, "WHO_AM_I 0x%x", whoami);
AkiraHeya 3:e7be5e23013d 65 //----Loop forever if communication doesn't happen
AkiraHeya 3:e7be5e23013d 66 while(1) ;
AkiraHeya 3:e7be5e23013d 67 }
AkiraHeya 3:e7be5e23013d 68 //----Get accelerometer sensitivity
AkiraHeya 3:e7be5e23013d 69 mpu9250.getAres();
AkiraHeya 3:e7be5e23013d 70 //----Get gyro sensitivity
AkiraHeya 3:e7be5e23013d 71 mpu9250.getGres();
onehorse 0:2e5e65a6fb30 72 pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
onehorse 0:2e5e65a6fb30 73 pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
onehorse 0:2e5e65a6fb30 74
AkiraHeya 3:e7be5e23013d 75 //****LOOP****
AkiraHeya 3:e7be5e23013d 76 while(1)
AkiraHeya 3:e7be5e23013d 77 {
AkiraHeya 3:e7be5e23013d 78 //----Time interval
AkiraHeya 3:e7be5e23013d 79 Now = t.read_us();
AkiraHeya 3:e7be5e23013d 80 delt_t = (Now - lastUpdate)/1000000.0f;
AkiraHeya 3:e7be5e23013d 81 lastUpdate = Now;
AkiraHeya 3:e7be5e23013d 82 // Read the x/y/z adc values
AkiraHeya 3:e7be5e23013d 83 mpu9250.readAccelData(accelCount);
AkiraHeya 3:e7be5e23013d 84 // Calculate the accleration value into actual g's
AkiraHeya 3:e7be5e23013d 85 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
AkiraHeya 3:e7be5e23013d 86 ay = (float)accelCount[1]*aRes - accelBias[1];
AkiraHeya 3:e7be5e23013d 87 az = (float)accelCount[2]*aRes - accelBias[2];
AkiraHeya 3:e7be5e23013d 88 th_ax = atan2(ay,sqrt(ax*ax+az*az))*(180.0f/PI);
AkiraHeya 3:e7be5e23013d 89 th_ay = -1*atan2(ax,az)*(180.0f/PI);
AkiraHeya 3:e7be5e23013d 90 // th_az = atan2(az,sqrt(ax*ax+ay*ay))*(180.0f/PI);
AkiraHeya 3:e7be5e23013d 91
AkiraHeya 3:e7be5e23013d 92 /*
AkiraHeya 3:e7be5e23013d 93 th_ax_LPF = 0.95*pre_th_ax + 0.05*th_ax;
AkiraHeya 3:e7be5e23013d 94 th_ay_LPF = 0.95*pre_th_ay + 0.05*th_ay;
AkiraHeya 3:e7be5e23013d 95 // th_az_LPF = 0.95*pre_th_az + 0.05*th_az;
AkiraHeya 3:e7be5e23013d 96 pre_th_ax = th_ax;
AkiraHeya 3:e7be5e23013d 97 pre_th_ay = th_ay;
AkiraHeya 3:e7be5e23013d 98 // pre_th_az = th_az;
AkiraHeya 3:e7be5e23013d 99 */
onehorse 0:2e5e65a6fb30 100
AkiraHeya 3:e7be5e23013d 101 // Read the x/y/z adc values
AkiraHeya 3:e7be5e23013d 102 mpu9250.readGyroData(gyroCount);
AkiraHeya 3:e7be5e23013d 103 // Calculate the gyro value into actual degrees per second
AkiraHeya 3:e7be5e23013d 104 gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
AkiraHeya 3:e7be5e23013d 105 gy = (float)gyroCount[1]*gRes - gyroBias[1];
AkiraHeya 3:e7be5e23013d 106 gz = (float)gyroCount[2]*gRes - gyroBias[2];
AkiraHeya 3:e7be5e23013d 107
AkiraHeya 3:e7be5e23013d 108 th_gx += (pre_gx + gx) * delt_t/2.0f;
AkiraHeya 3:e7be5e23013d 109 th_gy += (pre_gy + gy) * delt_t/2.0f;
AkiraHeya 3:e7be5e23013d 110 th_gz += (pre_gz + gz) * delt_t/2.0f;
AkiraHeya 3:e7be5e23013d 111 pre_gx = gx;
AkiraHeya 3:e7be5e23013d 112 pre_gy = gy;
AkiraHeya 3:e7be5e23013d 113 pre_gz = gz;
AkiraHeya 3:e7be5e23013d 114
AkiraHeya 3:e7be5e23013d 115 //----Complementary filter
AkiraHeya 3:e7be5e23013d 116 th_x = 0.95*(th_x + (pre_gx + gx) * delt_t/2.0f) + 0.05*th_ax;
AkiraHeya 3:e7be5e23013d 117 th_y = 0.95*(th_y + (pre_gy + gy) * delt_t/2.0f) + 0.05*th_ay;
AkiraHeya 3:e7be5e23013d 118
AkiraHeya 3:e7be5e23013d 119 //----Serial print
AkiraHeya 3:e7be5e23013d 120 sum_dt += delt_t;
AkiraHeya 3:e7be5e23013d 121 if (sum_dt > 0.0050f)
AkiraHeya 3:e7be5e23013d 122 {
AkiraHeya 3:e7be5e23013d 123 //--Angle from gyroscope and accel sensor [deg.]
AkiraHeya 3:e7be5e23013d 124 pc.printf("%8.3f , %8.3f , %8.3f\n", t.read(), th_gx, th_gy);
AkiraHeya 3:e7be5e23013d 125 //pc.printf("%8.3f , %8.3f , %8.3f ,%8.3f , %8.3f , %8.3f , %8.3f , %8.3f , %8.3f\n", t.read(), th_ax, th_ay, th_gx, th_gy, th_x, th_y, th_x_d, th_y_d);
AkiraHeya 3:e7be5e23013d 126 sum_dt = 0.0f;
AkiraHeya 3:e7be5e23013d 127 }
onehorse 0:2e5e65a6fb30 128 }
onehorse 0:2e5e65a6fb30 129 }