OX / Mbed 2 deprecated Ox_ver_function

hahaha

Dependencies:   mbed

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

Committer:
arthicha
Date:
Mon Dec 05 18:31:43 2016 +0000
Revision:
0:a291977ec0b1
Child:
1:d8ce226c8c2e
Publich

Who changed what in which revision?

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