Took Kris Winer's code and added some functions for the MPU6050
Fork of MPU6050 by
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