Diff: MPU6050.h

Revision:

2:19e22a4eaa18

Parent:

1:1e0baaf91e96

Child:

3:6624faa750c7

--- a/MPU6050.h	Wed May 08 00:34:55 2013 +0000
+++ b/MPU6050.h	Wed Feb 11 20:15:11 2015 +0000
@@ -45,11 +45,13 @@
 
 #include "I2Cdev.h"
 #include "helper_3dmath.h"
+#include "mbed.h"
 
 #define MPU6050_ADDRESS_AD0_LOW     0x68 // address pin low (GND), default for InvenSense evaluation board
 #define MPU6050_ADDRESS_AD0_HIGH    0x69 // address pin high (VCC)
 #define MPU6050_DEFAULT_ADDRESS     MPU6050_ADDRESS_AD0_LOW
 
+// DMP Registers
 #define MPU6050_RA_XG_OFFS_TC       0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
 #define MPU6050_RA_YG_OFFS_TC       0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
 #define MPU6050_RA_ZG_OFFS_TC       0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD
@@ -57,11 +59,17 @@
 #define MPU6050_RA_Y_FINE_GAIN      0x04 //[7:0] Y_FINE_GAIN
 #define MPU6050_RA_Z_FINE_GAIN      0x05 //[7:0] Z_FINE_GAIN
 #define MPU6050_RA_XA_OFFS_H        0x06 //[15:0] XA_OFFS
-#define MPU6050_RA_XA_OFFS_L_TC     0x07
+#define MPU6050_RA_XA_OFFS_L        0x07
 #define MPU6050_RA_YA_OFFS_H        0x08 //[15:0] YA_OFFS
-#define MPU6050_RA_YA_OFFS_L_TC     0x09
+#define MPU6050_RA_YA_OFFS_L        0x09
 #define MPU6050_RA_ZA_OFFS_H        0x0A //[15:0] ZA_OFFS
-#define MPU6050_RA_ZA_OFFS_L_TC     0x0B
+#define MPU6050_RA_ZA_OFFS_L        0x0B
+
+// Non-DMP Registers
+#define MPU6050_RA_SELF_TEST_X      0x0D
+#define MPU6050_RA_SELF_TEST_Y      0x0E
+#define MPU6050_RA_SELF_TEST_Z      0x0F
+#define MPU6050_RA_SELF_TEST_A      0x10
 #define MPU6050_RA_XG_OFFS_USRH     0x13 //[15:0] XG_OFFS_USR
 #define MPU6050_RA_XG_OFFS_USRL     0x14
 #define MPU6050_RA_YG_OFFS_USRH     0x15 //[15:0] YG_OFFS_USR
@@ -191,21 +199,36 @@
 #define MPU6050_DLPF_BW_10          0x05
 #define MPU6050_DLPF_BW_5           0x06
 
+#define MPU6050_GCONFIG_XA_ST_BIT       7 // Self test bits of Gyroscope Configuration register
+#define MPU6050_GCONFIG_YA_ST_BIT       6
+#define MPU6050_GCONFIG_ZA_ST_BIT       5
 #define MPU6050_GCONFIG_FS_SEL_BIT      4
 #define MPU6050_GCONFIG_FS_SEL_LENGTH   2
+#define MPU6050_GCONFIG_SELF_TEST_BYTE  0xE0
+#define MPU6050_GCONFIG_BYTE            0x00
+#define MPU6050_GCONFIG_MAX_VAR         14
+#define MPU6050_GCONFIG_MIN_DPS         10.f
+#define MPU6050_GCONFIG_MAX_DPS         105.f
+#define MPU6050_GCONFIG_SENS            (32768/250)
 
 #define MPU6050_GYRO_FS_250         0x00
 #define MPU6050_GYRO_FS_500         0x01
 #define MPU6050_GYRO_FS_1000        0x02
 #define MPU6050_GYRO_FS_2000        0x03
 
-#define MPU6050_ACONFIG_XA_ST_BIT           7
+#define MPU6050_ACONFIG_XA_ST_BIT           7 // Self test bits of Accelerometer Configuration register
 #define MPU6050_ACONFIG_YA_ST_BIT           6
 #define MPU6050_ACONFIG_ZA_ST_BIT           5
 #define MPU6050_ACONFIG_AFS_SEL_BIT         4
 #define MPU6050_ACONFIG_AFS_SEL_LENGTH      2
 #define MPU6050_ACONFIG_ACCEL_HPF_BIT       2
 #define MPU6050_ACONFIG_ACCEL_HPF_LENGTH    3
+#define MPU6050_ACONFIG_SELF_TEST_BYTE      0xF0
+#define MPU6050_ACONFIG_BYTE                0x00
+#define MPU6050_ACONFIG_MAX_VAR             14
+#define MPU6050_ACONFIG_MIN_G               0.3
+#define MPU6050_ACONFIG_MAX_G               0.95
+#define MPU6050_ACONFIG_SENS                (32768/16)
 
 #define MPU6050_ACCEL_FS_2          0x00
 #define MPU6050_ACCEL_FS_4          0x01
