Diff: MPU6000.cpp

Revision:

6:d9198ff2dcf9

Parent:

5:db1da6bc9dce

Child:

7:5e299dbf98ff

--- a/MPU6000.cpp	Wed Jun 03 17:32:43 2015 +0000
+++ b/MPU6000.cpp	Wed Jun 03 19:00:43 2015 +0000
@@ -6,7 +6,7 @@
 #include "MPU6000.h"
 #include "float.h"
 
-mpu6000_spi::mpu6000_spi(SPI& _spi, PinName _cs) : spi(_spi), cs(_cs), accFilterCurrent(0), accFilterPre(0), gyroFilterCurrent(0), gyroFliterPre(0){}
+mpu6000_spi::mpu6000_spi(SPI& _spi, PinName _cs) : spi(_spi), cs(_cs), accFilterCurrent(0), accFilterPre(0), gyroFilterCurrent(0), gyroFliterPre(0) {}
 
 /*-----------------------------------------------------------------------------------------------
                                     INITIALIZATION
@@ -197,27 +197,21 @@
 {
     // create instance of response
     unsigned int response;
-    
+
     // Begin transmission
     select();
-    
+
     // Bitwise OR of WHOAMI Register (default 0x68) and READ_FLAG
     response=spi.write(MPUREG_WHOAMI|READ_FLAG);
     response=spi.write(0x00);
-    
+
     // End transmission
     deselect();
-    
+
     return response;
 }
 
-/***********************************************************
-Function:         whoami_check()
-Access Specifier: Public
-Use:              Safety check. Compares WHO_AM_I response to expected response (See description of whoami() for expected response detail).
-Input:            None
-Output:           who_error. High = Safe. Low = Unsafe
-***********************************************************/
+/** High = Safe. Low = Unsafe */
 int mpu6000_spi::whoami_check()
 {
     // mask whoami() against expected value
@@ -225,10 +219,8 @@
 }
 
 
-/*------------------------------------------------------------------------------------------------
-                                 Get Tilt Angle from Accerometer
-8/21/2014 edited by Grace (Yi-Wen Liao)
-------------------------------------------------------------------------------------------------*/
+// Get Tilt Angle from Accerometer
+
 float mpu6000_spi::getAccTilt()
 {
     float Z,X;
@@ -244,26 +236,9 @@
             temp=FLT_MIN;
         }
     }
-    /*if(Z>=0 && X<=0) {
-        return atan(temp)*180/pi-180;
-    }
-    if(Z<=0 && X<=0) {
-        return atan(temp)*180/pi+180;*/
-    //}else{
+
     return atan(temp)*180/pi;
-    //}
-    //printf("Z=%f\n\r",Z);
-    /*if (Z >= 0 && Z < 0.7071) {
-        wait(0.0001);
-        if (read_acc(0) >= 0)  sign = 1;
-        else  sign = -1;
-        angleData = (pio2 - atan(Z/sqrt(1-Z*Z)))*sign*180/pi; //arccos
-    } else if (Z >= 0.7071) {
-        wait(0.0001);
-        X = read_acc(0);
-        angleData = atan(X/sqrt(1-X*X))*180/pi; //arcsin
-    }
-    return angleData;*/
+
 }
 
 
@@ -436,6 +411,7 @@
     float gyro = read_rot(1);
     float acc = getAccTilt();
 
+    // complementary filter
     accFilterCurrent = 0.8187*accFilterPre+0.1813*acc;
     gyroFilterCurrent = 0.8187*gyroFliterPre+0.0009063*gyro;
 
@@ -447,7 +423,7 @@
     return accFilterCurrent + gyroFilterCurrent;
 }
 
-float mpu6000_spi::read_angle_y()
+float mpu6000_spi::rereadAngle_y()
 {
     return accFilterCurrent + gyroFilterCurrent;
 }