OX / Mbed 2 deprecated O_V1

OX V1

Dependencies:   mbed

zmu9250.h@2:84a84429750a, 2016-12-05 (annotated)

Committer:
arthicha
Date:
Mon Dec 05 16:11:19 2016 +0000
Revision:
2:84a84429750a
Parent:
0:6182212860fb 
hhh;

Who changed what in which revision?

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