OX / Mbed 2 deprecated IMU_oo

new

Dependencies:   mbed

zmu9250.h@0:77a7d1a1c6db, 2016-12-03 (annotated)

Committer:
siwakon
Date:
Sat Dec 03 18:21:47 2016 +0000
Revision:
0:77a7d1a1c6db
Child:
1:6bc2c5d68446
ooo

Who changed what in which revision?

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