Diff: MPU6050.h

Branch:

7_????????????

Revision:

17:0a0c4277d960

Parent:

0:085b2c5e3254

Child:

26:50272431cd1e

--- a/MPU6050.h	Thu Oct 27 15:56:19 2016 +0000
+++ b/MPU6050.h	Fri Oct 28 15:08:14 2016 +0000
@@ -133,6 +133,9 @@
 #define MPU6050_ADDRESS 0x68<<1  // Device address when ADO = 0
 #endif
 
+//自分で定義したマクロ
+#define MPU_DELT_MIN 250
+
 // Set initial input parameters
 enum Ascale {
   AFS_2G = 0,
@@ -151,6 +154,8 @@
 // Specify sensor full scale
 int Gscale = GFS_250DPS;
 int Ascale = AFS_2G;
+float sum = 0;
+uint32_t sumCount = 0;
 
 //Set up I2C, (SDA,SCL)
 I2C i2c(p28,p27);
@@ -224,6 +229,65 @@
     }
 } 
  
+ void MPUInit(Timer t){
+      i2c.frequency(400000);  // use fast (400 kHz) I2C
+    t.start();
+    uint8_t whoami = readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);  // Read WHO_AM_I register for MPU-6050
+    if (whoami == 0x68) { // WHO_AM_I should always be 0x68
+        wait(1);
+        MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
+        wait(1);
+        if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) {
+            resetMPU6050(); // Reset registers to default in preparation for device calibration
+            calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
+            initMPU6050(); //pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
+            wait(2);
+        } else {
+        }
+    } else {
+        //pc.printf("out\n\r"); // Loop forever if communication doesn't happen
+    }   
+     }
+     
+     void mpuProcessing(Timer t){
+    if(readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) {  // check if data ready interrupt
+        readAccelData(accelCount);  // Read the x/y/z adc values
+        getAres();
+        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];
+        readGyroData(gyroCount);  // Read the x/y/z adc values
+        getGres();
+        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];
+        tempCount = readTempData();  // Read the x/y/z adc values
+        temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
+    }
+    Now = t.read_us();
+    deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
+    lastUpdate = Now;
+    sum += deltat;
+    sumCount++;
+    if(lastUpdate - firstUpdate > 10000000.0f) {
+        beta = 0.04;  // decrease filter gain after stabilized
+        zeta = 0.015; // increasey bias drift gain after stabilized
+    }
+    MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
+    delt_t = t.read_ms() - count;
+    if (delt_t > MPU_DELT_MIN) {
+        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]);
+        pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
+        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]);
+        pitch *= 180.0f / PI;
+        yaw   *= 180.0f / PI;
+        roll  *= 180.0f / PI;
+        myled= !myled;
+        count = t.read_ms();
+        sum = 0;
+        sumCount = 0;
+    }
+}
 
 void getGres() {
   switch (Gscale)