Yaroslav Barabanov / Mbed 2 deprecated turret_2017

для управления турелью

Dependencies:   mbed

mpu6050.cpp@3:e47c0c98f515, 2017-01-19 (annotated)

Committer:
Yar
Date:
Thu Jan 19 05:22:19 2017 +0000
Revision:
3:e47c0c98f515
MPU6050

Who changed what in which revision?

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