OX / Mbed 2 deprecated class_IMU_V1

test

Dependencies:   mbed

zmu9250.h@0:dd400e4fe461, 2016-12-05 (annotated)

Committer:
siwakon
Date:
Mon Dec 05 14:47:17 2016 +0000
Revision:
0:dd400e4fe461
asdasdasd

Who changed what in which revision?

UserRevisionLine numberNew contents of line
siwakon 0:dd400e4fe461 1 #include "mbed.h"
siwakon 0:dd400e4fe461 2 #include "MPU9250.h"
siwakon 0:dd400e4fe461 3 #include "math.h"
siwakon 0:dd400e4fe461 4 #include "kalman.h"
siwakon 0:dd400e4fe461 5
siwakon 0:dd400e4fe461 6 Serial aa(USBTX,USBRX);
siwakon 0:dd400e4fe461 7
siwakon 0:dd400e4fe461 8
siwakon 0:dd400e4fe461 9 class ZMU9250
siwakon 0:dd400e4fe461 10 {
siwakon 0:dd400e4fe461 11 public:
siwakon 0:dd400e4fe461 12 ZMU9250()
siwakon 0:dd400e4fe461 13 {
siwakon 0:dd400e4fe461 14
siwakon 0:dd400e4fe461 15 //Set up I2C
siwakon 0:dd400e4fe461 16 i2c.frequency(400000); // use fast (400 kHz) I2C
siwakon 0:dd400e4fe461 17 this->t.start();
siwakon 0:dd400e4fe461 18
siwakon 0:dd400e4fe461 19 // Read the WHO_AM_I register, this is a good test of communication
siwakon 0:dd400e4fe461 20 uint8_t whoami = this->mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
siwakon 0:dd400e4fe461 21 if ((whoami == 0x73)||(whoami == 0x71)) // WHO_AM_I should always be 0x68
siwakon 0:dd400e4fe461 22 {
siwakon 0:dd400e4fe461 23 wait(1);
siwakon 0:dd400e4fe461 24 this->mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
siwakon 0:dd400e4fe461 25 this->mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
siwakon 0:dd400e4fe461 26 this->mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
siwakon 0:dd400e4fe461 27 wait(2);
siwakon 0:dd400e4fe461 28 this->mpu9250.initMPU9250();
siwakon 0:dd400e4fe461 29 this->mpu9250.initAK8963(magCalibration);
siwakon 0:dd400e4fe461 30 wait(1);
siwakon 0:dd400e4fe461 31 }
siwakon 0:dd400e4fe461 32 else
siwakon 0:dd400e4fe461 33 {
siwakon 0:dd400e4fe461 34 while(1) ; // Loop forever if communication doesn't happen
siwakon 0:dd400e4fe461 35 }
siwakon 0:dd400e4fe461 36 this->mpu9250.getAres(); // Get accelerometer sensitivity
siwakon 0:dd400e4fe461 37 this->mpu9250.getGres(); // Get gyro sensitivity
siwakon 0:dd400e4fe461 38 this->mpu9250.getMres(); // Get magnetometer sensitivity
siwakon 0:dd400e4fe461 39 //magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated
siwakon 0:dd400e4fe461 40 //magbias[1] = +120.; // User environmental x-axis correction in milliGauss
siwakon 0:dd400e4fe461 41 //magbias[2] = +125.; // User environmental x-axis correction in milliGauss
siwakon 0:dd400e4fe461 42 magbias[0] = +470; // User environmental x-axis correction in milliGauss, should be automatically calculated
siwakon 0:dd400e4fe461 43 magbias[1] = +120; // User environmental x-axis correction in milliGauss
siwakon 0:dd400e4fe461 44 magbias[2] = +125; // User environmental x-axis correction in milliGauss
siwakon 0:dd400e4fe461 45 }
siwakon 0:dd400e4fe461 46
siwakon 0:dd400e4fe461 47 void Update()
siwakon 0:dd400e4fe461 48 {
siwakon 0:dd400e4fe461 49 if(this->mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
siwakon 0:dd400e4fe461 50 this->mpu9250.readAccelData(accelCount); // Read the x/y/z adc values
siwakon 0:dd400e4fe461 51 // Now we'll calculate the accleration value into actual g's
siwakon 0:dd400e4fe461 52 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
siwakon 0:dd400e4fe461 53 ay = (float)accelCount[1]*aRes - accelBias[1];
siwakon 0:dd400e4fe461 54 az = (float)accelCount[2]*aRes - accelBias[2];
siwakon 0:dd400e4fe461 55 this->mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values
siwakon 0:dd400e4fe461 56 // Calculate the gyro value into actual degrees per second
siwakon 0:dd400e4fe461 57 gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
siwakon 0:dd400e4fe461 58 gy = (float)gyroCount[1]*gRes - gyroBias[1];
siwakon 0:dd400e4fe461 59 gz = (float)gyroCount[2]*gRes - gyroBias[2];
siwakon 0:dd400e4fe461 60 this->mpu9250.readMagData(magCount); // Read the x/y/z adc values
siwakon 0:dd400e4fe461 61 // Calculate the magnetometer values in milliGauss
siwakon 0:dd400e4fe461 62 // Include factory calibration per data sheet and user environmental corrections
siwakon 0:dd400e4fe461 63 mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]+360.0f; // get actual magnetometer value, this depends on scale being set
siwakon 0:dd400e4fe461 64 my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1]-210.0f;
siwakon 0:dd400e4fe461 65 mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
siwakon 0:dd400e4fe461 66 //aa.printf("x %f\ty %f\tz %f\n",mx,my,mz);
siwakon 0:dd400e4fe461 67
siwakon 0:dd400e4fe461 68
siwakon 0:dd400e4fe461 69 } // end if one
siwakon 0:dd400e4fe461 70 Now = this->t.read_us();
siwakon 0:dd400e4fe461 71 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
siwakon 0:dd400e4fe461 72 lastUpdate = Now;
siwakon 0:dd400e4fe461 73 this->sum += deltat;
siwakon 0:dd400e4fe461 74 sumCount++;
siwakon 0:dd400e4fe461 75 this->mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
siwakon 0:dd400e4fe461 76
siwakon 0:dd400e4fe461 77 // Pass gyro rate as rad/s
siwakon 0:dd400e4fe461 78 /*if((rand()%20)>=0)
siwakon 0:dd400e4fe461 79 {
siwakon 0:dd400e4fe461 80 this->mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
siwakon 0:dd400e4fe461 81 }else
siwakon 0:dd400e4fe461 82 {
siwakon 0:dd400e4fe461 83 //this->mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
siwakon 0:dd400e4fe461 84 this->mpu9250.Mad_Update(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
siwakon 0:dd400e4fe461 85 }*/
siwakon 0:dd400e4fe461 86
siwakon 0:dd400e4fe461 87
siwakon 0:dd400e4fe461 88 // Serial print and/or display at 0.5 s rate independent of data rates
siwakon 0:dd400e4fe461 89 delt_t = this->t.read_ms() - count;
siwakon 0:dd400e4fe461 90 if (delt_t > 10) { // update LCD once per half-second independent of read rate
siwakon 0:dd400e4fe461 91 tempCount = this->mpu9250.readTempData(); // Read the adc values
siwakon 0:dd400e4fe461 92 temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
siwakon 0:dd400e4fe461 93 // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
siwakon 0:dd400e4fe461 94 // In this coordinate system, the positive z-axis is down toward Earth.
siwakon 0:dd400e4fe461 95 // 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.
siwakon 0:dd400e4fe461 96 // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
siwakon 0:dd400e4fe461 97 // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
siwakon 0:dd400e4fe461 98 // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
siwakon 0:dd400e4fe461 99 // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
siwakon 0:dd400e4fe461 100 // applied in the correct order which for this configuration is yaw, pitch, and then roll.
siwakon 0:dd400e4fe461 101 // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
siwakon 0:dd400e4fe461 102 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]);
siwakon 0:dd400e4fe461 103 //yaw = atan2(2.0f * (q[0] * q[2] + q[0] * q[3]), 1 - 2 * ( q[2] * q[2] + q[3] * q[3]));
siwakon 0:dd400e4fe461 104 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
siwakon 0:dd400e4fe461 105
siwakon 0:dd400e4fe461 106 //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]);
siwakon 0:dd400e4fe461 107 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]);
siwakon 0:dd400e4fe461 108 //pitch = atan2(sin(roll)*(q[1]*q[3]-q[0]*q[2]),q[1]*q[2]+q[0]*q[3]);
siwakon 0:dd400e4fe461 109 pitch *= 180.0f / PI;
siwakon 0:dd400e4fe461 110 yaw *= 180.0f / PI;
siwakon 0:dd400e4fe461 111 //yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
siwakon 0:dd400e4fe461 112 yaw -= 0.35f;
siwakon 0:dd400e4fe461 113 roll *= 180.0f / PI;
siwakon 0:dd400e4fe461 114 this->roll_x = roll;
siwakon 0:dd400e4fe461 115 this->pitch_y = pitch;
siwakon 0:dd400e4fe461 116 this->yaw_z = yaw;//(this->kal.getAngle(yaw*PI/180.0f,0.00,delt_t));
siwakon 0:dd400e4fe461 117 count = this->t.read_ms();
siwakon 0:dd400e4fe461 118 if(count > 1<<21) {
siwakon 0:dd400e4fe461 119 this->t.start(); // start the timer over again if ~30 minutes has passed
siwakon 0:dd400e4fe461 120 count = 0;
siwakon 0:dd400e4fe461 121 deltat= 0;
siwakon 0:dd400e4fe461 122 lastUpdate = this->t.read_us();
siwakon 0:dd400e4fe461 123 } // end if three.
siwakon 0:dd400e4fe461 124 this->sum = 0;
siwakon 0:dd400e4fe461 125 sumCount = 0;
siwakon 0:dd400e4fe461 126 } // end if two.
siwakon 0:dd400e4fe461 127 }
siwakon 0:dd400e4fe461 128
siwakon 0:dd400e4fe461 129
siwakon 0:dd400e4fe461 130 float Roll()
siwakon 0:dd400e4fe461 131 {
siwakon 0:dd400e4fe461 132 return roll_x;
siwakon 0:dd400e4fe461 133 }
siwakon 0:dd400e4fe461 134
siwakon 0:dd400e4fe461 135 float Pitch()
siwakon 0:dd400e4fe461 136 {
siwakon 0:dd400e4fe461 137 return pitch_y;
siwakon 0:dd400e4fe461 138 }
siwakon 0:dd400e4fe461 139
siwakon 0:dd400e4fe461 140 float Yaw()
siwakon 0:dd400e4fe461 141 {
siwakon 0:dd400e4fe461 142 return yaw_z;
siwakon 0:dd400e4fe461 143 }
siwakon 0:dd400e4fe461 144
siwakon 0:dd400e4fe461 145
siwakon 0:dd400e4fe461 146 private:
siwakon 0:dd400e4fe461 147 float sum;
siwakon 0:dd400e4fe461 148 uint32_t sumCount;
siwakon 0:dd400e4fe461 149 char buffer[14];
siwakon 0:dd400e4fe461 150 int roll_x;
siwakon 0:dd400e4fe461 151 kalman kal();
siwakon 0:dd400e4fe461 152 int pitch_y;
siwakon 0:dd400e4fe461 153 int yaw_z;
siwakon 0:dd400e4fe461 154 MPU9250 mpu9250;
siwakon 0:dd400e4fe461 155 Timer t;
siwakon 0:dd400e4fe461 156
siwakon 0:dd400e4fe461 157
siwakon 0:dd400e4fe461 158 };
siwakon 0:dd400e4fe461 159
siwakon 0:dd400e4fe461 160