Johan Beverini
/
Project_5A_2
For a school project
Fork of MPU6050IMU by
main.cpp
- Committer:
- JohanBeverini
- Date:
- 2018-03-15
- Revision:
- 3:4c1180a712e3
- Parent:
- 1:cea9d83b8636
File content as of revision 3:4c1180a712e3:
#include "mbed.h" #include "MPU6050.h" #include <math.h> float sum = 0; uint32_t sumCount = 0; MPU6050 mpu6050; AnalogOut ANA1(A3); //AnalogOut ANA2(PA_5); Ticker ms; Timer t; Serial pc(SERIAL_TX, SERIAL_RX); // tx, rx Serial BT(PA_9, PA_10); // tx, rx float alpha, betaa, gammaa; float axx, ayy, azz; float poid[3]; float a, b, c, d, e, s; int i; float matrice[3][3], resultat[3]; bool first = true; bool tick_mili; float x_x_filter[3]={0,0,0}, x_y_filter[3]={0,0,0}; float y_x_filter[3]={0,0,0}, y_y_filter[3]={0,0,0}; float z_x_filter[3]={0,0,0}, z_y_filter[3]={0,0,0}; float a_coef[3]={1.0000, -1.5610, 0.6414}; float b_coef[3]={0.0201, 0.0402, 0.0201}; float x_x_filter_ph[3]={0,0,0}, x_y_filter_ph[3]={0,0,0}; float y_x_filter_ph[3]={0,0,0}, y_y_filter_ph[3]={0,0,0}; float z_x_filter_ph[3]={0,0,0}, z_y_filter_ph[3]={0,0,0}; float a_coef_ph[3]={1.0000, -1.9956, 0.9956}; float b_coef_ph[3]={0.9978, -1.9956, 0.9978}; float gx_filtre, gy_filtre, gz_filtre; float gx_filtre2=0.0f, gy_filtre2=0.0f, gz_filtre2=0.0f; float trapeze_x = 0.0f; float trapeze_y = 0.0f; float trapeze_z = 0.0f; void mili(void){ tick_mili=true; } int main() { pc.baud(9600); BT.baud(9600); pc.printf("hello word\n"); BT.printf("connection...\n"); //Set up I2C i2c.frequency(400000); // use fast (400 kHz) I2C alpha=0; betaa=0; gammaa=0; ms.attach(&mili, 0.001); t.start(); //lcd.init(); //lcd.setBrightness(0.05); // Read the WHO_AM_I register, this is a good test of communication uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050 pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x68\n\r"); if (whoami == 0x68) // WHO_AM_I should always be 0x68 { pc.printf("MPU6050 is online..."); wait(1); //lcd.clear(); //lcd.printString("MPU6050 OK", 0, 0); mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values pc.printf("x-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[0]); pc.printf("% of factory value \n\r"); pc.printf("y-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[1]); pc.printf("% of factory value \n\r"); pc.printf("z-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[2]); pc.printf("% of factory value \n\r"); pc.printf("x-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[3]); pc.printf("% of factory value \n\r"); pc.printf("y-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[4]); pc.printf("% of factory value \n\r"); pc.printf("z-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[5]); pc.printf("% of factory value \n\r"); wait(1); if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) { mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers mpu6050.initMPU6050(); pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature //lcd.clear(); //lcd.printString("MPU6050", 0, 0); //lcd.printString("pass self test", 0, 1); //lcd.printString("initializing", 0, 2); wait(2); } else { pc.printf("Device did not the pass self-test!\n\r"); //lcd.clear(); //lcd.printString("MPU6050", 0, 0); //lcd.printString("no pass", 0, 1); //lcd.printString("self test", 0, 2); } } else { pc.printf("Could not connect to MPU6050: \n\r"); pc.printf("%#x \n", whoami); //lcd.clear(); //lcd.printString("MPU6050", 0, 0); //lcd.printString("no connection", 0, 1); //lcd.printString("0x", 0, 2); lcd.setXYAddress(20, 2); lcd.printChar(whoami); while(1) ; // Loop forever if communication doesn't happen } while(1) { if (tick_mili==true){ tick_mili=false; // If data ready bit set, all data registers have new data if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt mpu6050.readAccelData(accelCount); // Read the x/y/z adc values mpu6050.getAres(); // Now we'll calculate the accleration value into actual g's ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set ay = (float)accelCount[1]*aRes - accelBias[1]; az = (float)accelCount[2]*aRes - accelBias[2]; mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values mpu6050.getGres(); // Calculate the gyro value into actual degrees per second gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set gy = (float)gyroCount[1]*gRes; // - gyroBias[1]; gz = (float)gyroCount[2]*gRes; // - gyroBias[2]; tempCount = mpu6050.readTempData(); // Read the x/y/z adc values temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade } Now = t.read_us(); deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update lastUpdate = Now; sum += deltat; sumCount++; if(lastUpdate - firstUpdate > 10000000.0f) { beta = 0.04; // decrease filter gain after stabilized zeta = 0.015; // increasey bias drift gain after stabilized } // Pass gyro rate as rad/s //mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f); //gx*=PI/180.0f; //gy*=PI/180.0f; //gz*=PI/180.0f; //gx/=1000.0f; //gy/=1000.0f; //gz/=1000.0f; ////////filtre PB 100Hz / PH 1Hz //x_x_filter[6]=x_x_filter[5]; x_x_filter[5]=x_x_filter[4]; x_x_filter[4]=x_x_filter[3]; //x_x_filter[3]=x_x_filter[2]; x_x_filter[2]=x_x_filter[1]; x_x_filter[1]=x_x_filter[0]; x_x_filter[0]=gx; //x_y_filter[6]=x_y_filter[5]; x_y_filter[5]=x_y_filter[4]; x_y_filter[4]=x_y_filter[3]; //x_y_filter[3]=x_y_filter[2]; x_y_filter[2]=x_y_filter[1]; x_y_filter[1]=x_y_filter[0]; x_y_filter[0]=b_coef[0]*x_x_filter[0]+b_coef[1]*x_x_filter[1]+b_coef[2]*x_x_filter[2] //+b_coef[3]*x_x_filter[3] //+b_coef[4]*x_x_filter[4]+b_coef[5]*x_x_filter[5]+b_coef[6]*x_x_filter[6] -(a_coef[1]*x_y_filter[1]+a_coef[2]*x_y_filter[2]); //+a_coef[3]*x_y_filter[3]); //+a_coef[4]*x_y_filter[4]+a_coef[5]*x_y_filter[5]+a_coef[6]*x_y_filter[6]); gx_filtre=x_y_filter[0]; //y_x_filter[6]=y_x_filter[5]; y_x_filter[5]=y_x_filter[4]; y_x_filter[4]=y_x_filter[3]; //y_x_filter[3]=y_x_filter[2]; y_x_filter[2]=y_x_filter[1]; y_x_filter[1]=y_x_filter[0]; y_x_filter[0]=gy; //y_y_filter[6]=y_y_filter[5]; y_y_filter[5]=y_y_filter[4]; y_y_filter[4]=y_y_filter[3]; //y_y_filter[3]=y_y_filter[2]; y_y_filter[2]=y_y_filter[1]; y_y_filter[1]=y_y_filter[0]; y_y_filter[0]=b_coef[0]*y_x_filter[0]+b_coef[1]*y_x_filter[1]+b_coef[2]*y_x_filter[2] //+b_coef[3]*y_x_filter[3] //+b_coef[4]*y_x_filter[4]+b_coef[5]*y_x_filter[5]+b_coef[6]*y_x_filter[6] -(a_coef[1]*y_y_filter[1]+a_coef[2]*y_y_filter[2]); //+a_coef[3]*y_y_filter[3]); //+a_coef[4]*y_y_filter[4]+a_coef[5]*y_y_filter[5]+a_coef[6]*y_y_filter[6]); gy_filtre=y_y_filter[0]; //z_x_filter[6]=z_x_filter[5]; z_x_filter[5]=z_x_filter[4]; z_x_filter[4]=z_x_filter[3]; //z_x_filter[3]=z_x_filter[2]; z_x_filter[2]=z_x_filter[1]; z_x_filter[1]=z_x_filter[0]; z_x_filter[0]=gz; //z_y_filter[6]=z_y_filter[5]; z_y_filter[5]=z_y_filter[4]; z_y_filter[4]=z_y_filter[3]; //z_y_filter[3]=z_y_filter[2]; z_y_filter[2]=z_y_filter[1]; z_y_filter[1]=z_y_filter[0]; z_y_filter[0]=b_coef[0]*z_x_filter[0]+b_coef[1]*z_x_filter[1]+b_coef[2]*z_x_filter[2] //+b_coef[3]*z_x_filter[3] //+b_coef[4]*z_x_filter[4]+b_coef[5]*z_x_filter[5]+b_coef[6]*z_x_filter[6] -(a_coef[1]*z_y_filter[1]+a_coef[2]*z_y_filter[2]); //+a_coef[3]*z_y_filter[3]); //+a_coef[4]*z_y_filter[4]+a_coef[5]*z_y_filter[5]+a_coef[6]*z_y_filter[6]); gz_filtre=z_y_filter[0]; ////////filtre PB 100Hz / PH 1Hz //x_x_filter[6]=x_x_filter[5]; x_x_filter[5]=x_x_filter[4]; x_x_filter[4]=x_x_filter[3]; //x_x_filter_ph[3]=x_x_filter_ph[2]; x_x_filter_ph[2]=x_x_filter_ph[1]; x_x_filter_ph[1]=x_x_filter_ph[0]; x_x_filter_ph[0]=gx_filtre; //x_y_filter[6]=x_y_filter[5]; x_y_filter[5]=x_y_filter[4]; x_y_filter[4]=x_y_filter[3]; //x_y_filter_ph[3]=x_y_filter_ph[2]; x_y_filter_ph[2]=x_y_filter_ph[1]; x_y_filter_ph[1]=x_y_filter_ph[0]; x_y_filter_ph[0]=b_coef_ph[0]*x_x_filter_ph[0]+b_coef_ph[1]*x_x_filter_ph[1]+b_coef_ph[2]*x_x_filter_ph[2] //+b_coef_ph[3]*x_x_filter_ph[3] //+b_coef[4]*x_x_filter[4]+b_coef[5]*x_x_filter[5]+b_coef[6]*x_x_filter[6] -(a_coef_ph[1]*x_y_filter_ph[1]+a_coef_ph[2]*x_y_filter_ph[2]); //+a_coef_ph[3]*x_y_filter_ph[3]); //+a_coef[4]*x_y_filter[4]+a_coef[5]*x_y_filter[5]+a_coef[6]*x_y_filter[6]); gx_filtre=x_y_filter_ph[0]; //y_x_filter[6]=y_x_filter[5]; y_x_filter[5]=y_x_filter[4]; y_x_filter[4]=y_x_filter[3]; //y_x_filter_ph[3]=y_x_filter_ph[2]; y_x_filter_ph[2]=y_x_filter_ph[1]; y_x_filter_ph[1]=y_x_filter_ph[0]; y_x_filter_ph[0]=gy_filtre; //y_y_filter[6]=y_y_filter[5]; y_y_filter[5]=y_y_filter[4]; y_y_filter[4]=y_y_filter[3]; //y_y_filter_ph[3]=y_y_filter_ph[2]; y_y_filter_ph[2]=y_y_filter_ph[1]; y_y_filter_ph[1]=y_y_filter_ph[0]; y_y_filter_ph[0]=b_coef_ph[0]*y_x_filter_ph[0]+b_coef_ph[1]*y_x_filter_ph[1]+b_coef_ph[2]*y_x_filter_ph[2] //+b_coef_ph[3]*y_x_filter_ph[3] //+b_coef[4]*y_x_filter[4]+b_coef[5]*y_x_filter[5]+b_coef[6]*y_x_filter[6] -(a_coef_ph[1]*y_y_filter_ph[1]+a_coef_ph[2]*y_y_filter_ph[2]); //+a_coef_ph[3]*y_y_filter_ph[3]); //+a_coef[4]*y_y_filter[4]+a_coef[5]*y_y_filter[5]+a_coef[6]*y_y_filter[6]); gy_filtre=y_y_filter_ph[0]; //z_x_filter[6]=z_x_filter[5]; z_x_filter[5]=z_x_filter[4]; z_x_filter[4]=z_x_filter[3]; //z_x_filter_ph[3]=z_x_filter_ph[2]; z_x_filter_ph[2]=z_x_filter_ph[1]; z_x_filter_ph[1]=z_x_filter_ph[0]; z_x_filter_ph[0]=gz_filtre; //z_y_filter[6]=z_y_filter[5]; z_y_filter[5]=z_y_filter[4]; z_y_filter[4]=z_y_filter[3]; //z_y_filter_ph[3]=z_y_filter_ph[2]; z_y_filter_ph[2]=z_y_filter_ph[1]; z_y_filter_ph[1]=z_y_filter_ph[0]; z_y_filter_ph[0]=b_coef_ph[0]*z_x_filter_ph[0]+b_coef_ph[1]*z_x_filter_ph[1]+b_coef_ph[2]*z_x_filter_ph[2] //+b_coef_ph[3]*z_x_filter_ph[3] //+b_coef[4]*z_x_filter[4]+b_coef[5]*z_x_filter[5]+b_coef[6]*z_x_filter[6] -(a_coef_ph[1]*z_y_filter_ph[1]+a_coef_ph[2]*z_y_filter_ph[2]); //+a_coef_ph[3]*z_y_filter_ph[3]); //+a_coef[4]*z_y_filter[4]+a_coef[5]*z_y_filter[5]+a_coef[6]*z_y_filter[6]); gz_filtre=z_y_filter_ph[0]; trapeze_x=deltat*((gx_filtre+gx_filtre2)/2.0f); trapeze_y=deltat*((gy_filtre+gy_filtre2)/2.0f); trapeze_z=deltat*((gz_filtre+gz_filtre2)/2.0f); gx_filtre2=gx_filtre; gy_filtre2=gy_filtre; gz_filtre2=gz_filtre; //calcule angle alpha+=trapeze_x; betaa+=trapeze_y; gammaa+=trapeze_z; if(alpha>=360.0f){alpha-=360.0f;} if(alpha<=-360.0f){alpha+=360.0f;} if(betaa>=360.0f){betaa-=360.0f;} if(betaa<=-360.0f){betaa+=360.0f;} if(gammaa>=360.0f){gammaa-=360.0f;} if(gammaa<=-360.0f){gammaa+=360.0f;} ANA1.write((alpha+500.0f)/1000.0f); //ANA2.write(alpha/360.0f); // Serial print and/or display at 0.5 s rate independent of data rates delt_t = t.read_ms() - count; if (delt_t > 100) { // update LCD once per half-second independent of read rate pc.printf("ax = %f", 1000*ax); pc.printf(" ay = %f", 1000*ay); pc.printf(" az = %f mg\n\r", 1000*az); pc.printf("gx = %f", gx); pc.printf(" gy = %f", gy); pc.printf(" gz = %f deg/s\n\r", gz); // pc.printf("post filtre : gx = %f", gx_filtre2); // pc.printf(" gy = %f", gy_filtre2); // pc.printf(" gz = %f deg/s\n\r", gz_filtre2); pc.printf(" temperature = %f C\n\r", temperature); // pc.printf("q0 = %f\n\r", q[0]); // pc.printf("q1 = %f\n\r", q[1]); // pc.printf("q2 = %f\n\r", q[2]); // pc.printf("q3 = %f\n\r", q[3]); //lcd.clear(); //lcd.printString("MPU6050", 0, 0); //lcd.printString("x y z", 0, 1); //lcd.setXYAddress(0, 2); lcd.printChar((char)(1000*ax)); //lcd.setXYAddress(20, 2); lcd.printChar((char)(1000*ay)); //lcd.setXYAddress(40, 2); lcd.printChar((char)(1000*az)); lcd.printString("mg", 66, 2); // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation. // In this coordinate system, the positive z-axis is down toward Earth. // 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. // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative. // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll. // These arise from the definition of the homogeneous rotation matrix constructed from quaternions. // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be // applied in the correct order which for this configuration is yaw, pitch, and then roll. // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links. //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]); //pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); //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]); //pitch *= 180.0f / PI; //yaw *= 180.0f / PI; //roll *= 180.0f / PI; // pc.printf("Yaw, Pitch, Roll: \n\r"); // pc.printf("%f", yaw); // pc.printf(", "); // pc.printf("%f", pitch); // pc.printf(", "); // pc.printf("%f\n\r", roll); // pc.printf("average rate = "); pc.printf("%f", (sumCount/sum)); pc.printf(" Hz\n\r"); //pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll); //pc.printf("average rate = %f\n\r", (float) sumCount/sum); //BT.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll); //BT.printf("average rate = %f\n\r", (float) sumCount/sum); //alpha=yaw; //betaa=pitch; //gammaa=roll; pc.printf("delta = %f\n\r", (float) deltat); // pc.printf("alpha, beta, gamma: %f %f %f\n\r", alpha, betaa, gammaa); axx=ax; ayy=ay; azz=az; ////////////////////////////////////////////////////////Matrice d'Euler(); c = cos(alpha*PI/180.0f); s = sin(alpha*PI/180.0f); a = cos(betaa*PI/180.0f); b = sin(betaa*PI/180.0f); d = cos(gammaa*PI/180.0f); e = sin(gammaa*PI/180.0f); matrice[0][0] = e*a - e*c*b; matrice[0][1] = (-d)*b - e*c*a; matrice[0][2] = e*s; matrice[1][0] = e*a + d*c*b; matrice[1][1] = (-e)*b + d*c*a; matrice[1][2] = (-d)*s; matrice[2][0] = s*b; matrice[2][1] = s*a; matrice[2][2] = c; for(i=0; i<3; i++) { float temp = 0; temp = axx * matrice[i][0] + ayy * matrice[i][1] + azz * matrice[i][2]; resultat[i] = temp; } ////////////////////////////////////////////////////////// // if (first==true){ // poid[0]=resultat[0]; // poid[1]=resultat[1]; // poid[2]=resultat[2]; // first=false; // } else { // resultat[0]-=poid[0]; // resultat[1]-=poid[1]; // resultat[2]-=poid[2]; // } // pc.printf("acceleration sans Euler : %f ; %f ; %f\n\r", axx, ayy, azz); // pc.printf("acceleration avec Euler : %f ; %f ; %f\n\r", resultat[0], resultat[1], resultat[2]); BT.printf("acceleration sans Euler : %f ; %f ; %f\n\r", axx, ayy, azz); BT.printf("acceleration avec Euler : %f ; %f ; %f\n\r", resultat[0], resultat[1], resultat[2]); myled= !myled; count = t.read_ms(); sum = 0; sumCount = 0; } } if (BT.readable()) { char c = BT.getc(); if(c == 'a') { BT.printf("\nOK\n"); } } } }