Not a real MPU6050 but imc-20689
Fork of MPU6050 by
Diff: MPU6050.h
- Revision:
- 3:d2b927200037
- Parent:
- 2:5c63e20c50f3
--- a/MPU6050.h Mon Sep 10 21:26:25 2012 +0000 +++ b/MPU6050.h Tue Oct 23 10:48:22 2018 +0000 @@ -1,274 +1,166 @@ -/*Use #define MPU6050_ES before you include this file if you have an engineering sample (older EVBs will have them), to find out if you have one, check your accelerometer output. -If it is half of what you expected, and you still are on the correct planet, you got an engineering sample -*/ - - #ifndef MPU6050_H #define MPU6050_H -/** - * Includes - */ #include "mbed.h" +#include "math.h" +#define PI 3.14159265359 // This value will be used when calculating angles +#define dt 0.005 // 200 Hz sampling period -/** - * Defines - */ -#ifndef MPU6050_ADDRESS - #define MPU6050_ADDRESS 0x69 // address pin low (GND), default for InvenSense evaluation board +#define XGOFFS_TC 0x00 // Bit 7 PWR_MODE, bits 6:1 XG_OFFS_TC, bit 0 OTP_BNK_VLD +#define YGOFFS_TC 0x01 +#define ZGOFFS_TC 0x02 +#define X_FINE_GAIN 0x03 // [7:0] fine gain +#define Y_FINE_GAIN 0x04 +#define Z_FINE_GAIN 0x05 +#define XA_OFFSET_H 0x06 // User-defined trim values for accelerometer +#define XA_OFFSET_L_TC 0x07 +#define YA_OFFSET_H 0x08 +#define YA_OFFSET_L_TC 0x09 +#define ZA_OFFSET_H 0x0A +#define ZA_OFFSET_L_TC 0x0B +#define SELF_TEST_X 0x0D +#define SELF_TEST_Y 0x0E +#define SELF_TEST_Z 0x0F +#define SELF_TEST_A 0x10 +#define XG_OFFS_USRH 0x13 // User-defined trim values for gyroscope; supported in MPU-6050? +#define XG_OFFS_USRL 0x14 +#define YG_OFFS_USRH 0x15 +#define YG_OFFS_USRL 0x16 +#define ZG_OFFS_USRH 0x17 +#define ZG_OFFS_USRL 0x18 +#define SMPLRT_DIV 0x19 +#define CONFIG 0x1A +#define GYRO_CONFIG 0x1B +#define ACCEL_CONFIG 0x1C +#define FF_THR 0x1D // Free-fall +#define FF_DUR 0x1E // Free-fall +#define MOT_THR 0x1F // Motion detection threshold bits [7:0] +#define MOT_DUR 0x20 // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms +#define ZMOT_THR 0x21 // Zero-motion detection threshold bits [7:0] +#define ZRMOT_DUR 0x22 // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms +#define FIFO_EN 0x23 +#define I2C_MST_CTRL 0x24 +#define I2C_SLV0_ADDR 0x25 +#define I2C_SLV0_REG 0x26 +#define I2C_SLV0_CTRL 0x27 +#define I2C_SLV1_ADDR 0x28 +#define I2C_SLV1_REG 0x29 +#define I2C_SLV1_CTRL 0x2A +#define I2C_SLV2_ADDR 0x2B +#define I2C_SLV2_REG 0x2C +#define I2C_SLV2_CTRL 0x2D +#define I2C_SLV3_ADDR 0x2E +#define I2C_SLV3_REG 0x2F +#define I2C_SLV3_CTRL 0x30 +#define I2C_SLV4_ADDR 0x31 +#define I2C_SLV4_REG 0x32 +#define I2C_SLV4_DO 0x33 +#define I2C_SLV4_CTRL 0x34 +#define I2C_SLV4_DI 0x35 +#define I2C_MST_STATUS 0x36 +#define INT_PIN_CFG 0x37 +#define INT_ENABLE 0x38 +#define DMP_INT_STATUS 0x39 // Check DMP interrupt +#define INT_STATUS 0x3A +#define ACCEL_XOUT_H 0x3B +#define ACCEL_XOUT_L 0x3C +#define ACCEL_YOUT_H 0x3D +#define ACCEL_YOUT_L 0x3E +#define ACCEL_ZOUT_H 0x3F +#define ACCEL_ZOUT_L 0x40 +#define TEMP_OUT_H 0x41 +#define TEMP_OUT_L 0x42 +#define GYRO_XOUT_H 0x43 +#define GYRO_XOUT_L 0x44 +#define GYRO_YOUT_H 0x45 +#define GYRO_YOUT_L 0x46 +#define GYRO_ZOUT_H 0x47 +#define GYRO_ZOUT_L 0x48 +#define EXT_SENS_DATA_00 0x49 +#define EXT_SENS_DATA_01 0x4A +#define EXT_SENS_DATA_02 0x4B +#define EXT_SENS_DATA_03 0x4C +#define EXT_SENS_DATA_04 0x4D +#define EXT_SENS_DATA_05 0x4E +#define EXT_SENS_DATA_06 0x4F +#define EXT_SENS_DATA_07 0x50 +#define EXT_SENS_DATA_08 0x51 +#define EXT_SENS_DATA_09 0x52 +#define EXT_SENS_DATA_10 0x53 +#define EXT_SENS_DATA_11 0x54 +#define EXT_SENS_DATA_12 0x55 +#define EXT_SENS_DATA_13 0x56 +#define EXT_SENS_DATA_14 0x57 +#define EXT_SENS_DATA_15 0x58 +#define EXT_SENS_DATA_16 0x59 +#define EXT_SENS_DATA_17 0x5A +#define EXT_SENS_DATA_18 0x5B +#define EXT_SENS_DATA_19 0x5C +#define EXT_SENS_DATA_20 0x5D +#define EXT_SENS_DATA_21 0x5E +#define EXT_SENS_DATA_22 0x5F +#define EXT_SENS_DATA_23 0x60 +#define MOT_DETECT_STATUS 0x61 +#define I2C_SLV0_DO 0x63 +#define I2C_SLV1_DO 0x64 +#define I2C_SLV2_DO 0x65 +#define I2C_SLV3_DO 0x66 +#define I2C_MST_DELAY_CTRL 0x67 +#define SIGNAL_PATH_RESET 0x68 +#define MOT_DETECT_CTRL 0x69 +#define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP +#define PWR_MGMT_1 0x6B // Device defaults to the SLEEP mode +#define PWR_MGMT_2 0x6C +#define DMP_BANK 0x6D // Activates a specific bank in the DMP +#define DMP_RW_PNT 0x6E // Set read/write pointer to a specific start address in specified DMP bank +#define DMP_REG 0x6F // Register in DMP from which to read or to which to write +#define DMP_REG_1 0x70 +#define DMP_REG_2 0x71 +#define FIFO_COUNTH 0x72 +#define FIFO_COUNTL 0x73 +#define FIFO_R_W 0x74 +#define WHO_AM_I_MPU6050 0x75 // Should return 0x68 + +// Using the GY-521 breakout board, I set ADO to 0 by grounding through a 4k7 resistor +// Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1 +#define ADO 0 +#if ADO +#define MPU6050_ADDRESS 0x69<<1 // Device address when ADO = 1 +#else +#define MPU6050_ADDRESS 0x68<<1 // Device address when ADO = 0 #endif -#ifdef MPU6050_ES - #define DOUBLE_ACCELERO -#endif - -/** - * Registers - */ - #define MPU6050_CONFIG_REG 0x1A - #define MPU6050_GYRO_CONFIG_REG 0x1B - #define MPU6050_ACCELERO_CONFIG_REG 0x1C - - #define MPU6050_INT_PIN_CFG 0x37 - - #define MPU6050_ACCEL_XOUT_H_REG 0x3B - #define MPU6050_ACCEL_YOUT_H_REG 0x3D - #define MPU6050_ACCEL_ZOUT_H_REG 0x3F - - #define MPU6050_TEMP_H_REG 0x41 - - #define MPU6050_GYRO_XOUT_H_REG 0x43 - #define MPU6050_GYRO_YOUT_H_REG 0x45 - #define MPU6050_GYRO_ZOUT_H_REG 0x47 - - - - #define MPU6050_PWR_MGMT_1_REG 0x6B - #define MPU6050_WHO_AM_I_REG 0x75 - - - - /** - * Definitions - */ -#define MPU6050_SLP_BIT 6 -#define MPU6050_BYPASS_BIT 1 +extern float aRes, gRes; -#define MPU6050_BW_256 0 -#define MPU6050_BW_188 1 -#define MPU6050_BW_98 2 -#define MPU6050_BW_42 3 -#define MPU6050_BW_20 4 -#define MPU6050_BW_10 5 -#define MPU6050_BW_5 6 - -#define MPU6050_ACCELERO_RANGE_2G 0 -#define MPU6050_ACCELERO_RANGE_4G 1 -#define MPU6050_ACCELERO_RANGE_8G 2 -#define MPU6050_ACCELERO_RANGE_16G 3 - -#define MPU6050_GYRO_RANGE_250 0 -#define MPU6050_GYRO_RANGE_500 1 -#define MPU6050_GYRO_RANGE_1000 2 -#define MPU6050_GYRO_RANGE_2000 3 - +/* whoAmI func uses this func, variables etc */ -/** MPU6050 IMU library. - * - * Example: - * @code - * Later, maybe - * @endcode - */ -class MPU6050 { - public: - /** - * Constructor. - * - * Sleep mode of MPU6050 is immediatly disabled - * - * @param sda - mbed pin to use for the SDA I2C line. - * @param scl - mbed pin to use for the SCL I2C line. - */ - MPU6050(PinName sda, PinName scl); - +extern Serial pc; - /** - * Tests the I2C connection by reading the WHO_AM_I register. - * - * @return True for a working connection, false for an error - */ - bool testConnection( void ); - - /** - * Sets the bandwidth of the digital low-pass filter - * - * Macros: MPU6050_BW_256 - MPU6050_BW_188 - MPU6050_BW_98 - MPU6050_BW_42 - MPU6050_BW_20 - MPU6050_BW_10 - MPU6050_BW_5 - * Last number is the gyro's BW in Hz (accelero BW is virtually identical) - * - * @param BW - The three bits that set the bandwidth (use the predefined macros) - */ - void setBW( char BW ); - - /** - * Sets the auxiliary I2C bus in bypass mode to read the sensors behind the MPU6050 (useful for eval board, otherwise just connect them to primary I2C bus) - * - * @param state - Enables/disables the I2C bypass mode - */ - void setI2CBypass ( bool state ); - - /** - * Sets the Accelero full-scale range - * - * Macros: MPU6050_ACCELERO_RANGE_2G - MPU6050_ACCELERO_RANGE_4G - MPU6050_ACCELERO_RANGE_8G - MPU6050_ACCELERO_RANGE_16G - * - * @param range - The two bits that set the full-scale range (use the predefined macros) - */ - void setAcceleroRange(char range); - - /** - * Reads the accelero x-axis. - * - * @return 16-bit signed integer x-axis accelero data - */ - int getAcceleroRawX( void ); - - /** - * Reads the accelero y-axis. - * - * @return 16-bit signed integer y-axis accelero data - */ - int getAcceleroRawY( void ); - - /** - * Reads the accelero z-axis. - * - * @return 16-bit signed integer z-axis accelero data - */ - int getAcceleroRawZ( void ); - - /** - * Reads all accelero data. - * - * @param data - pointer to signed integer array with length three: data[0] = X, data[1] = Y, data[2] = Z - */ - void getAcceleroRaw( int *data ); - - /** - * Reads all accelero data, gives the acceleration in m/s2 - * - * Function uses the last setup value of the full scale range, if you manually set in another range, this won't work. - * - * @param data - pointer to float array with length three: data[0] = X, data[1] = Y, data[2] = Z - */ - void getAccelero( float *data ); - - /** - * Sets the Gyro full-scale range - * - * Macros: MPU6050_GYRO_RANGE_250 - MPU6050_GYRO_RANGE_500 - MPU6050_GYRO_RANGE_1000 - MPU6050_GYRO_RANGE_2000 - * - * @param range - The two bits that set the full-scale range (use the predefined macros) - */ - void setGyroRange(char range); - - /** - * Reads the gyro x-axis. - * - * @return 16-bit signed integer x-axis gyro data - */ - int getGyroRawX( void ); - - /** - * Reads the gyro y-axis. - * - * @return 16-bit signed integer y-axis gyro data - */ - int getGyroRawY( void ); - - /** - * Reads the gyro z-axis. - * - * @return 16-bit signed integer z-axis gyro data - */ - int getGyroRawZ( void ); - - /** - * Reads all gyro data. - * - * @param data - pointer to signed integer array with length three: data[0] = X, data[1] = Y, data[2] = Z - */ - void getGyroRaw( int *data ); - - /** - * Reads all gyro data, gives the gyro in rad/s - * - * Function uses the last setup value of the full scale range, if you manually set in another range, this won't work. - * - * @param data - pointer to float array with length three: data[0] = X, data[1] = Y, data[2] = Z - */ - void getGyro( float *data); - - /** - * Reads temperature data. - * - * @return 16 bit signed integer with the raw temperature register value - */ - int getTempRaw( void ); - - /** - * Returns current temperature - * - * @returns float with the current temperature - */ - float getTemp( void ); - - /** - * Sets the sleep mode of the MPU6050 - * - * @param state - true for sleeping, false for wake up - */ - void setSleepMode( bool state ); - - - /** - * Writes data to the device, could be private, but public is handy so you can transmit directly to the MPU. - * - * @param adress - register address to write to - * @param data - data to write - */ - void write( char address, char data); - - /** - * Read data from the device, could be private, but public is handy so you can transmit directly to the MPU. - * - * @param adress - register address to write to - * @return - data from the register specified by RA - */ - char read( char adress); - - /** - * Read multtiple regigsters from the device, more efficient than using multiple normal reads. - * - * @param adress - register address to write to - * @param length - number of bytes to read - * @param data - pointer where the data needs to be written to - */ - void read( char adress, char *data, int length); - - - - - private: - - I2C connection; - char currentAcceleroRange; - char currentGyroRange; - - +/* Sensor datas to be used in program */ +extern float ax,ay,az; +extern float gx,gy,gz; +extern int16_t accelData[3],gyroData[3],tempData; +extern float accelBias[3], gyroBias[3]; + +/* Function Prototypes */ +class MPU6050 +{ + protected: + public: + void getAres(); + void getGres(); + void writeByte(uint8_t address, uint8_t subAddress, uint8_t data); + char readByte(uint8_t address, uint8_t subAddress); + void readBytes(uint8_t address, uint8_t subAddress, uint8_t byteNum, uint8_t* dest); + void whoAmI(); + void init(); + void reset(); + void readAccelData(int16_t* dest); + void readGyroData(int16_t* dest); + int16_t readTempData(); + void calibrate(float* dest1, float* dest2); + void complementaryFilter(float* pitch, float* roll); }; - - -#endif +#endif \ No newline at end of file