Nicola d'Ambrosio / Mbed 2 deprecated ncola_f4_def_v2_def

sdaffass

Dependencies:   SDFileSystem_conMOD mbed-rtos mbed

Fork of ncola_f4_def_v2_def by Unina_corse

accellerometro.h@12:f5aae967c4d7, 2018-01-29 (annotated)

Committer:
NdA994
Date:
Mon Jan 29 13:31:16 2018 +0000
Revision:
12:f5aae967c4d7
Parent:
9:7f0c1261e905 
ruote foniche con binario;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
NdA994 3:3b2b8b0955f9 1 #ifndef __ACCELLEROMETRO__
NdA994 3:3b2b8b0955f9 2 #define __ACCELLEROMETRO__
NdA994 3:3b2b8b0955f9 3
NdA994 3:3b2b8b0955f9 4 #include "MPU6050.h"
NdA994 3:3b2b8b0955f9 5 #include "setting.h"
NdA994 3:3b2b8b0955f9 6
NdA994 3:3b2b8b0955f9 7
NdA994 3:3b2b8b0955f9 8 float sum = 0;
NdA994 3:3b2b8b0955f9 9 uint32_t sumCount = 0;
NdA994 3:3b2b8b0955f9 10
NdA994 3:3b2b8b0955f9 11 MPU6050 mpu6050;
NdA994 3:3b2b8b0955f9 12
NdA994 3:3b2b8b0955f9 13 Timer t;
NdA994 3:3b2b8b0955f9 14
NdA994 3:3b2b8b0955f9 15 void initAccellerometro(){
NdA994 3:3b2b8b0955f9 16 t.start();
NdA994 3:3b2b8b0955f9 17
NdA994 3:3b2b8b0955f9 18 i2c.frequency(400000);
NdA994 3:3b2b8b0955f9 19 uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050 //DEBUG
NdA994 3:3b2b8b0955f9 20 pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x68\n\r"); //DEBUG
NdA994 3:3b2b8b0955f9 21
NdA994 3:3b2b8b0955f9 22 if (whoami == 0x68){
NdA994 3:3b2b8b0955f9 23 pc.printf("MPU6050 is online...");
NdA994 3:3b2b8b0955f9 24 wait(1);
NdA994 3:3b2b8b0955f9 25
NdA994 3:3b2b8b0955f9 26 mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
NdA994 3:3b2b8b0955f9 27 pc.printf("x-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[0]); pc.printf("% of factory value \n\r");
NdA994 3:3b2b8b0955f9 28 pc.printf("y-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[1]); pc.printf("% of factory value \n\r");
NdA994 3:3b2b8b0955f9 29 pc.printf("z-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[2]); pc.printf("% of factory value \n\r");
NdA994 3:3b2b8b0955f9 30 pc.printf("x-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[3]); pc.printf("% of factory value \n\r");
NdA994 3:3b2b8b0955f9 31 pc.printf("y-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[4]); pc.printf("% of factory value \n\r");
NdA994 3:3b2b8b0955f9 32 pc.printf("z-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[5]); pc.printf("% of factory value \n\r");
NdA994 3:3b2b8b0955f9 33 wait(1);
NdA994 3:3b2b8b0955f9 34
NdA994 3:3b2b8b0955f9 35 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) {
NdA994 3:3b2b8b0955f9 36 mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
NdA994 3:3b2b8b0955f9 37 mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
NdA994 3:3b2b8b0955f9 38 mpu6050.initMPU6050(); pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
NdA994 3:3b2b8b0955f9 39
NdA994 3:3b2b8b0955f9 40 wait(2);
NdA994 3:3b2b8b0955f9 41 }
NdA994 3:3b2b8b0955f9 42 else{
NdA994 3:3b2b8b0955f9 43 pc.printf("Device did not the pass self-test!\n\r");
NdA994 3:3b2b8b0955f9 44 }
NdA994 3:3b2b8b0955f9 45 }
NdA994 3:3b2b8b0955f9 46 else{
NdA994 3:3b2b8b0955f9 47 pc.printf("Could not connect to MPU6050: \n\r");
NdA994 3:3b2b8b0955f9 48 pc.printf("%#x \n", whoami);
NdA994 3:3b2b8b0955f9 49
NdA994 3:3b2b8b0955f9 50 while(1) ; // Loop forever if communication doesn't happen
NdA994 3:3b2b8b0955f9 51 }
NdA994 3:3b2b8b0955f9 52 }
NdA994 3:3b2b8b0955f9 53
NdA994 3:3b2b8b0955f9 54 void raccoltaDati(){
NdA994 3:3b2b8b0955f9 55 if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt
NdA994 3:3b2b8b0955f9 56 mpu6050.readAccelData(accelCount); // Read the x/y/z adc values
NdA994 3:3b2b8b0955f9 57 mpu6050.getAres();
NdA994 3:3b2b8b0955f9 58
NdA994 3:3b2b8b0955f9 59 // Now we'll calculate the accleration value into actual g's
NdA994 3:3b2b8b0955f9 60 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
NdA994 3:3b2b8b0955f9 61 ay = (float)accelCount[1]*aRes - accelBias[1];
NdA994 3:3b2b8b0955f9 62 az = (float)accelCount[2]*aRes - accelBias[2];
NdA994 3:3b2b8b0955f9 63
NdA994 9:7f0c1261e905 64 fprintf(fp,"A%03.0f%03.0f%03.0f\n\r", 100*ax+400, 100*ay+400, 100*az+400);
NdA994 9:7f0c1261e905 65
NdA994 9:7f0c1261e905 66
NdA994 3:3b2b8b0955f9 67 mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values
NdA994 3:3b2b8b0955f9 68 mpu6050.getGres();
NdA994 3:3b2b8b0955f9 69
NdA994 3:3b2b8b0955f9 70 // Calculate the gyro value into actual degrees per second
NdA994 3:3b2b8b0955f9 71 gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set
NdA994 3:3b2b8b0955f9 72 gy = (float)gyroCount[1]*gRes; // - gyroBias[1];
NdA994 3:3b2b8b0955f9 73 gz = (float)gyroCount[2]*gRes; // - gyroBias[2];
NdA994 3:3b2b8b0955f9 74
NdA994 3:3b2b8b0955f9 75 tempCount = mpu6050.readTempData(); // Read the x/y/z adc values
NdA994 3:3b2b8b0955f9 76 temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
NdA994 3:3b2b8b0955f9 77 }
NdA994 3:3b2b8b0955f9 78
NdA994 3:3b2b8b0955f9 79 Now = t.read_us();
NdA994 3:3b2b8b0955f9 80 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
NdA994 3:3b2b8b0955f9 81 lastUpdate = Now;
NdA994 3:3b2b8b0955f9 82
NdA994 3:3b2b8b0955f9 83 sum += deltat;
NdA994 3:3b2b8b0955f9 84 sumCount++;
NdA994 3:3b2b8b0955f9 85
NdA994 3:3b2b8b0955f9 86 if(lastUpdate - firstUpdate > 10000000.0f) {
NdA994 3:3b2b8b0955f9 87 beta = 0.04; // decrease filter gain after stabilized
NdA994 3:3b2b8b0955f9 88 zeta = 0.015; // increasey bias drift gain after stabilized
NdA994 3:3b2b8b0955f9 89 }
NdA994 3:3b2b8b0955f9 90
NdA994 3:3b2b8b0955f9 91 // Pass gyro rate as rad/s
NdA994 3:3b2b8b0955f9 92 mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
NdA994 3:3b2b8b0955f9 93
NdA994 3:3b2b8b0955f9 94 // Serial print and/or display at 0.5 s rate independent of data rates
NdA994 3:3b2b8b0955f9 95 delt_t = t.read_ms() - count;
NdA994 3:3b2b8b0955f9 96 if (delt_t > 500) { // update LCD once per half-second independent of read rate
NdA994 3:3b2b8b0955f9 97 /*
NdA994 3:3b2b8b0955f9 98 pc.printf("ax = %f", 1000*ax);
NdA994 3:3b2b8b0955f9 99 pc.printf(" ay = %f", 1000*ay);
NdA994 3:3b2b8b0955f9 100 pc.printf(" az = %f mg\n\r", 1000*az);
NdA994 3:3b2b8b0955f9 101
NdA994 3:3b2b8b0955f9 102 pc.printf("gx = %f", gx);
NdA994 3:3b2b8b0955f9 103 pc.printf(" gy = %f", gy);
NdA994 3:3b2b8b0955f9 104 pc.printf(" gz = %f deg/s\n\r", gz);
NdA994 3:3b2b8b0955f9 105
NdA994 3:3b2b8b0955f9 106 pc.printf(" temperature = %f C\n\r", temperature);
NdA994 3:3b2b8b0955f9 107
NdA994 3:3b2b8b0955f9 108 pc.printf("q0 = %f\n\r", q[0]);
NdA994 3:3b2b8b0955f9 109 pc.printf("q1 = %f\n\r", q[1]);
NdA994 3:3b2b8b0955f9 110 pc.printf("q2 = %f\n\r", q[2]);
NdA994 3:3b2b8b0955f9 111 pc.printf("q3 = %f\n\r", q[3]);
NdA994 3:3b2b8b0955f9 112 */
NdA994 3:3b2b8b0955f9 113
NdA994 3:3b2b8b0955f9 114 // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
NdA994 3:3b2b8b0955f9 115 // In this coordinate system, the positive z-axis is down toward Earth.
NdA994 3:3b2b8b0955f9 116 // 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.
NdA994 3:3b2b8b0955f9 117 // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
NdA994 3:3b2b8b0955f9 118 // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
NdA994 3:3b2b8b0955f9 119 // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
NdA994 3:3b2b8b0955f9 120 // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
NdA994 3:3b2b8b0955f9 121 // applied in the correct order which for this configuration is yaw, pitch, and then roll.
NdA994 3:3b2b8b0955f9 122 // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
NdA994 3:3b2b8b0955f9 123 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]);
NdA994 3:3b2b8b0955f9 124 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
NdA994 3:3b2b8b0955f9 125 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]);
NdA994 3:3b2b8b0955f9 126 pitch *= 180.0f / PI;
NdA994 3:3b2b8b0955f9 127 yaw *= 180.0f / PI;
NdA994 3:3b2b8b0955f9 128 roll *= 180.0f / PI;
NdA994 3:3b2b8b0955f9 129
NdA994 3:3b2b8b0955f9 130 /*
NdA994 3:3b2b8b0955f9 131 pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
NdA994 3:3b2b8b0955f9 132 pc.printf("average rate = %f\n\r", (float) sumCount/sum);
NdA994 3:3b2b8b0955f9 133 */
NdA994 3:3b2b8b0955f9 134
NdA994 3:3b2b8b0955f9 135 count = t.read_ms();
NdA994 3:3b2b8b0955f9 136 sum = 0;
NdA994 3:3b2b8b0955f9 137 sumCount = 0;
NdA994 3:3b2b8b0955f9 138 }
NdA994 3:3b2b8b0955f9 139 }
NdA994 3:3b2b8b0955f9 140
NdA994 3:3b2b8b0955f9 141
NdA994 3:3b2b8b0955f9 142
NdA994 3:3b2b8b0955f9 143 #endif