use this
Dependencies: mbed
Fork of greenimugetithothot by
Revision 1:44b26709b117, committed 2017-12-10
- Comitter:
- csggreen
- Date:
- Sun Dec 10 07:13:07 2017 +0000
- Parent:
- 0:92bbf3093a25
- Child:
- 2:e8807de906c9
- Commit message:
- use this
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/GETMPU9250.h Sun Dec 10 07:13:07 2017 +0000 @@ -0,0 +1,132 @@ +#include "mbed.h" +#include "MPU9250.h" +#include "math.h" + +Serial aa(D1, D0); +class ZMU9250 +{ + public: + ZMU9250() + { + + //Set up I2C + i2c.frequency(400000); // use fast (400 kHz) I2C + this->t.start(); + + // Read the WHO_AM_I register, this is a good test of communication + uint8_t whoami = this->mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250 + if ((whoami == 0x71)||(whoami == 0x73)) // WHO_AM_I should always be 0x68 + { + wait(1); + this->mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration + this->mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values + this->mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers + wait(2); + this->mpu9250.initMPU9250(); + this->mpu9250.initAK8963(magCalibration); + wait(1); + } + else + { + while(1) ; // Loop forever if communication doesn't happen + } + this->mpu9250.getAres(); // Get accelerometer sensitivity + this->mpu9250.getGres(); // Get gyro sensitivity + this->mpu9250.getMres(); // Get magnetometer sensitivity + magbias[0] = +470; // User environmental x-axis correction in milliGauss, should be automatically calculated + magbias[1] = +120; // User environmental x-axis correction in milliGauss + magbias[2] = +125; // User environmental x-axis correction in milliGauss + } + + void Update() + { + if(this->mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt + this->mpu9250.readAccelData(accelCount); // Read the x/y/z adc values + // Now we'll calculate the accleration value into actual g's + ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set + ay = (float)accelCount[1]*aRes - accelBias[1]; + az = (float)accelCount[2]*aRes - accelBias[2]; + this->mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values + // Calculate the gyro value into actual degrees per second + gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set + gy = (float)gyroCount[1]*gRes - gyroBias[1]; + gz = (float)gyroCount[2]*gRes - gyroBias[2]; + this->mpu9250.readMagData(magCount); // Read the x/y/z adc values + // Calculate the magnetometer values in milliGauss + // Include factory calibration per data sheet and user environmental corrections + mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]+360.0f; // get actual magnetometer value, this depends on scale being set + my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1]-210.0f; + mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2]; + //aa.printf("mx %f\tmy %f\tmz %fgx %f\tgy %f\tgz %fax %f\tay %f\taz %f\n\n\n",mx,my,mz,gx,gy,gz,ax,ay,az); + //wait(0.5); + + + } + Now = this->t.read_us(); + deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update + lastUpdate = Now; + this->sum += deltat; + sumCount++; + this->mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx, gy, gz, my, mx, mz); + delt_t = this->t.read_ms() - count; + if (delt_t > 10) { // update LCD once per half-second independent of read rate + tempCount = this->mpu9250.readTempData(); // Read the adc values + temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade + this->axis_gx =gx; + this->axis_gy =gy; + this->axis_gz =gz; + this->axis_ax =ax; + this->axis_ay =ay; + this->axis_az =az; + count = this->t.read_ms(); + if(count > 1<<21) { + this->t.start(); // start the timer over again if ~30 minutes has passed + count = 0; + deltat= 0; + lastUpdate = this->t.read_us(); + } + this->sum = 0; + sumCount = 0; + } + } + + + float g_ax() + { + return axis_ax; + } + float g_ay() + { + return axis_ay; + } + float g_az() + { + return axis_az; + } + float g_gx() + { + return axis_gx; + } + float g_gy() + { + return axis_gy; + } + float g_gz() + { + return axis_gz; + } + + + + private: + float sum; + uint32_t sumCount; + char buffer[14]; + MPU9250 mpu9250; + Timer t; + float axis_az,axis_ay,axis_ax,axis_gz,axis_gy,axis_gx; + + +}; + +
--- a/main.cpp Thu Dec 07 14:20:22 2017 +0000 +++ b/main.cpp Sun Dec 10 07:13:07 2017 +0000 @@ -1,5 +1,5 @@ #include "mbed.h" -#include "zmu9250.h" +#include "GETMPU9250.h" #include "math.h" ZMU9250 axis_Gen; @@ -17,11 +17,11 @@ axis_ax = axis_Gen.g_ax()*180; axis_ay = axis_Gen.g_ay()*180; axis_az = axis_Gen.g_az()*180; - axis_gx = axis_Gen.g_gx()*180; - axis_gy = axis_Gen.g_gy()*180; - axis_gz = axis_Gen.g_gz()*180; - //pc.printf(" axis_x %f\t axis_y %f\t axis_z %f\n\n\n\r", axis_gx, axis_gy, axis_gz); - pc.printf(" axis_x %f\t axis_y %f\t axis_z %f\n\n\n\r", axis_ax, axis_ay, axis_az); + axis_gx = axis_Gen.g_gx(); + axis_gy = axis_Gen.g_gy(); + axis_gz = axis_Gen.g_gz(); + pc.printf(" axis_x %f\t axis_y %f\t axis_z %f\n\n\n\r", axis_gx, axis_gy, axis_gz); + //pc.printf(" axis_x %f\t axis_y %f\t axis_z %f\n\n\n\r", axis_ax, axis_ay, axis_az); wait(0.2); } }
--- a/zmu9250.h Thu Dec 07 14:20:22 2017 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,132 +0,0 @@ -#include "mbed.h" -#include "MPU9250.h" -#include "math.h" - -Serial aa(D1, D0); -class ZMU9250 -{ - public: - ZMU9250() - { - - //Set up I2C - i2c.frequency(400000); // use fast (400 kHz) I2C - this->t.start(); - - // Read the WHO_AM_I register, this is a good test of communication - uint8_t whoami = this->mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250 - if ((whoami == 0x71)||(whoami == 0x73)) // WHO_AM_I should always be 0x68 - { - wait(1); - this->mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration - this->mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values - this->mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers - wait(2); - this->mpu9250.initMPU9250(); - this->mpu9250.initAK8963(magCalibration); - wait(1); - } - else - { - while(1) ; // Loop forever if communication doesn't happen - } - this->mpu9250.getAres(); // Get accelerometer sensitivity - this->mpu9250.getGres(); // Get gyro sensitivity - this->mpu9250.getMres(); // Get magnetometer sensitivity - magbias[0] = +470; // User environmental x-axis correction in milliGauss, should be automatically calculated - magbias[1] = +120; // User environmental x-axis correction in milliGauss - magbias[2] = +125; // User environmental x-axis correction in milliGauss - } - - void Update() - { - if(this->mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt - this->mpu9250.readAccelData(accelCount); // Read the x/y/z adc values - // Now we'll calculate the accleration value into actual g's - ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set - ay = (float)accelCount[1]*aRes - accelBias[1]; - az = (float)accelCount[2]*aRes - accelBias[2]; - this->mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values - // Calculate the gyro value into actual degrees per second - gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set - gy = (float)gyroCount[1]*gRes - gyroBias[1]; - gz = (float)gyroCount[2]*gRes - gyroBias[2]; - this->mpu9250.readMagData(magCount); // Read the x/y/z adc values - // Calculate the magnetometer values in milliGauss - // Include factory calibration per data sheet and user environmental corrections - mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]+360.0f; // get actual magnetometer value, this depends on scale being set - my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1]-210.0f; - mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2]; - //aa.printf("mx %f\tmy %f\tmz %fgx %f\tgy %f\tgz %fax %f\tay %f\taz %f\n\n\n",mx,my,mz,gx,gy,gz,ax,ay,az); - //wait(0.5); - - - } - Now = this->t.read_us(); - deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update - lastUpdate = Now; - this->sum += deltat; - sumCount++; - this->mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz); - delt_t = this->t.read_ms() - count; - if (delt_t > 10) { // update LCD once per half-second independent of read rate - tempCount = this->mpu9250.readTempData(); // Read the adc values - temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade - this->axis_gx =gx; - this->axis_gy =gy; - this->axis_gz =gz; - this->axis_ax =ax; - this->axis_ay =ay; - this->axis_az =az; - count = this->t.read_ms(); - if(count > 1<<21) { - this->t.start(); // start the timer over again if ~30 minutes has passed - count = 0; - deltat= 0; - lastUpdate = this->t.read_us(); - } // end if three. - this->sum = 0; - sumCount = 0; - } // end if two. - } - - - float g_ax() - { - return axis_ax; - } - float g_ay() - { - return axis_ay; - } - float g_az() - { - return axis_az; - } - float g_gx() - { - return axis_gx; - } - float g_gy() - { - return axis_gy; - } - float g_gz() - { - return axis_gz; - } - - - - private: - float sum; - uint32_t sumCount; - char buffer[14]; - MPU9250 mpu9250; - Timer t; - float axis_az,axis_ay,axis_ax,axis_gz,axis_gy,axis_gx; - - -}; - -