Songw Jiang
Not a real MPU6050 but imc-20689
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