OX / Mbed 2 deprecated Ox_ver_function

hahaha

Dependencies:   mbed

zmu9250.h@1:d8ce226c8c2e, 2016-12-06 (annotated)

Committer:
arthicha
Date:
Tue Dec 06 06:11:54 2016 +0000
Revision:
1:d8ce226c8c2e
Parent:
0:a291977ec0b1
Child:
2:ce3ee4bc8cf7
;UUpdate;

Who changed what in which revision?

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