imu for l432kc

Dependencies:   mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers BTL432KC.cpp Source File

BTL432KC.cpp

00001 /* MPU9250 Basic Example Code
00002  by: Kris Winer
00003  date: April 1, 2014
00004  license: Beerware - Use this code however you'd like. If you
00005  find it useful you can buy me a beer some time.
00006 
00007  Demonstrate basic MPU-9250 functionality including parameterizing the register addresses, initializing the sensor,
00008  getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to
00009  allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and
00010  Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1.
00011 
00012  SDA and SCL should have external pull-up resistors (to 3.3V).
00013  10k resistors are on the EMSENSR-9250 breakout board.
00014 
00015  Hardware setup:
00016  MPU9250 Breakout --------- Arduino
00017  VDD ---------------------- 3.3V
00018  VDDI --------------------- 3.3V
00019  SDA ----------------------- A4
00020  SCL ----------------------- A5
00021  GND ---------------------- GND
00022 
00023  Note: The MPU9250 is an I2C sensor and uses the Arduino Wire library.
00024  Because the sensor is not 5V tolerant, we are using a 3.3 V 8 MHz Pro Mini or a 3.3 V Teensy 3.1.
00025  We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file.
00026  We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ  to 400000L /twi.h utility file.
00027  */
00028 
00029 //#include "ST_F401_84MHZ.h"
00030 //F401_init84 myinit(0);
00031 #include "mbed.h"
00032 #include "MPU9250.h"
00033 
00034 // Using NOKIA 5110 monochrome 84 x 48 pixel display
00035 // pin 9 - Serial clock out (SCLK)
00036 // pin 8 - Serial data out (DIN)
00037 // pin 7 - Data/Command select (D/C)
00038 // pin 5 - LCD chip select (CS)
00039 // pin 6 - LCD reset (RST)
00040 //Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6);
00041 
00042 float sum = 0;
00043 uint32_t sumCount = 0;
00044 char buffer[14];
00045 
00046 MPU9250 mpu9250;
00047 
00048 Timer t;
00049 
00050 Serial serial(PA_9,PA_10);
00051 Serial pc(USBTX, USBRX); // tx, rx
00052 
00053 //        VCC,   SCE,  RST,  D/C,  MOSI,S CLK, LED
00054 
00055 //DigitalIn boardbtn(USER_BUTTON);
00056 static float acc_xyz[3]={0,0,0};
00057 static float v_xyz[3]={0,0,0};
00058  float displacement_body[3]={0,0,0};
00059 static float displacement_world[3]={0,0,0};
00060 void AccelToVelocity(float ax,float ay,float az,float dt){
00061     static float old_acc[3]={0,0,0};
00062     static float old_v[3]={0,0,0};
00063     float delta_ax=ax-old_acc[0];
00064     float delta_ay=ay-old_acc[1];
00065     float delta_az=az-old_acc[2];
00066     old_acc[0]=ax;
00067     old_acc[1]=ay;
00068     old_acc[2]=az;
00069     old_v[0]=v_xyz[0]-old_v[0];
00070     old_v[1]=v_xyz[1]-old_v[1];
00071     old_v[2]=v_xyz[2]-old_v[2];
00072     v_xyz[0]+=ax*dt;
00073     v_xyz[1]+=ay*dt;
00074     v_xyz[2]+=az*dt;
00075     
00076     
00077     displacement_body[0]+=v_xyz[0]*dt;
00078     displacement_body[1]+=v_xyz[1]*dt;
00079     displacement_body[2]+=v_xyz[2]*dt;
00080   //  pc.printf("%f\n",dt);
00081     /*displacement_body[0]+=fillter_vx*dt;
00082     displacement_body[1]+=fillter_vy*dt;
00083     displacement_body[2]+=fillter_vz*dt;*/
00084     
00085     }
00086 /*void VelocityToDisplacement(float dt){
00087     displacement_body[0]+=v_xyz[0]*dt;
00088     displacement_body[1]+=v_xyz[1]*dt;
00089     displacement_body[2]+=v_xyz[2]*dt;
00090     
00091     }*/
00092 int main()
00093 {   
00094     pc.baud(115200);
00095 
00096     //Set up I2C
00097     i2c.frequency(400000);  // use fast (400 kHz) I2C
00098 
00099 //    pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
00100 
00101     t.start();
00102 
00103 
00104 
00105 
00106     // Read the WHO_AM_I register, this is a good test of communication
00107     uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
00108 //    pc.printf("I AM 0x%x\n\r", whoami);
00109 //    pc.printf("I SHOULD BE 0x71\n\r");
00110 
00111     if (whoami == 0x71) { // WHO_AM_I should always be 0x68
00112 //        pc.printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami);
00113 //        pc.printf("MPU9250 is online...\n\r");
00114 
00115 //        sprintf(buffer, "0x%x", whoami);
00116 
00117         wait(1);
00118 
00119         mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
00120         mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
00121 /*        pc.printf("x-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[0]);
00122         pc.printf("y-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[1]);
00123         pc.printf("z-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[2]);
00124         pc.printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]);
00125         pc.printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]);
00126         pc.printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]);*/
00127 //     if(boardbtn==0){
00128         // wait(2);
00129         mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
00130         pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
00131         pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
00132         pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
00133         pc.printf("x accel bias = %f\n\r", accelBias[0]);
00134         pc.printf("y accel bias = %f\n\r", accelBias[1]);
00135         pc.printf("z accel bias = %f\n\r", accelBias[2]);
00136         wait(2);
00137         
00138 /*
00139 // -----------------------------------------------------------------
00140         }else{
00141         gyroBias[0]=-1.244275;
00142         gyroBias[1]=0.122137;
00143         gyroBias[2]=-0.717557;
00144         accelBias[0]=0.007996;                                      // For Manual Calibate with button
00145         accelBias[1]=0.021240;
00146         accelBias[2]=-0.020508;
00147         }
00148 //------------------------------------------------------------------
00149 */
00150         mpu9250.initMPU9250();
00151 //        pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
00152         mpu9250.initAK8963(magCalibration);
00153 /*        pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
00154         pc.printf("Accelerometer full-scale range = %f  g\n\r", 2.0f*(float)(1<<Ascale));
00155         pc.printf("Gyroscope full-scale range = %f  deg/s\n\r", 250.0f*(float)(1<<Gscale));
00156         if(Mscale == 0) pc.printf("Magnetometer resolution = 14  bits\n\r");
00157         if(Mscale == 1) pc.printf("Magnetometer resolution = 16  bits\n\r");
00158         if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
00159         if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
00160         wait(1);
00161 */
00162     } else {
00163         pc.printf("Could not connect to MPU9250: \n\r");
00164         pc.printf("%#x \n",  whoami);
00165 
00166 
00167         sprintf(buffer, "WHO_AM_I 0x%x", whoami);
00168 
00169 
00170         while(1) ; // Loop forever if communication doesn't happen
00171 
00172     }
00173 
00174     mpu9250.getAres(); // Get accelerometer sensitivity
00175     mpu9250.getGres(); // Get gyro sensitivity
00176     mpu9250.getMres(); // Get magnetometer sensitivity
00177 //    pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
00178 //    pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
00179 //    pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
00180     magbias[0] = +470.;  // User environmental x-axis correction in milliGauss, should be automatically calculated
00181     magbias[1] = +120.;  // User environmental x-axis correction in milliGauss
00182     magbias[2] = +125.;  // User environmental x-axis correction in milliGauss
00183 
00184     while(1) {
00185 
00186         // If intPin goes high, all data registers have new data
00187         if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {  // On interrupt, check if data ready interrupt
00188 
00189             mpu9250.readAccelData(accelCount);  // Read the x/y/z adc values
00190             // Now we'll calculate the accleration value into actual g's
00191             ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
00192             ay = (float)accelCount[1]*aRes - accelBias[1];
00193             az = (float)accelCount[2]*aRes - accelBias[2];
00194 
00195             mpu9250.readGyroData(gyroCount);  // Read the x/y/z adc values
00196             // Calculate the gyro value into actual degrees per second
00197             gx = (float)gyroCount[0]*gRes - gyroBias[0];  // get actual gyro value, this depends on scale being set
00198             gy = (float)gyroCount[1]*gRes - gyroBias[1];
00199             gz = (float)gyroCount[2]*gRes - gyroBias[2];
00200 
00201             mpu9250.readMagData(magCount);  // Read the x/y/z adc values
00202             // Calculate the magnetometer values in milliGauss
00203             // Include factory calibration per data sheet and user environmental corrections
00204             mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0];  // get actual magnetometer value, this depends on scale being set
00205             my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
00206             mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
00207         }
00208 
00209         Now = t.read_us();
00210         deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
00211         lastUpdate = Now;
00212 
00213         sum += deltat;
00214         sumCount++;
00215 
00216 //    if(lastUpdate - firstUpdate > 10000000.0f) {
00217 //     beta = 0.04;  // decrease filter gain after stabilized
00218 //     zeta = 0.015; // increasey bias drift gain after stabilized
00219 //   }
00220 
00221         // Pass gyro rate as rad/s
00222 // mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
00223         mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
00224 
00225         // Serial print and/or display at 0.5 s rate independent of data rates
00226         delt_t = t.read_ms() - count;
00227         if (delt_t > 100) { // update LCD once per half-second independent of read rate
00228 
00229     /*pc.printf("ax = %f", 1000*ax);
00230     pc.printf(" ay = %f", 1000*ay);
00231     pc.printf(" az = %f  mg\n\r", 1000*az);
00232 */
00233    pc.printf("gx = %f", gx);
00234    pc.printf(" gy = %f", gy);
00235    pc.printf(" gz = %f  deg/s\n\r", gz);
00236 
00237   // pc.printf("gx = %f", mx);
00238   // pc.printf(" gy = %f", my);
00239  // pc.printf(" gz = %f  mG\n\r", mz);
00240 
00241             tempCount = mpu9250.readTempData();  // Read the adc values
00242             temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
00243 //   pc.printf(" temperature = %f  C\n\r", temperature);
00244 //
00245 //    pc.printf("q0 = %f\n\r", q[0]);
00246 //    pc.printf("q1 = %f\n\r", q[1]);
00247 //    pc.printf("q2 = %f\n\r", q[2]);
00248 //    pc.printf("q3 = %f\n\r", q[3]);
00249 
00250             /*    lcd.clear();
00251                 lcd.printString("MPU9250", 0, 0);
00252                 lcd.printString("x   y   z", 0, 1);
00253                 sprintf(buffer, "%d %d %d mg", (int)(1000.0f*ax), (int)(1000.0f*ay), (int)(1000.0f*az));
00254                 lcd.printString(buffer, 0, 2);
00255                 sprintf(buffer, "%d %d %d deg/s", (int)gx, (int)gy, (int)gz);
00256                 lcd.printString(buffer, 0, 3);
00257                 sprintf(buffer, "%d %d %d mG", (int)mx, (int)my, (int)mz);
00258                 lcd.printString(buffer, 0, 4);
00259              */
00260             // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
00261             // In this coordinate system, the positive z-axis is down toward Earth.
00262             // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
00263             // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
00264             // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
00265             // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
00266             // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
00267             // applied in the correct order which for this configuration is yaw, pitch, and then roll.
00268             // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
00269             yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
00270             pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
00271             roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
00272             pitch *= 180.0f / PI;
00273             yaw   *= 180.0f / PI;
00274             yaw   -= 85.0f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
00275             roll  *= 180.0f / PI;
00276 
00277   //          pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
00278  //   pc.printf("average rate = %f\n\r", (float) sumCount/sum);
00279 //    sprintf(buffer, "YPR: %f %f %f", yaw, pitch, roll);
00280 //    lcd.printString(buffer, 0, 4);
00281 //    sprintf(buffer, "rate = %f", (float) sumCount/sum);
00282 //    lcd.printString(buffer, 0, 5);
00283 
00284             
00285     acc_xyz[0]=ax;
00286         acc_xyz[1]=ay;
00287         acc_xyz[2]=az;
00288         float mag_a=sqrt(pow(acc_xyz[0],2)+pow(acc_xyz[1],2)+pow(acc_xyz[2],2));
00289         AccelToVelocity(mag_a*cos(yaw),mag_a*sin(yaw),mag_a*sin(roll),float(delt_t)/10);//deltat*1000000
00290         static float fillter_vx=0;
00291     static float fillter_vy=0;
00292     static float fillter_vz=0;
00293     fillter_vx=v_xyz[0]*0.5+fillter_vx*0.5;
00294     fillter_vy=v_xyz[1]*0.5+fillter_vy*0.5;
00295     fillter_vz=v_xyz[2]*0.5+fillter_vz*0.5;
00296         //VelocityToDisplacement(float(delt_t));
00297         //pc.printf("%f\n",sqrt(displacement_body[0]*displacement_body[0]));
00298         //pc.printf("%f %f\n",displacement_body[0],displacement_body[1]);
00299        // displacement_world[0]+=sqrt(pow(displacement_body[0],2)+pow(displacement_body[1],2)+pow(displacement_body[2],2))
00300        // pc.printf("%f %f \n",v_xyz[0],v_xyz[1]);
00301        // pc.printf("%f %f %f\n",fillter_vx,fillter_vy,fillter_vz);
00302             myled= !myled;
00303             count = t.read_ms();
00304             if(count > 1<<21) {
00305                 t.start(); // start the timer over again if ~30 minutes has passed
00306                 count = 0;
00307                 deltat= 0;
00308                 lastUpdate = t.read_us();
00309             }
00310             sum = 0;
00311             sumCount = 0;
00312 //-----------------------------------------------------------------------------------------        
00313         
00314         if(serial.writeable()) {
00315             serial.printf("%f %f %f %f %f %f %f %f %f \n",ax,ay,az,gx,gy,gz,roll,pitch,yaw);
00316             //pc.printf("%f_%f_%f_g%f_g%f_g%f_ \n",ax,ay,az,gx,gy,gz);
00317             
00318             pc.printf("%f_%f_%f \n",roll,pitch,yaw);
00319             
00320             
00321             }
00322         }
00323         
00324         
00325     }
00326     
00327 }
00328