kor bork wa cop koy ma
Fork of testIMU2_copy2 by
Embed:
(wiki syntax)
Show/hide line numbers
zmu9250.h
00001 #include "mbed.h" 00002 #include "MPU9250.h" 00003 #include "math.h" 00004 #include "kalman.h" 00005 00006 Serial aa(USBTX,USBRX); 00007 00008 00009 class ZMU9250 00010 { 00011 public: 00012 ZMU9250() 00013 { 00014 00015 //Set up I2C 00016 i2c.frequency(400000); // use fast (400 kHz) I2C 00017 this->t.start(); 00018 00019 // Read the WHO_AM_I register, this is a good test of communication 00020 uint8_t whoami = this->mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250 00021 if ((whoami == 0x71)||(whoami == 0x73)) // WHO_AM_I should always be 0x68 00022 { 00023 wait(1); 00024 this->mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration 00025 this->mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values 00026 this->mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers 00027 wait(2); 00028 this->mpu9250.initMPU9250(); 00029 this->mpu9250.initAK8963(magCalibration); 00030 wait(1); 00031 } 00032 else 00033 { 00034 while(1) ; // Loop forever if communication doesn't happen 00035 } 00036 this->mpu9250.getAres(); // Get accelerometer sensitivity 00037 this->mpu9250.getGres(); // Get gyro sensitivity 00038 this->mpu9250.getMres(); // Get magnetometer sensitivity 00039 //magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated 00040 //magbias[1] = +120.; // User environmental x-axis correction in milliGauss 00041 //magbias[2] = +125.; // User environmental x-axis correction in milliGauss 00042 magbias[0] = +470; // User environmental x-axis correction in milliGauss, should be automatically calculated 00043 magbias[1] = +120; // User environmental x-axis correction in milliGauss 00044 magbias[2] = +125; // User environmental x-axis correction in milliGauss 00045 } 00046 00047 void Update() 00048 { 00049 if(this->mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt 00050 this->mpu9250.readAccelData(accelCount); // Read the x/y/z adc values 00051 // Now we'll calculate the accleration value into actual g's 00052 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set 00053 ay = (float)accelCount[1]*aRes - accelBias[1]; 00054 az = (float)accelCount[2]*aRes - accelBias[2]; 00055 this->mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values 00056 // Calculate the gyro value into actual degrees per second 00057 gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set 00058 gy = (float)gyroCount[1]*gRes - gyroBias[1]; 00059 gz = (float)gyroCount[2]*gRes - gyroBias[2]; 00060 this->mpu9250.readMagData(magCount); // Read the x/y/z adc values 00061 // Calculate the magnetometer values in milliGauss 00062 // Include factory calibration per data sheet and user environmental corrections 00063 mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]+360.0f; // get actual magnetometer value, this depends on scale being set 00064 my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1]-210.0f; 00065 mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2]; 00066 //aa.printf("x %f\ty %f\tz %f\n",mx,my,mz); 00067 00068 00069 } // end if one 00070 Now = this->t.read_us(); 00071 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update 00072 lastUpdate = Now; 00073 this->sum += deltat; 00074 sumCount++; 00075 this->mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz); 00076 00077 // Pass gyro rate as rad/s 00078 /*if((rand()%20)>=0) 00079 { 00080 this->mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz); 00081 }else 00082 { 00083 //this->mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz); 00084 this->mpu9250.Mad_Update(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f); 00085 }*/ 00086 00087 00088 // Serial print and/or display at 0.5 s rate independent of data rates 00089 delt_t = this->t.read_ms() - count; 00090 if (delt_t > 10) { // update LCD once per half-second independent of read rate 00091 tempCount = this->mpu9250.readTempData(); // Read the adc values 00092 temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade 00093 // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation. 00094 // In this coordinate system, the positive z-axis is down toward Earth. 00095 // 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. 00096 // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative. 00097 // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll. 00098 // These arise from the definition of the homogeneous rotation matrix constructed from quaternions. 00099 // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be 00100 // applied in the correct order which for this configuration is yaw, pitch, and then roll. 00101 // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links. 00102 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]); 00103 //yaw = atan2(2.0f * (q[0] * q[2] + q[0] * q[3]), 1 - 2 * ( q[2] * q[2] + q[3] * q[3])); 00104 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); 00105 00106 //pitch = atan2(2.0f * (q[1] * q[3] - q[0] * q[2]),q[0]*q[0]-q[1]*q[1]+q[2]*q[2]-q[3]*q[3]); 00107 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]); 00108 //pitch = atan2(sin(roll)*(q[1]*q[3]-q[0]*q[2]),q[1]*q[2]+q[0]*q[3]); 00109 pitch *= 180.0f / PI; 00110 yaw *= 180.0f / PI; 00111 //yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04 00112 yaw -= 0.35f; 00113 roll *= 180.0f / PI; 00114 this->roll_x = roll; 00115 this->pitch_y = pitch; 00116 this->yaw_z = yaw;//(this->kal.getAngle(yaw*PI/180.0f,0.00,delt_t)); 00117 count = this->t.read_ms(); 00118 if(count > 1<<21) { 00119 this->t.start(); // start the timer over again if ~30 minutes has passed 00120 count = 0; 00121 deltat= 0; 00122 lastUpdate = this->t.read_us(); 00123 } // end if three. 00124 this->sum = 0; 00125 sumCount = 0; 00126 } // end if two. 00127 } 00128 00129 00130 float Roll() 00131 { 00132 return roll_x; 00133 } 00134 00135 float Pitch() 00136 { 00137 return pitch_y; 00138 } 00139 00140 float Yaw() 00141 { 00142 return yaw_z; 00143 } 00144 00145 00146 private: 00147 float sum; 00148 uint32_t sumCount; 00149 char buffer[14]; 00150 int roll_x; 00151 kalman kal(); 00152 int pitch_y; 00153 int yaw_z; 00154 MPU9250 mpu9250; 00155 Timer t; 00156 00157 00158 }; 00159 00160
Generated on Sun Jul 17 2022 00:07:07 by
1.7.2
