Veloc

Dependencies:   mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.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 pc(USBTX, USBRX); // tx, rx
00051 
00052 //        VCC,   SCE,  RST,  D/C,  MOSI,S CLK, LED
00053 
00054 DigitalIn boardbtn(USER_BUTTON);
00055 static float acc_xyz[3]={0,0,0};
00056 static float v_xyz[3]={0,0,0};
00057  float displacement_body[3]={0,0,0};
00058 static float displacement_world[3]={0,0,0};
00059 void AccelToVelocity(float ax,float ay,float az,float dt){
00060     static float old_acc[3]={0,0,0};
00061     static float old_v[3]={0,0,0};
00062     float delta_ax=ax-old_acc[0];
00063     float delta_ay=ay-old_acc[1];
00064     float delta_az=az-old_acc[2];
00065     old_acc[0]=ax;
00066     old_acc[1]=ay;
00067     old_acc[2]=az;
00068     old_v[0]=v_xyz[0]-old_v[0];
00069     old_v[1]=v_xyz[1]-old_v[1];
00070     old_v[2]=v_xyz[2]-old_v[2];
00071     v_xyz[0]+=ax*dt;
00072     v_xyz[1]+=ay*dt;
00073     v_xyz[2]+=az*dt;
00074     
00075     
00076     displacement_body[0]+=v_xyz[0]*dt;
00077     displacement_body[1]+=v_xyz[1]*dt;
00078     displacement_body[2]+=v_xyz[2]*dt;
00079   //  pc.printf("%f\n",dt);
00080     /*displacement_body[0]+=fillter_vx*dt;
00081     displacement_body[1]+=fillter_vy*dt;
00082     displacement_body[2]+=fillter_vz*dt;*/
00083     
00084     }
00085 /*void VelocityToDisplacement(float dt){
00086     displacement_body[0]+=v_xyz[0]*dt;
00087     displacement_body[1]+=v_xyz[1]*dt;
00088     displacement_body[2]+=v_xyz[2]*dt;
00089     
00090     }*/
00091 int main()
00092 {   
00093     pc.baud(9600);
00094 
00095     //Set up I2C
00096     i2c.frequency(400000);  // use fast (400 kHz) I2C
00097 
00098 //    pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
00099 
00100     t.start();
00101 
00102 
00103 
00104 
00105     // Read the WHO_AM_I register, this is a good test of communication
00106     uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
00107 //    pc.printf("I AM 0x%x\n\r", whoami);
00108 //    pc.printf("I SHOULD BE 0x71\n\r");
00109 
00110     if (whoami == 0x71) { // WHO_AM_I should always be 0x68
00111 //        pc.printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami);
00112 //        pc.printf("MPU9250 is online...\n\r");
00113 
00114 //        sprintf(buffer, "0x%x", whoami);
00115 
00116         wait(1);
00117 
00118         mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
00119         mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
00120 /*        pc.printf("x-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[0]);
00121         pc.printf("y-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[1]);
00122         pc.printf("z-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[2]);
00123         pc.printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]);
00124         pc.printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]);
00125         pc.printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]);*/
00126 //     if(boardbtn==0){
00127         // wait(2);
00128         mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
00129         pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
00130         pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
00131         pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
00132         pc.printf("x accel bias = %f\n\r", accelBias[0]);
00133         pc.printf("y accel bias = %f\n\r", accelBias[1]);
00134         pc.printf("z accel bias = %f\n\r", accelBias[2]);
00135         wait(2);
00136         
00137 /*
00138 // -----------------------------------------------------------------
00139         }else{
00140         gyroBias[0]=-1.244275;
00141         gyroBias[1]=0.122137;
00142         gyroBias[2]=-0.717557;
00143         accelBias[0]=0.007996;                                      // For Manual Calibate with button
00144         accelBias[1]=0.021240;
00145         accelBias[2]=-0.020508;
00146         }
00147 //------------------------------------------------------------------
00148 */
00149         mpu9250.initMPU9250();
00150 //        pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
00151         mpu9250.initAK8963(magCalibration);
00152 /*        pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
00153         pc.printf("Accelerometer full-scale range = %f  g\n\r", 2.0f*(float)(1<<Ascale));
00154         pc.printf("Gyroscope full-scale range = %f  deg/s\n\r", 250.0f*(float)(1<<Gscale));
00155         if(Mscale == 0) pc.printf("Magnetometer resolution = 14  bits\n\r");
00156         if(Mscale == 1) pc.printf("Magnetometer resolution = 16  bits\n\r");
00157         if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
00158         if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
00159         wait(1);
00160 */
00161     } else {
00162         pc.printf("Could not connect to MPU9250: \n\r");
00163         pc.printf("%#x \n",  whoami);
00164 
00165 
00166         sprintf(buffer, "WHO_AM_I 0x%x", whoami);
00167 
00168 
00169         while(1) ; // Loop forever if communication doesn't happen
00170 
00171     }
00172 
00173     mpu9250.getAres(); // Get accelerometer sensitivity
00174     mpu9250.getGres(); // Get gyro sensitivity
00175     mpu9250.getMres(); // Get magnetometer sensitivity
00176 //    pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
00177 //    pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
00178 //    pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
00179     magbias[0] = +470.;  // User environmental x-axis correction in milliGauss, should be automatically calculated
00180     magbias[1] = +120.;  // User environmental x-axis correction in milliGauss
00181     magbias[2] = +125.;  // User environmental x-axis correction in milliGauss
00182 
00183     while(1) {
00184 
00185         // If intPin goes high, all data registers have new data
00186         if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {  // On interrupt, check if data ready interrupt
00187 
00188             mpu9250.readAccelData(accelCount);  // Read the x/y/z adc values
00189             // Now we'll calculate the accleration value into actual g's
00190             ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
00191             ay = (float)accelCount[1]*aRes - accelBias[1];
00192             az = (float)accelCount[2]*aRes - accelBias[2];
00193 
00194             mpu9250.readGyroData(gyroCount);  // Read the x/y/z adc values
00195             // Calculate the gyro value into actual degrees per second
00196             gx = (float)gyroCount[0]*gRes - gyroBias[0];  // get actual gyro value, this depends on scale being set
00197             gy = (float)gyroCount[1]*gRes - gyroBias[1];
00198             gz = (float)gyroCount[2]*gRes - gyroBias[2];
00199 
00200             mpu9250.readMagData(magCount);  // Read the x/y/z adc values
00201             // Calculate the magnetometer values in milliGauss
00202             // Include factory calibration per data sheet and user environmental corrections
00203             mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0];  // get actual magnetometer value, this depends on scale being set
00204             my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
00205             mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
00206         }
00207 
00208         Now = t.read_us();
00209         deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
00210         lastUpdate = Now;
00211 
00212         sum += deltat;
00213         sumCount++;
00214 
00215 //    if(lastUpdate - firstUpdate > 10000000.0f) {
00216 //     beta = 0.04;  // decrease filter gain after stabilized
00217 //     zeta = 0.015; // increasey bias drift gain after stabilized
00218 //   }
00219 
00220         // Pass gyro rate as rad/s
00221 // mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
00222         mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
00223 
00224         // Serial print and/or display at 0.5 s rate independent of data rates
00225         delt_t = t.read_ms() - count;
00226         if (delt_t > 100) { // update LCD once per half-second independent of read rate
00227 
00228   //  pc.printf("ax = %f", 1000*ax);
00229   //  pc.printf(" ay = %f", 1000*ay);
00230    // pc.printf(" az = %f  mg\n\r", 1000*az);
00231 
00232   // pc.printf("gx = %f", gx);
00233   //            pc.printf(" gy = %f", gy);
00234   // pc.printf(" gz = %f  deg/s\n\r", gz);
00235 
00236   // pc.printf("gx = %f", mx);
00237   // pc.printf(" gy = %f", my);
00238  // pc.printf(" gz = %f  mG\n\r", mz);
00239 
00240             tempCount = mpu9250.readTempData();  // Read the adc values
00241             temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
00242 //   pc.printf(" temperature = %f  C\n\r", temperature);
00243 //
00244 //    pc.printf("q0 = %f\n\r", q[0]);
00245 //    pc.printf("q1 = %f\n\r", q[1]);
00246 //    pc.printf("q2 = %f\n\r", q[2]);
00247 //    pc.printf("q3 = %f\n\r", q[3]);
00248 
00249             /*    lcd.clear();
00250                 lcd.printString("MPU9250", 0, 0);
00251                 lcd.printString("x   y   z", 0, 1);
00252                 sprintf(buffer, "%d %d %d mg", (int)(1000.0f*ax), (int)(1000.0f*ay), (int)(1000.0f*az));
00253                 lcd.printString(buffer, 0, 2);
00254                 sprintf(buffer, "%d %d %d deg/s", (int)gx, (int)gy, (int)gz);
00255                 lcd.printString(buffer, 0, 3);
00256                 sprintf(buffer, "%d %d %d mG", (int)mx, (int)my, (int)mz);
00257                 lcd.printString(buffer, 0, 4);
00258              */
00259             // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
00260             // In this coordinate system, the positive z-axis is down toward Earth.
00261             // 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.
00262             // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
00263             // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
00264             // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
00265             // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
00266             // applied in the correct order which for this configuration is yaw, pitch, and then roll.
00267             // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
00268             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]);
00269             pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
00270             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]);
00271             pitch *= 180.0f / PI;
00272             yaw   *= 180.0f / PI;
00273             yaw   -= 85.0f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
00274             roll  *= 180.0f / PI;
00275 
00276   //          pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
00277  //   pc.printf("average rate = %f\n\r", (float) sumCount/sum);
00278 //    sprintf(buffer, "YPR: %f %f %f", yaw, pitch, roll);
00279 //    lcd.printString(buffer, 0, 4);
00280 //    sprintf(buffer, "rate = %f", (float) sumCount/sum);
00281 //    lcd.printString(buffer, 0, 5);
00282 
00283             
00284     acc_xyz[0]=ax;
00285         acc_xyz[1]=ay;
00286         acc_xyz[2]=az;
00287         float mag_a=sqrt(pow(acc_xyz[0],2)+pow(acc_xyz[1],2)+pow(acc_xyz[2],2));
00288         AccelToVelocity(mag_a*cos(yaw),mag_a*sin(yaw),mag_a*sin(roll),float(delt_t)/10);//deltat*1000000
00289         static float fillter_vx=0;
00290     static float fillter_vy=0;
00291     static float fillter_vz=0;
00292     fillter_vx=v_xyz[0]*0.5+fillter_vx*0.5;
00293     fillter_vy=v_xyz[1]*0.5+fillter_vy*0.5;
00294     fillter_vz=v_xyz[2]*0.5+fillter_vz*0.5;
00295         //VelocityToDisplacement(float(delt_t));
00296         //pc.printf("%f\n",sqrt(displacement_body[0]*displacement_body[0]));
00297         pc.printf("%f %f\n",displacement_body[0],displacement_body[1]);
00298        // displacement_world[0]+=sqrt(pow(displacement_body[0],2)+pow(displacement_body[1],2)+pow(displacement_body[2],2))
00299        // pc.printf("%f %f \n",v_xyz[0],v_xyz[1]);
00300        // pc.printf("%f %f %f\n",fillter_vx,fillter_vy,fillter_vz);
00301             myled= !myled;
00302             count = t.read_ms();
00303             if(count > 1<<21) {
00304                 t.start(); // start the timer over again if ~30 minutes has passed
00305                 count = 0;
00306                 deltat= 0;
00307                 lastUpdate = t.read_us();
00308             }
00309             sum = 0;
00310             sumCount = 0;
00311 //-----------------------------------------------------------------------------------------        
00312         
00313         
00314         }
00315     }
00316     
00317 }