Kalibrasi AVRG

Dependencies:   mbed

Committer:
adavidkhowantolim
Date:
Mon Oct 29 10:56:02 2018 +0000
Revision:
6:b970c0e20295
Parent:
4:cd99bf0e7502
Child:
7:d4fd89e4f19d
MNTAP 29/10/18 - EGI;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
onehorse 0:2e5e65a6fb30 1 /* MPU9250 Basic Example Code
onehorse 0:2e5e65a6fb30 2 by: Kris Winer
onehorse 0:2e5e65a6fb30 3 date: April 1, 2014
hisyamfs 3:bf624b3ae5b9 4 license: Beerware - Use this code however you'd like. If you
onehorse 0:2e5e65a6fb30 5 find it useful you can buy me a beer some time.
hisyamfs 3:bf624b3ae5b9 6
hisyamfs 3:bf624b3ae5b9 7 Demonstrate basic MPU-9250 functionality including parameterizing the register addresses, initializing the sensor,
hisyamfs 3:bf624b3ae5b9 8 getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to
hisyamfs 3:bf624b3ae5b9 9 allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and
onehorse 0:2e5e65a6fb30 10 Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1.
hisyamfs 3:bf624b3ae5b9 11
onehorse 0:2e5e65a6fb30 12 SDA and SCL should have external pull-up resistors (to 3.3V).
onehorse 0:2e5e65a6fb30 13 10k resistors are on the EMSENSR-9250 breakout board.
hisyamfs 3:bf624b3ae5b9 14
onehorse 0:2e5e65a6fb30 15 Hardware setup:
onehorse 0:2e5e65a6fb30 16 MPU9250 Breakout --------- Arduino
onehorse 0:2e5e65a6fb30 17 VDD ---------------------- 3.3V
onehorse 0:2e5e65a6fb30 18 VDDI --------------------- 3.3V
onehorse 0:2e5e65a6fb30 19 SDA ----------------------- A4
onehorse 0:2e5e65a6fb30 20 SCL ----------------------- A5
onehorse 0:2e5e65a6fb30 21 GND ---------------------- GND
hisyamfs 3:bf624b3ae5b9 22
hisyamfs 3:bf624b3ae5b9 23 Note: The MPU9250 is an I2C sensor and uses the Arduino Wire library.
onehorse 0:2e5e65a6fb30 24 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.
onehorse 0:2e5e65a6fb30 25 We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file.
onehorse 0:2e5e65a6fb30 26 We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ to 400000L /twi.h utility file.
onehorse 0:2e5e65a6fb30 27 */
hisyamfs 3:bf624b3ae5b9 28
hisyamfs 3:bf624b3ae5b9 29
onehorse 0:2e5e65a6fb30 30 #include "mbed.h"
onehorse 0:2e5e65a6fb30 31 #include "MPU9250.h"
onehorse 0:2e5e65a6fb30 32
onehorse 0:2e5e65a6fb30 33 float sum = 0;
onehorse 0:2e5e65a6fb30 34 uint32_t sumCount = 0;
onehorse 0:2e5e65a6fb30 35 char buffer[14];
onehorse 0:2e5e65a6fb30 36
hisyamfs 3:bf624b3ae5b9 37 MPU9250 mpu9250;
hisyamfs 3:bf624b3ae5b9 38 Timer t;
hisyamfs 3:bf624b3ae5b9 39 Serial pc(USBTX, USBRX); // tx, rx
onehorse 0:2e5e65a6fb30 40
onehorse 0:2e5e65a6fb30 41 int main()
onehorse 0:2e5e65a6fb30 42 {
hisyamfs 3:bf624b3ae5b9 43 pc.baud(9600);
adavidkhowantolim 6:b970c0e20295 44 // pc.printf("start");
hisyamfs 3:bf624b3ae5b9 45 //Set up I2C
hisyamfs 3:bf624b3ae5b9 46 i2c.frequency(400000); // use fast (400 kHz) I2C
adavidkhowantolim 6:b970c0e20295 47 // pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
hisyamfs 3:bf624b3ae5b9 48 t.start();
hisyamfs 3:bf624b3ae5b9 49 // lcd.init();
onehorse 0:2e5e65a6fb30 50 // lcd.setBrightness(0.05);
hisyamfs 3:bf624b3ae5b9 51 // Read the WHO_AM_I register, this is a good test of communication
hisyamfs 3:bf624b3ae5b9 52 uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
adavidkhowantolim 6:b970c0e20295 53 // pc.printf("I AM 0x%x\n\r", whoami);
adavidkhowantolim 6:b970c0e20295 54 // pc.printf("I SHOULD BE 0x73\n\r");
hisyamfs 3:bf624b3ae5b9 55
hisyamfs 3:bf624b3ae5b9 56 if (whoami == 0x73) { // WHO_AM_I should always be 0x68
adavidkhowantolim 6:b970c0e20295 57 // pc.printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami);
adavidkhowantolim 6:b970c0e20295 58 // pc.printf("MPU9250 is online...\n\r");
adavidkhowantolim 4:cd99bf0e7502 59 /*
hisyamfs 3:bf624b3ae5b9 60 // lcd.clear();
hisyamfs 3:bf624b3ae5b9 61 // lcd.printString("MPU9250 is", 0, 0);
hisyamfs 3:bf624b3ae5b9 62 sprintf(buffer, "0x%x", whoami);
hisyamfs 3:bf624b3ae5b9 63 // lcd.printString(buffer, 0, 1);
hisyamfs 3:bf624b3ae5b9 64 // lcd.printString("shoud be 0x71", 0, 2);
hisyamfs 3:bf624b3ae5b9 65 wait(1);
hisyamfs 3:bf624b3ae5b9 66
hisyamfs 3:bf624b3ae5b9 67 mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
hisyamfs 3:bf624b3ae5b9 68 mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
hisyamfs 3:bf624b3ae5b9 69 pc.printf("x-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[0]);
hisyamfs 3:bf624b3ae5b9 70 pc.printf("y-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[1]);
hisyamfs 3:bf624b3ae5b9 71 pc.printf("z-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[2]);
hisyamfs 3:bf624b3ae5b9 72 pc.printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]);
hisyamfs 3:bf624b3ae5b9 73 pc.printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]);
hisyamfs 3:bf624b3ae5b9 74 pc.printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]);
hisyamfs 3:bf624b3ae5b9 75 mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
hisyamfs 3:bf624b3ae5b9 76 pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
hisyamfs 3:bf624b3ae5b9 77 pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
hisyamfs 3:bf624b3ae5b9 78 pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
hisyamfs 3:bf624b3ae5b9 79 pc.printf("x accel bias = %f\n\r", accelBias[0]);
hisyamfs 3:bf624b3ae5b9 80 pc.printf("y accel bias = %f\n\r", accelBias[1]);
hisyamfs 3:bf624b3ae5b9 81 pc.printf("z accel bias = %f\n\r", accelBias[2]);
hisyamfs 3:bf624b3ae5b9 82 wait(2);
adavidkhowantolim 4:cd99bf0e7502 83 */
hisyamfs 3:bf624b3ae5b9 84 mpu9250.initMPU9250();
adavidkhowantolim 6:b970c0e20295 85 // pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
hisyamfs 3:bf624b3ae5b9 86 mpu9250.initAK8963(magCalibration);
adavidkhowantolim 6:b970c0e20295 87 // pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
adavidkhowantolim 6:b970c0e20295 88 // pc.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale));
adavidkhowantolim 6:b970c0e20295 89 // pc.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale));
adavidkhowantolim 6:b970c0e20295 90 // if(Mscale == 0) pc.printf("Magnetometer resolution = 14 bits\n\r");
adavidkhowantolim 6:b970c0e20295 91 // if(Mscale == 1) pc.printf("Magnetometer resolution = 16 bits\n\r");
adavidkhowantolim 6:b970c0e20295 92 // if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
adavidkhowantolim 6:b970c0e20295 93 // if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
adavidkhowantolim 6:b970c0e20295 94 // wait(1);
adavidkhowantolim 6:b970c0e20295 95 } else {
adavidkhowantolim 6:b970c0e20295 96 // pc.printf("Could not connect to MPU9250: \n\r");
adavidkhowantolim 6:b970c0e20295 97 // pc.printf("%#x \n", whoami);
hisyamfs 3:bf624b3ae5b9 98
hisyamfs 3:bf624b3ae5b9 99 // lcd.clear();
hisyamfs 3:bf624b3ae5b9 100 // lcd.printString("MPU9250", 0, 0);
hisyamfs 3:bf624b3ae5b9 101 // lcd.printString("no connection", 0, 1);
hisyamfs 3:bf624b3ae5b9 102 sprintf(buffer, "WHO_AM_I 0x%x", whoami);
hisyamfs 3:bf624b3ae5b9 103 // lcd.printString(buffer, 0, 2);
hisyamfs 3:bf624b3ae5b9 104
hisyamfs 3:bf624b3ae5b9 105 while(1) ; // Loop forever if communication doesn't happen
onehorse 0:2e5e65a6fb30 106 }
onehorse 0:2e5e65a6fb30 107
onehorse 0:2e5e65a6fb30 108 mpu9250.getAres(); // Get accelerometer sensitivity
onehorse 0:2e5e65a6fb30 109 mpu9250.getGres(); // Get gyro sensitivity
onehorse 0:2e5e65a6fb30 110 mpu9250.getMres(); // Get magnetometer sensitivity
adavidkhowantolim 6:b970c0e20295 111 // pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
adavidkhowantolim 6:b970c0e20295 112 // pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
adavidkhowantolim 6:b970c0e20295 113 // pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
onehorse 0:2e5e65a6fb30 114 magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated
onehorse 0:2e5e65a6fb30 115 magbias[1] = +120.; // User environmental x-axis correction in milliGauss
onehorse 0:2e5e65a6fb30 116 magbias[2] = +125.; // User environmental x-axis correction in milliGauss
onehorse 0:2e5e65a6fb30 117
hisyamfs 3:bf624b3ae5b9 118 while(1) {
hisyamfs 3:bf624b3ae5b9 119 // If intPin goes high, all data registers have new data
hisyamfs 3:bf624b3ae5b9 120 if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
hisyamfs 3:bf624b3ae5b9 121 mpu9250.readAccelData(accelCount); // Read the x/y/z adc values
hisyamfs 3:bf624b3ae5b9 122 // Now we'll calculate the accleration value into actual g's
hisyamfs 3:bf624b3ae5b9 123 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
hisyamfs 3:bf624b3ae5b9 124 ay = (float)accelCount[1]*aRes - accelBias[1];
hisyamfs 3:bf624b3ae5b9 125 az = (float)accelCount[2]*aRes - accelBias[2];
hisyamfs 3:bf624b3ae5b9 126 mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values
hisyamfs 3:bf624b3ae5b9 127 // Calculate the gyro value into actual degrees per second
hisyamfs 3:bf624b3ae5b9 128 gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
hisyamfs 3:bf624b3ae5b9 129 gy = (float)gyroCount[1]*gRes - gyroBias[1];
hisyamfs 3:bf624b3ae5b9 130 gz = (float)gyroCount[2]*gRes - gyroBias[2];
hisyamfs 3:bf624b3ae5b9 131 mpu9250.readMagData(magCount); // Read the x/y/z adc values
hisyamfs 3:bf624b3ae5b9 132 // Calculate the magnetometer values in milliGauss
hisyamfs 3:bf624b3ae5b9 133 // Include factory calibration per data sheet and user environmental corrections
adavidkhowantolim 6:b970c0e20295 134 mx = (((float)magCount[0]*mRes*magCalibration[0] - magbias[0])+ 348.056595)*1.013699505; // get actual magnetometer value, this depends on scale being set
adavidkhowantolim 6:b970c0e20295 135 my = (((float)magCount[1]*mRes*magCalibration[1] - magbias[1])+ 129.145752)*0.992905726;
adavidkhowantolim 6:b970c0e20295 136 mz = (((float)magCount[2]*mRes*magCalibration[2] - magbias[2])+365.9966815)* 0.99367091;
adavidkhowantolim 6:b970c0e20295 137
hisyamfs 3:bf624b3ae5b9 138 }
onehorse 0:2e5e65a6fb30 139
hisyamfs 3:bf624b3ae5b9 140 Now = t.read_us();
hisyamfs 3:bf624b3ae5b9 141 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
hisyamfs 3:bf624b3ae5b9 142 lastUpdate = Now;
hisyamfs 3:bf624b3ae5b9 143
hisyamfs 3:bf624b3ae5b9 144 sum += deltat;
hisyamfs 3:bf624b3ae5b9 145 sumCount++;
hisyamfs 3:bf624b3ae5b9 146
onehorse 0:2e5e65a6fb30 147 // if(lastUpdate - firstUpdate > 10000000.0f) {
onehorse 0:2e5e65a6fb30 148 // beta = 0.04; // decrease filter gain after stabilized
onehorse 0:2e5e65a6fb30 149 // zeta = 0.015; // increasey bias drift gain after stabilized
hisyamfs 3:bf624b3ae5b9 150 // }
hisyamfs 3:bf624b3ae5b9 151
hisyamfs 3:bf624b3ae5b9 152 // Pass gyro rate as rad/s
hisyamfs 3:bf624b3ae5b9 153 mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
hisyamfs 3:bf624b3ae5b9 154 // mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
hisyamfs 3:bf624b3ae5b9 155
hisyamfs 3:bf624b3ae5b9 156 // Serial print and/or display at 2 s rate independent of data rates
hisyamfs 3:bf624b3ae5b9 157 delt_t = t.read_ms() - count;
adavidkhowantolim 4:cd99bf0e7502 158 if (delt_t > 1000) { // update LCD once per half-second independent of read rate
adavidkhowantolim 4:cd99bf0e7502 159 /*
hisyamfs 3:bf624b3ae5b9 160 pc.printf("ax = %f", 1000*ax);
hisyamfs 3:bf624b3ae5b9 161 pc.printf(" ay = %f", 1000*ay);
hisyamfs 3:bf624b3ae5b9 162 pc.printf(" az = %f mg\n\r", 1000*az);
hisyamfs 3:bf624b3ae5b9 163
hisyamfs 3:bf624b3ae5b9 164 pc.printf("gx = %f", gx);
hisyamfs 3:bf624b3ae5b9 165 pc.printf(" gy = %f", gy);
hisyamfs 3:bf624b3ae5b9 166 pc.printf(" gz = %f deg/s\n\r", gz);
adavidkhowantolim 4:cd99bf0e7502 167 */
adavidkhowantolim 6:b970c0e20295 168 // pc.printf("mx = %f", mx);
adavidkhowantolim 6:b970c0e20295 169 // pc.printf(" my = %f", my);
adavidkhowantolim 6:b970c0e20295 170 // pc.printf(" mz = %f mG\n\r", mz);
adavidkhowantolim 6:b970c0e20295 171 pc.printf("%f\t%f\t%f\n", mx , my , mz);
adavidkhowantolim 4:cd99bf0e7502 172 /*
hisyamfs 3:bf624b3ae5b9 173 tempCount = mpu9250.readTempData(); // Read the adc values
hisyamfs 3:bf624b3ae5b9 174 temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
hisyamfs 3:bf624b3ae5b9 175 pc.printf(" temperature = %f C\n\r", temperature);
onehorse 0:2e5e65a6fb30 176
hisyamfs 3:bf624b3ae5b9 177 pc.printf("q0 = %f\n\r", q[0]);
hisyamfs 3:bf624b3ae5b9 178 pc.printf("q1 = %f\n\r", q[1]);
hisyamfs 3:bf624b3ae5b9 179 pc.printf("q2 = %f\n\r", q[2]);
hisyamfs 3:bf624b3ae5b9 180 pc.printf("q3 = %f\n\r", q[3]);
adavidkhowantolim 4:cd99bf0e7502 181 */
hisyamfs 3:bf624b3ae5b9 182 /* lcd.clear();
hisyamfs 3:bf624b3ae5b9 183 lcd.printString("MPU9250", 0, 0);
hisyamfs 3:bf624b3ae5b9 184 lcd.printString("x y z", 0, 1);
hisyamfs 3:bf624b3ae5b9 185 sprintf(buffer, "%d %d %d mg", (int)(1000.0f*ax), (int)(1000.0f*ay), (int)(1000.0f*az));
hisyamfs 3:bf624b3ae5b9 186 lcd.printString(buffer, 0, 2);
hisyamfs 3:bf624b3ae5b9 187 sprintf(buffer, "%d %d %d deg/s", (int)gx, (int)gy, (int)gz);
hisyamfs 3:bf624b3ae5b9 188 lcd.printString(buffer, 0, 3);
hisyamfs 3:bf624b3ae5b9 189 sprintf(buffer, "%d %d %d mG", (int)mx, (int)my, (int)mz);
hisyamfs 3:bf624b3ae5b9 190 lcd.printString(buffer, 0, 4);
hisyamfs 3:bf624b3ae5b9 191 */
hisyamfs 3:bf624b3ae5b9 192 // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
hisyamfs 3:bf624b3ae5b9 193 // In this coordinate system, the positive z-axis is down toward Earth.
hisyamfs 3:bf624b3ae5b9 194 // 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.
hisyamfs 3:bf624b3ae5b9 195 // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
hisyamfs 3:bf624b3ae5b9 196 // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
hisyamfs 3:bf624b3ae5b9 197 // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
hisyamfs 3:bf624b3ae5b9 198 // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
hisyamfs 3:bf624b3ae5b9 199 // applied in the correct order which for this configuration is yaw, pitch, and then roll.
hisyamfs 3:bf624b3ae5b9 200 // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
hisyamfs 3:bf624b3ae5b9 201 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]);
hisyamfs 3:bf624b3ae5b9 202 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
hisyamfs 3:bf624b3ae5b9 203 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]);
hisyamfs 3:bf624b3ae5b9 204 pitch *= 180.0f / PI;
hisyamfs 3:bf624b3ae5b9 205 yaw *= 180.0f / PI;
hisyamfs 3:bf624b3ae5b9 206 yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
hisyamfs 3:bf624b3ae5b9 207 roll *= 180.0f / PI;
hisyamfs 3:bf624b3ae5b9 208
adavidkhowantolim 6:b970c0e20295 209 pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
adavidkhowantolim 6:b970c0e20295 210 // pc.printf("average rate = %f\n\r", (float) sumCount/sum);
onehorse 0:2e5e65a6fb30 211 // sprintf(buffer, "YPR: %f %f %f", yaw, pitch, roll);
onehorse 0:2e5e65a6fb30 212 // lcd.printString(buffer, 0, 4);
onehorse 0:2e5e65a6fb30 213 // sprintf(buffer, "rate = %f", (float) sumCount/sum);
onehorse 0:2e5e65a6fb30 214 // lcd.printString(buffer, 0, 5);
hisyamfs 3:bf624b3ae5b9 215
hisyamfs 3:bf624b3ae5b9 216 myled= !myled;
hisyamfs 3:bf624b3ae5b9 217 count = t.read_ms();
onehorse 0:2e5e65a6fb30 218
hisyamfs 3:bf624b3ae5b9 219 if(count > 1<<21) {
hisyamfs 3:bf624b3ae5b9 220 t.start(); // start the timer over again if ~30 minutes has passed
hisyamfs 3:bf624b3ae5b9 221 count = 0;
hisyamfs 3:bf624b3ae5b9 222 deltat= 0;
hisyamfs 3:bf624b3ae5b9 223 lastUpdate = t.read_us();
hisyamfs 3:bf624b3ae5b9 224 }
hisyamfs 3:bf624b3ae5b9 225 sum = 0;
hisyamfs 3:bf624b3ae5b9 226 sumCount = 0;
hisyamfs 3:bf624b3ae5b9 227 }
onehorse 0:2e5e65a6fb30 228 }
hisyamfs 3:bf624b3ae5b9 229
hisyamfs 3:bf624b3ae5b9 230 }