Rogelio Vazquez / Mbed 2 deprecated Robosub_test

first publish

Dependencies:   HMC5883L mbed

IMU.h@0:ce3ac53af6e4, 2017-05-15 (annotated)

Committer:
roger_wee
Date:
Mon May 15 21:21:08 2017 +0000
Revision:
0:ce3ac53af6e4
first_commit;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
roger_wee 0:ce3ac53af6e4 1 #include "MPU6050.h"
roger_wee 0:ce3ac53af6e4 2
roger_wee 0:ce3ac53af6e4 3 float sum = 0;
roger_wee 0:ce3ac53af6e4 4 uint32_t sumCount = 0;
roger_wee 0:ce3ac53af6e4 5 Timer t;
roger_wee 0:ce3ac53af6e4 6 Serial pc(USBTX, USBRX);
roger_wee 0:ce3ac53af6e4 7
roger_wee 0:ce3ac53af6e4 8 void IMUinit(MPU6050 &mpu6050)
roger_wee 0:ce3ac53af6e4 9 {
roger_wee 0:ce3ac53af6e4 10 //start timer/clock
roger_wee 0:ce3ac53af6e4 11 t.start();
roger_wee 0:ce3ac53af6e4 12
roger_wee 0:ce3ac53af6e4 13 // Read the WHO_AM_I register, this is a good test of communication
roger_wee 0:ce3ac53af6e4 14 uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050
roger_wee 0:ce3ac53af6e4 15 pc.printf("I AM 0x%x\n\r", whoami);
roger_wee 0:ce3ac53af6e4 16 pc.printf("I SHOULD BE 0x68\n\r");
roger_wee 0:ce3ac53af6e4 17
roger_wee 0:ce3ac53af6e4 18 if (whoami == 0x68) { // WHO_AM_I should always be 0x68
roger_wee 0:ce3ac53af6e4 19 pc.printf("MPU6050 is online...");
roger_wee 0:ce3ac53af6e4 20 wait(1);
roger_wee 0:ce3ac53af6e4 21 //lcd.clear();
roger_wee 0:ce3ac53af6e4 22 //lcd.printString("MPU6050 OK", 0, 0);
roger_wee 0:ce3ac53af6e4 23
roger_wee 0:ce3ac53af6e4 24 mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
roger_wee 0:ce3ac53af6e4 25 pc.printf("x-axis self test: acceleration trim within : ");
roger_wee 0:ce3ac53af6e4 26 pc.printf("%f", SelfTest[0]);
roger_wee 0:ce3ac53af6e4 27 pc.printf("% of factory value \n\r");
roger_wee 0:ce3ac53af6e4 28 pc.printf("y-axis self test: acceleration trim within : ");
roger_wee 0:ce3ac53af6e4 29 pc.printf("%f", SelfTest[1]);
roger_wee 0:ce3ac53af6e4 30 pc.printf("% of factory value \n\r");
roger_wee 0:ce3ac53af6e4 31 pc.printf("z-axis self test: acceleration trim within : ");
roger_wee 0:ce3ac53af6e4 32 pc.printf("%f", SelfTest[2]);
roger_wee 0:ce3ac53af6e4 33 pc.printf("% of factory value \n\r");
roger_wee 0:ce3ac53af6e4 34 pc.printf("x-axis self test: gyration trim within : ");
roger_wee 0:ce3ac53af6e4 35 pc.printf("%f", SelfTest[3]);
roger_wee 0:ce3ac53af6e4 36 pc.printf("% of factory value \n\r");
roger_wee 0:ce3ac53af6e4 37 pc.printf("y-axis self test: gyration trim within : ");
roger_wee 0:ce3ac53af6e4 38 pc.printf("%f", SelfTest[4]);
roger_wee 0:ce3ac53af6e4 39 pc.printf("% of factory value \n\r");
roger_wee 0:ce3ac53af6e4 40 pc.printf("z-axis self test: gyration trim within : ");
roger_wee 0:ce3ac53af6e4 41 pc.printf("%f", SelfTest[5]);
roger_wee 0:ce3ac53af6e4 42 pc.printf("% of factory value \n\r");
roger_wee 0:ce3ac53af6e4 43 wait(1);
roger_wee 0:ce3ac53af6e4 44
roger_wee 0:ce3ac53af6e4 45 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) {
roger_wee 0:ce3ac53af6e4 46 mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
roger_wee 0:ce3ac53af6e4 47
roger_wee 0:ce3ac53af6e4 48 mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
roger_wee 0:ce3ac53af6e4 49
roger_wee 0:ce3ac53af6e4 50 mpu6050.resetMPU6050();
roger_wee 0:ce3ac53af6e4 51
roger_wee 0:ce3ac53af6e4 52 mpu6050.initMPU6050();
roger_wee 0:ce3ac53af6e4 53 pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
roger_wee 0:ce3ac53af6e4 54 wait(2);
roger_wee 0:ce3ac53af6e4 55
roger_wee 0:ce3ac53af6e4 56 } else {
roger_wee 0:ce3ac53af6e4 57 pc.printf("Device did not the pass self-test!\n\r");
roger_wee 0:ce3ac53af6e4 58 }
roger_wee 0:ce3ac53af6e4 59 } else {
roger_wee 0:ce3ac53af6e4 60 pc.printf("Could not connect to MPU6050: \n\r");
roger_wee 0:ce3ac53af6e4 61 pc.printf("%#x \n", whoami);
roger_wee 0:ce3ac53af6e4 62
roger_wee 0:ce3ac53af6e4 63 while(1) ; // Loop forever if communication doesn't happen
roger_wee 0:ce3ac53af6e4 64 }
roger_wee 0:ce3ac53af6e4 65 }
roger_wee 0:ce3ac53af6e4 66
roger_wee 0:ce3ac53af6e4 67
roger_wee 0:ce3ac53af6e4 68 void IMUPrintData(MPU6050 &mpu6050)
roger_wee 0:ce3ac53af6e4 69 {
roger_wee 0:ce3ac53af6e4 70
roger_wee 0:ce3ac53af6e4 71 // pc.printf("Beginning IMU read\n");
roger_wee 0:ce3ac53af6e4 72 // If data ready bit set, all data registers have new data
roger_wee 0:ce3ac53af6e4 73 if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt
roger_wee 0:ce3ac53af6e4 74
roger_wee 0:ce3ac53af6e4 75 mpu6050.readAccelData(accelCount); // Read the x/y/z adc values
roger_wee 0:ce3ac53af6e4 76 mpu6050.getAres();
roger_wee 0:ce3ac53af6e4 77
roger_wee 0:ce3ac53af6e4 78 // Now we'll calculate the accleration value into actual g's
roger_wee 0:ce3ac53af6e4 79 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
roger_wee 0:ce3ac53af6e4 80 ay = (float)accelCount[1]*aRes - accelBias[1];
roger_wee 0:ce3ac53af6e4 81 az = (float)accelCount[2]*aRes - accelBias[2];
roger_wee 0:ce3ac53af6e4 82
roger_wee 0:ce3ac53af6e4 83 mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values
roger_wee 0:ce3ac53af6e4 84 mpu6050.getGres();
roger_wee 0:ce3ac53af6e4 85
roger_wee 0:ce3ac53af6e4 86 // Calculate the gyro value into actual degrees per second
roger_wee 0:ce3ac53af6e4 87 gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
roger_wee 0:ce3ac53af6e4 88 gy = (float)gyroCount[1]*gRes - gyroBias[1];
roger_wee 0:ce3ac53af6e4 89 gz = (float)gyroCount[2]*gRes - gyroBias[2];
roger_wee 0:ce3ac53af6e4 90
roger_wee 0:ce3ac53af6e4 91 tempCount = mpu6050.readTempData(); // Read the x/y/z adc values
roger_wee 0:ce3ac53af6e4 92 temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
roger_wee 0:ce3ac53af6e4 93 }
roger_wee 0:ce3ac53af6e4 94
roger_wee 0:ce3ac53af6e4 95 Now = t.read_us();
roger_wee 0:ce3ac53af6e4 96 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
roger_wee 0:ce3ac53af6e4 97 sampleFreq = 1/deltat;
roger_wee 0:ce3ac53af6e4 98 lastUpdate = Now;
roger_wee 0:ce3ac53af6e4 99
roger_wee 0:ce3ac53af6e4 100 sum += deltat;
roger_wee 0:ce3ac53af6e4 101 sumCount++;
roger_wee 0:ce3ac53af6e4 102
roger_wee 0:ce3ac53af6e4 103 if(lastUpdate - firstUpdate > 10000000.0f) {
roger_wee 0:ce3ac53af6e4 104 beta = 0.04; // decrease filter gain after stabilized
roger_wee 0:ce3ac53af6e4 105 zeta = 0.015; // increasey bias drift gain after stabilized
roger_wee 0:ce3ac53af6e4 106 }
roger_wee 0:ce3ac53af6e4 107
roger_wee 0:ce3ac53af6e4 108 //Convert gyro rate as rad/s
roger_wee 0:ce3ac53af6e4 109 gx *= PI/180.0f;
roger_wee 0:ce3ac53af6e4 110 gy *= PI/180.0f;
roger_wee 0:ce3ac53af6e4 111 gz *= PI/180.0f;
roger_wee 0:ce3ac53af6e4 112
roger_wee 0:ce3ac53af6e4 113
roger_wee 0:ce3ac53af6e4 114 // Pass gyro rate as rad/s
roger_wee 0:ce3ac53af6e4 115 mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx, gy, gz);
roger_wee 0:ce3ac53af6e4 116 //mpu6050.MadgwickAHRSupdate(gx, gy, gz, ax, ay, az, magData[0], magData[1], magData[2]);
roger_wee 0:ce3ac53af6e4 117
roger_wee 0:ce3ac53af6e4 118 // Serial print and/or display at 0.5 s rate independent of data rates
roger_wee 0:ce3ac53af6e4 119 delt_t = t.read_ms() - count;
roger_wee 0:ce3ac53af6e4 120 if (delt_t > 0) { // update LCD once per half-second independent of read rate
roger_wee 0:ce3ac53af6e4 121
roger_wee 0:ce3ac53af6e4 122 // pc.printf("ax = %f", 1000*ax);
roger_wee 0:ce3ac53af6e4 123 // pc.printf(" ay = %f", 1000*ay);
roger_wee 0:ce3ac53af6e4 124 // pc.printf(" az = %f mg\n\r", 1000*az);
roger_wee 0:ce3ac53af6e4 125
roger_wee 0:ce3ac53af6e4 126 // pc.printf("gx = %f", gx);
roger_wee 0:ce3ac53af6e4 127 // pc.printf(" gy = %f", gy);
roger_wee 0:ce3ac53af6e4 128 // pc.printf(" gz = %f deg/s\n\r", gz);
roger_wee 0:ce3ac53af6e4 129
roger_wee 0:ce3ac53af6e4 130 // pc.printf(" temperature = %f C\n\r", temperature);
roger_wee 0:ce3ac53af6e4 131
roger_wee 0:ce3ac53af6e4 132 // pc.printf("q0 = %f\n\r", q[0]);
roger_wee 0:ce3ac53af6e4 133 // pc.printf("q1 = %f\n\r", q[1]);
roger_wee 0:ce3ac53af6e4 134 // pc.printf("q2 = %f\n\r", q[2]);
roger_wee 0:ce3ac53af6e4 135 // pc.printf("q3 = %f\n\r", q[3]);
roger_wee 0:ce3ac53af6e4 136
roger_wee 0:ce3ac53af6e4 137 // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
roger_wee 0:ce3ac53af6e4 138 // In this coordinate system, the positive z-axis is down toward Earth.
roger_wee 0:ce3ac53af6e4 139 // 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.
roger_wee 0:ce3ac53af6e4 140 // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
roger_wee 0:ce3ac53af6e4 141 // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
roger_wee 0:ce3ac53af6e4 142 // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
roger_wee 0:ce3ac53af6e4 143 // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
roger_wee 0:ce3ac53af6e4 144 // applied in the correct order which for this configuration is yaw, pitch, and then roll.
roger_wee 0:ce3ac53af6e4 145 // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
roger_wee 0:ce3ac53af6e4 146 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]);
roger_wee 0:ce3ac53af6e4 147 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
roger_wee 0:ce3ac53af6e4 148 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]);
roger_wee 0:ce3ac53af6e4 149 pitch *= 180.0f / PI;
roger_wee 0:ce3ac53af6e4 150 yaw *= 180.0f / PI;
roger_wee 0:ce3ac53af6e4 151 roll *= 180.0f / PI;
roger_wee 0:ce3ac53af6e4 152
roger_wee 0:ce3ac53af6e4 153 // pc.printf("Yaw, Pitch, Roll: \n\r");
roger_wee 0:ce3ac53af6e4 154 // pc.printf("%f", yaw);
roger_wee 0:ce3ac53af6e4 155 // pc.printf(", ");
roger_wee 0:ce3ac53af6e4 156 // pc.printf("%f", pitch);
roger_wee 0:ce3ac53af6e4 157 // pc.printf(", ");
roger_wee 0:ce3ac53af6e4 158 // pc.printf("%f\n\r", roll);
roger_wee 0:ce3ac53af6e4 159 // pc.printf("average rate = "); pc.printf("%f", (sumCount/sum)); pc.printf(" Hz\n\r");
roger_wee 0:ce3ac53af6e4 160
roger_wee 0:ce3ac53af6e4 161 //pc.printf("average rate = %f\n\r", (float) sumCount/sum);
roger_wee 0:ce3ac53af6e4 162
roger_wee 0:ce3ac53af6e4 163 //myled= !myled;
roger_wee 0:ce3ac53af6e4 164 count = t.read_ms();
roger_wee 0:ce3ac53af6e4 165 sum = 0;
roger_wee 0:ce3ac53af6e4 166 sumCount = 0;
roger_wee 0:ce3ac53af6e4 167 }
roger_wee 0:ce3ac53af6e4 168 }
roger_wee 0:ce3ac53af6e4 169
roger_wee 0:ce3ac53af6e4 170 void IMUUpdate(MPU6050 &mpu6050)
roger_wee 0:ce3ac53af6e4 171 {
roger_wee 0:ce3ac53af6e4 172 // If data ready bit set, all data registers have new data
roger_wee 0:ce3ac53af6e4 173 if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt
roger_wee 0:ce3ac53af6e4 174 mpu6050.readAccelData(accelCount); // Read the x/y/z adc values
roger_wee 0:ce3ac53af6e4 175 mpu6050.getAres();
roger_wee 0:ce3ac53af6e4 176
roger_wee 0:ce3ac53af6e4 177 // Now we'll calculate the accleration value into actual g's
roger_wee 0:ce3ac53af6e4 178 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
roger_wee 0:ce3ac53af6e4 179 ay = (float)accelCount[1]*aRes - accelBias[1];
roger_wee 0:ce3ac53af6e4 180 az = (float)accelCount[2]*aRes - accelBias[2];
roger_wee 0:ce3ac53af6e4 181
roger_wee 0:ce3ac53af6e4 182 mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values
roger_wee 0:ce3ac53af6e4 183 mpu6050.getGres();
roger_wee 0:ce3ac53af6e4 184
roger_wee 0:ce3ac53af6e4 185 // Calculate the gyro value into actual degrees per second
roger_wee 0:ce3ac53af6e4 186 gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set
roger_wee 0:ce3ac53af6e4 187 gy = (float)gyroCount[1]*gRes; // - gyroBias[1];
roger_wee 0:ce3ac53af6e4 188 gz = (float)gyroCount[2]*gRes; // - gyroBias[2];
roger_wee 0:ce3ac53af6e4 189
roger_wee 0:ce3ac53af6e4 190 tempCount = mpu6050.readTempData(); // Read the x/y/z adc values
roger_wee 0:ce3ac53af6e4 191 temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
roger_wee 0:ce3ac53af6e4 192 }
roger_wee 0:ce3ac53af6e4 193
roger_wee 0:ce3ac53af6e4 194 Now = t.read_us();
roger_wee 0:ce3ac53af6e4 195 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
roger_wee 0:ce3ac53af6e4 196 lastUpdate = Now;
roger_wee 0:ce3ac53af6e4 197
roger_wee 0:ce3ac53af6e4 198 sum += deltat;
roger_wee 0:ce3ac53af6e4 199 sumCount++;
roger_wee 0:ce3ac53af6e4 200
roger_wee 0:ce3ac53af6e4 201 if(lastUpdate - firstUpdate > 10000000.0f) {
roger_wee 0:ce3ac53af6e4 202 beta = 0.04; // decrease filter gain after stabilized
roger_wee 0:ce3ac53af6e4 203 zeta = 0.015; // increasey bias drift gain after stabilized
roger_wee 0:ce3ac53af6e4 204 }
roger_wee 0:ce3ac53af6e4 205
roger_wee 0:ce3ac53af6e4 206 // Pass gyro rate as rad/s
roger_wee 0:ce3ac53af6e4 207 mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
roger_wee 0:ce3ac53af6e4 208
roger_wee 0:ce3ac53af6e4 209 // Serial print and/or display at 0.5 s rate independent of data rates
roger_wee 0:ce3ac53af6e4 210 delt_t = t.read_ms() - count;
roger_wee 0:ce3ac53af6e4 211
roger_wee 0:ce3ac53af6e4 212 // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
roger_wee 0:ce3ac53af6e4 213 // In this coordinate system, the positive z-axis is down toward Earth.
roger_wee 0:ce3ac53af6e4 214 // 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.
roger_wee 0:ce3ac53af6e4 215 // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
roger_wee 0:ce3ac53af6e4 216 // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
roger_wee 0:ce3ac53af6e4 217 // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
roger_wee 0:ce3ac53af6e4 218 // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
roger_wee 0:ce3ac53af6e4 219 // applied in the correct order which for this configuration is yaw, pitch, and then roll.
roger_wee 0:ce3ac53af6e4 220 // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
roger_wee 0:ce3ac53af6e4 221 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]);
roger_wee 0:ce3ac53af6e4 222 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
roger_wee 0:ce3ac53af6e4 223 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]);
roger_wee 0:ce3ac53af6e4 224 pitch *= 180.0f / PI;
roger_wee 0:ce3ac53af6e4 225 yaw *= 180.0f / PI;
roger_wee 0:ce3ac53af6e4 226 roll *= 180.0f / PI;
roger_wee 0:ce3ac53af6e4 227
roger_wee 0:ce3ac53af6e4 228 //update timer for filter
roger_wee 0:ce3ac53af6e4 229 count = t.read_ms();
roger_wee 0:ce3ac53af6e4 230 sum = 0;
roger_wee 0:ce3ac53af6e4 231 sumCount = 0;
roger_wee 0:ce3ac53af6e4 232
roger_wee 0:ce3ac53af6e4 233 }
roger_wee 0:ce3ac53af6e4 234
roger_wee 0:ce3ac53af6e4 235