航空研究会 / Mbed 2 deprecated gy-91_tset

a

Dependencies:   mbed BMP280 MPU9250

main.cpp@0:16eae2d34f40, 2020-02-16 (annotated)

Committer:
imanomadao
Date:
Sun Feb 16 07:52:53 2020 +0000
Revision:
0:16eae2d34f40
a; ;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
imanomadao 0:16eae2d34f40 1 #include"mbed.h"
imanomadao 0:16eae2d34f40 2 #include"MPU9255.h"
imanomadao 0:16eae2d34f40 3
imanomadao 0:16eae2d34f40 4 float sum = 0;
imanomadao 0:16eae2d34f40 5 uint32_t sumCount = 0;
imanomadao 0:16eae2d34f40 6
imanomadao 0:16eae2d34f40 7 MPU9255 mpu9255;
imanomadao 0:16eae2d34f40 8
imanomadao 0:16eae2d34f40 9 Timer t;
imanomadao 0:16eae2d34f40 10
imanomadao 0:16eae2d34f40 11 RawSerial pc(USBTX, USBRX, 9600);
imanomadao 0:16eae2d34f40 12
imanomadao 0:16eae2d34f40 13
imanomadao 0:16eae2d34f40 14 int main(void)
imanomadao 0:16eae2d34f40 15 {
imanomadao 0:16eae2d34f40 16 uint8_t whoami = mpu9255.readByte(MPU9255_ADDRESS, WHO_AM_I_MPU9255); // Read WHO_AM_I register for MPU-9255
imanomadao 0:16eae2d34f40 17 pc.printf("I AM 0x%x\n\r", whoami);
imanomadao 0:16eae2d34f40 18 pc.printf("I SHOULD BE 0x73\n\r"); //if you use mpu9255, it should be 0x73
imanomadao 0:16eae2d34f40 19
imanomadao 0:16eae2d34f40 20 if (whoami == 0x73) // WHO_AM_I should always be 0x68
imanomadao 0:16eae2d34f40 21 {
imanomadao 0:16eae2d34f40 22 pc.printf("MPU9255 is online...\n\r");
imanomadao 0:16eae2d34f40 23 wait(1);
imanomadao 0:16eae2d34f40 24
imanomadao 0:16eae2d34f40 25
imanomadao 0:16eae2d34f40 26 mpu9255.resetMPU9255(); // Reset registers to default in preparation for device calibration
imanomadao 0:16eae2d34f40 27 mpu9255.initMPU9255(); // Initialize MPU925
imanomadao 0:16eae2d34f40 28 pc.printf("MPU9255 initialized for active data mode....\n\r");
imanomadao 0:16eae2d34f40 29 mpu9255.calibrateMPU9255(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
imanomadao 0:16eae2d34f40 30 pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
imanomadao 0:16eae2d34f40 31 pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
imanomadao 0:16eae2d34f40 32 pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
imanomadao 0:16eae2d34f40 33 pc.printf("x accel bias = %f\n\r", accelBias[0]);
imanomadao 0:16eae2d34f40 34 pc.printf("y accel bias = %f\n\r", accelBias[1]);
imanomadao 0:16eae2d34f40 35 pc.printf("z accel bias = %f\n\r", accelBias[2]);
imanomadao 0:16eae2d34f40 36 wait(2);
imanomadao 0:16eae2d34f40 37 //mpu9255.initMPU9255();
imanomadao 0:16eae2d34f40 38 //pc.printf("MPU9255 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
imanomadao 0:16eae2d34f40 39 mpu9255.resetAK8963();
imanomadao 0:16eae2d34f40 40 mpu9255.initAK8963(magCalibration);
imanomadao 0:16eae2d34f40 41 pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
imanomadao 0:16eae2d34f40 42 pc.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale));
imanomadao 0:16eae2d34f40 43 pc.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale));
imanomadao 0:16eae2d34f40 44 if(Mscale == 0) pc.printf("Magnetometer resolution = 14 bits\n\r");
imanomadao 0:16eae2d34f40 45 if(Mscale == 1) pc.printf("Magnetometer resolution = 16 bits\n\r");
imanomadao 0:16eae2d34f40 46 if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
imanomadao 0:16eae2d34f40 47 if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
imanomadao 0:16eae2d34f40 48 wait(2);
imanomadao 0:16eae2d34f40 49 }
imanomadao 0:16eae2d34f40 50 else
imanomadao 0:16eae2d34f40 51 {
imanomadao 0:16eae2d34f40 52 pc.printf("Could not connect to MPU9255: \n\r");
imanomadao 0:16eae2d34f40 53 pc.printf("%#x \n", whoami);
imanomadao 0:16eae2d34f40 54
imanomadao 0:16eae2d34f40 55
imanomadao 0:16eae2d34f40 56
imanomadao 0:16eae2d34f40 57 while(1) ; // Loop forever if communication doesn't happen
imanomadao 0:16eae2d34f40 58 }
imanomadao 0:16eae2d34f40 59
imanomadao 0:16eae2d34f40 60 mpu9255.getAres(); // Get accelerometer sensitivity
imanomadao 0:16eae2d34f40 61 mpu9255.getGres(); // Get gyro sensitivity
imanomadao 0:16eae2d34f40 62 mpu9255.getMres(); // Get magnetometer sensitivity
imanomadao 0:16eae2d34f40 63 pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
imanomadao 0:16eae2d34f40 64 pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
imanomadao 0:16eae2d34f40 65 pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
imanomadao 0:16eae2d34f40 66 magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated
imanomadao 0:16eae2d34f40 67 magbias[1] = +120.; // User environmental x-axis correction in milliGauss
imanomadao 0:16eae2d34f40 68 magbias[2] = +125.; // User environmental x-axis correction in milliGauss
imanomadao 0:16eae2d34f40 69
imanomadao 0:16eae2d34f40 70 while(1) {
imanomadao 0:16eae2d34f40 71
imanomadao 0:16eae2d34f40 72 // If intPin goes high, all data registers have new data
imanomadao 0:16eae2d34f40 73 if(mpu9255.readByte(MPU9255_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
imanomadao 0:16eae2d34f40 74
imanomadao 0:16eae2d34f40 75 mpu9255.readAccelData(accelCount); // Read the x/y/z adc values
imanomadao 0:16eae2d34f40 76 // Now we'll calculate the accleration value into actual g's
imanomadao 0:16eae2d34f40 77 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
imanomadao 0:16eae2d34f40 78 ay = (float)accelCount[1]*aRes - accelBias[1];
imanomadao 0:16eae2d34f40 79 az = (float)accelCount[2]*aRes - accelBias[2];
imanomadao 0:16eae2d34f40 80
imanomadao 0:16eae2d34f40 81 mpu9255.readGyroData(gyroCount); // Read the x/y/z adc values
imanomadao 0:16eae2d34f40 82 // Calculate the gyro value into actual degrees per second
imanomadao 0:16eae2d34f40 83 gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
imanomadao 0:16eae2d34f40 84 gy = (float)gyroCount[1]*gRes - gyroBias[1];
imanomadao 0:16eae2d34f40 85 gz = (float)gyroCount[2]*gRes - gyroBias[2];
imanomadao 0:16eae2d34f40 86
imanomadao 0:16eae2d34f40 87 mpu9255.readMagData(magCount); // Read the x/y/z adc values
imanomadao 0:16eae2d34f40 88 // Calculate the magnetometer values in milliGauss
imanomadao 0:16eae2d34f40 89 // Include factory calibration per data sheet and user environmental corrections
imanomadao 0:16eae2d34f40 90 mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set
imanomadao 0:16eae2d34f40 91 my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
imanomadao 0:16eae2d34f40 92 mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
imanomadao 0:16eae2d34f40 93 }
imanomadao 0:16eae2d34f40 94
imanomadao 0:16eae2d34f40 95 Now = t.read_us();
imanomadao 0:16eae2d34f40 96 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
imanomadao 0:16eae2d34f40 97 lastUpdate = Now;
imanomadao 0:16eae2d34f40 98
imanomadao 0:16eae2d34f40 99 sum += deltat;
imanomadao 0:16eae2d34f40 100 sumCount++;
imanomadao 0:16eae2d34f40 101
imanomadao 0:16eae2d34f40 102 // if(lastUpdate - firstUpdate > 10000000.0f) {
imanomadao 0:16eae2d34f40 103 // beta = 0.04; // decrease filter gain after stabilized
imanomadao 0:16eae2d34f40 104 // zeta = 0.015; // increasey bias drift gain after stabilized
imanomadao 0:16eae2d34f40 105 // }
imanomadao 0:16eae2d34f40 106
imanomadao 0:16eae2d34f40 107 // Pass gyro rate as rad/s
imanomadao 0:16eae2d34f40 108 mpu9255.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
imanomadao 0:16eae2d34f40 109 // mpu9255.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
imanomadao 0:16eae2d34f40 110
imanomadao 0:16eae2d34f40 111 // Serial print and/or display at 0.5 s rate independent of data rates
imanomadao 0:16eae2d34f40 112 delt_t = t.read_ms() - count_display;
imanomadao 0:16eae2d34f40 113 //if (delt_t > 500) { // update LCD once per half-second independent of read rate
imanomadao 0:16eae2d34f40 114 //if (t.read_ms() > 500){
imanomadao 0:16eae2d34f40 115 //while(1){
imanomadao 0:16eae2d34f40 116
imanomadao 0:16eae2d34f40 117 pc.printf("ax = %f", 1000*ax);
imanomadao 0:16eae2d34f40 118 pc.printf(" ay = %f", 1000*ay);
imanomadao 0:16eae2d34f40 119 pc.printf(" az = %f mg\n\r", 1000*az);
imanomadao 0:16eae2d34f40 120
imanomadao 0:16eae2d34f40 121 pc.printf("gx = %f", gx);
imanomadao 0:16eae2d34f40 122 pc.printf(" gy = %f", gy);
imanomadao 0:16eae2d34f40 123 pc.printf(" gz = %f deg/s\n\r", gz);
imanomadao 0:16eae2d34f40 124
imanomadao 0:16eae2d34f40 125 pc.printf("mx = %f", mx);
imanomadao 0:16eae2d34f40 126 pc.printf(" my = %f", my);
imanomadao 0:16eae2d34f40 127 pc.printf(" mz = %f mG\n\r", mz);
imanomadao 0:16eae2d34f40 128
imanomadao 0:16eae2d34f40 129 pc.printf("\r\n");
imanomadao 0:16eae2d34f40 130
imanomadao 0:16eae2d34f40 131 /*tempCount = mpu9255.readTempData(); // Read the adc values
imanomadao 0:16eae2d34f40 132 temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
imanomadao 0:16eae2d34f40 133 pc.printf(" temperature = %f C\n\r", temperature);
imanomadao 0:16eae2d34f40 134
imanomadao 0:16eae2d34f40 135 pc.printf("q0 = %f\n\r", q[0]);
imanomadao 0:16eae2d34f40 136 pc.printf("q1 = %f\n\r", q[1]);
imanomadao 0:16eae2d34f40 137 pc.printf("q2 = %f\n\r", q[2]);
imanomadao 0:16eae2d34f40 138 pc.printf("q3 = %f\n\r", q[3]);
imanomadao 0:16eae2d34f40 139
imanomadao 0:16eae2d34f40 140
imanomadao 0:16eae2d34f40 141
imanomadao 0:16eae2d34f40 142 // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
imanomadao 0:16eae2d34f40 143 // In this coordinate system, the positive z-axis is down toward Earth.
imanomadao 0:16eae2d34f40 144 // 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.
imanomadao 0:16eae2d34f40 145 // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
imanomadao 0:16eae2d34f40 146 // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
imanomadao 0:16eae2d34f40 147 // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
imanomadao 0:16eae2d34f40 148 // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
imanomadao 0:16eae2d34f40 149 // applied in the correct order which for this configuration is yaw, pitch, and then roll.
imanomadao 0:16eae2d34f40 150 // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
imanomadao 0:16eae2d34f40 151 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]);
imanomadao 0:16eae2d34f40 152 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
imanomadao 0:16eae2d34f40 153 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]);
imanomadao 0:16eae2d34f40 154 pitch *= 180.0f / PI;
imanomadao 0:16eae2d34f40 155 yaw *= 180.0f / PI;
imanomadao 0:16eae2d34f40 156 yaw -= 7.6f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
imanomadao 0:16eae2d34f40 157 roll *= 180.0f / PI;
imanomadao 0:16eae2d34f40 158
imanomadao 0:16eae2d34f40 159 pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
imanomadao 0:16eae2d34f40 160 pc.printf("average rate = %f\n\r", (float) sumCount/sum);
imanomadao 0:16eae2d34f40 161
imanomadao 0:16eae2d34f40 162 myled= !myled;
imanomadao 0:16eae2d34f40 163 count_display = t.read_ms();
imanomadao 0:16eae2d34f40 164 sum = 0;
imanomadao 0:16eae2d34f40 165 sumCount = 0; */
imanomadao 0:16eae2d34f40 166
imanomadao 0:16eae2d34f40 167 wait_ms(2000);
imanomadao 0:16eae2d34f40 168 //}
imanomadao 0:16eae2d34f40 169 }
imanomadao 0:16eae2d34f40 170 }
imanomadao 0:16eae2d34f40 171