@@ -227,6 +250,8 @@
 #define MPU6050_SLV2_FIFO_EN_BIT    2
 #define MPU6050_SLV1_FIFO_EN_BIT    1
 #define MPU6050_SLV0_FIFO_EN_BIT    0
+#define MPU6050_ENABLE_ALL_FIFO     0x78
+#define MPU6050_DISABLE_ALL_FIFO    0x00
 
 #define MPU6050_MULT_MST_EN_BIT     7
 #define MPU6050_WAIT_FOR_ES_BIT     6
@@ -359,6 +384,8 @@
 #define MPU6050_USERCTRL_I2C_MST_RESET_BIT      1
 #define MPU6050_USERCTRL_SIG_COND_RESET_BIT     0
 
+#define MPU6050_PWR1_LENGTH             8
+#define MPU6050_PWR1_BIT                7
 #define MPU6050_PWR1_DEVICE_RESET_BIT   7
 #define MPU6050_PWR1_SLEEP_BIT          6
 #define MPU6050_PWR1_CYCLE_BIT          5
@@ -393,25 +420,36 @@
 #define MPU6050_BANKSEL_MEM_SEL_BIT         4
 #define MPU6050_BANKSEL_MEM_SEL_LENGTH      5
 
-#define MPU6050_WHO_AM_I_BIT        6
-#define MPU6050_WHO_AM_I_LENGTH     6
+#define MPU6050_WHO_AM_I_BIT        7
+#define MPU6050_WHO_AM_I_LENGTH     8
 
 #define MPU6050_DMP_MEMORY_BANKS        8
 #define MPU6050_DMP_MEMORY_BANK_SIZE    256
 #define MPU6050_DMP_MEMORY_CHUNK_SIZE   16
 
+// 16-bit ADCs so (2^16)/2 for +/- 32768
+#define MPU6050_ADC_RANGE               32768
+
+#define PI 3.14159265358979323846f
+
 // note: DMP code memory blocks defined at end of header file
 
 class MPU6050 {
     private:
         I2Cdev i2Cdev;
         Serial debugSerial;
+        Timer timer;
     public:
         MPU6050();
         MPU6050(uint8_t address);
 
         void initialize();
         bool testConnection();
+        int selfTest(float* destination);
+        void get_st_biases(long *gyro, long *accel, unsigned char hw_test);
+        int accel_self_test(long *bias_regular, long *bias_st, float *st_shift_var);
+        void get_accel_prod_shift(float *st_shift);
+        int gyro_self_test(long *bias_regular, long *bias_st, float *st_shift_var);
 
         // AUX_VDDIO register
         uint8_t getAuxVDDIOLevel();
@@ -420,6 +458,9 @@
         // SMPLRT_DIV register
         uint8_t getRate();
         void setRate(uint8_t rate);
+        
+        // Sample Rate
+        uint32_t getSampRate();
 
         // CONFIG register
         uint8_t getExternalFrameSync();
@@ -428,6 +469,12 @@
         void setDLPFMode(uint8_t bandwidth);
 
         // GYRO_CONFIG register
+        bool getGyroXSelfTest();
+        void setGyroXSelfTest(bool enabled);
+        bool getGyroYSelfTest();
+        void setGyroYSelfTest(bool enabled);
+        bool getGyroZSelfTest();
+        void setGyroZSelfTest(bool enabled);
         uint8_t getFullScaleGyroRange();
         void setFullScaleGyroRange(uint8_t range);
 
@@ -646,6 +693,7 @@
         void resetSensors();
 
         // PWR_MGMT_1 register
+        uint8_t getPwrMgmt1Reg();
         void reset();
         bool getSleepEnabled();
         void setSleepEnabled(bool enabled);
@@ -686,6 +734,10 @@
         
         // ======== UNDOCUMENTED/DMP REGISTERS/METHODS ========
         
+        // Get the data and convert it
+        void getAndConvertData(float *ax, float *ay, float *az, float *yaw, float *pitch, float *roll, float *accel_bias, float *gyro_bias);
+        void MadgwickQuaternionUpdate(float *ax, float *ay, float *az, float gx, float gy, float gz, float *q);
+        
         // XG_OFFS_TC register
         uint8_t getOTPBankValid();
         void setOTPBankValid(bool enabled);
@@ -848,7 +900,7 @@
             uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0);
             uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0);
             uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0);
-            uint8_t dm  pGetTemperature(int32_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0);
             uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0);
             uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0);
             uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0);
@@ -989,6 +1041,18 @@
     private:
         uint8_t devAddr;
         uint8_t buffer[14];
+        
+        float last_update, first_update, cur_time;
+        
+        // Gyroscope meas. error in rad/s (start at 60 deg/s), then reduce
+        // after ~10 to 3
+        float GyroMeasError;
+        float beta; // compute beta
+        float GyroMeasDrift; // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
+        
+        // Compute zeta, the other free parameter in the Madgwick scheme
+        // usually set to a small or zero value
+        float zeta;
 };
 
 #endif /* _MPU6050_H_ */
\ No newline at end of file