Montvydas Klumbys / MPU6050

Thanks Erik Olieman for his beta library, that saved me a huge amount of time when getting Raw data from MPU6050 module! I was able to update this library by adding additional functions, which would allow a fast angle calculation by using complementary filter. I will probably be updating this library more soon to add additional functionality or make some changes that would look sensible.

Dependents:   QuadcopterProgram 3DTracking ControlYokutan2017 Gyro ... more

Fork of MPU6050 by Erik -

MPU6050.h@11:9b414412b09e, 2015-02-20 (annotated)

Committer:
moklumbys
Date:
Fri Feb 20 00:13:01 2015 +0000
Revision:
11:9b414412b09e
Parent:
10:bd9665d14241 
I modified some small things in the library and additionally added some functions for calculating actual angle values from the one read by the sensor.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Sissors 1:a3366f09e95c 1 /*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.
Sissors 0:6757f7363a9f 2 If it is half of what you expected, and you still are on the correct planet, you got an engineering sample
Sissors 0:6757f7363a9f 3 */
Sissors 0:6757f7363a9f 4
Sissors 0:6757f7363a9f 5
Sissors 0:6757f7363a9f 6 #ifndef MPU6050_H
Sissors 0:6757f7363a9f 7 #define MPU6050_H
Sissors 0:6757f7363a9f 8
Sissors 0:6757f7363a9f 9 /**
Sissors 0:6757f7363a9f 10 * Includes
Sissors 0:6757f7363a9f 11 */
Sissors 0:6757f7363a9f 12 #include "mbed.h"
Sissors 1:a3366f09e95c 13
Sissors 0:6757f7363a9f 14
Sissors 0:6757f7363a9f 15 /**
Sissors 0:6757f7363a9f 16 * Defines
Sissors 0:6757f7363a9f 17 */
Sissors 1:a3366f09e95c 18 #ifndef MPU6050_ADDRESS
moklumbys 3:187152513f8d 19 #define MPU6050_ADDRESS 0x68 // address pin low (GND), default for InvenSense evaluation board
Sissors 1:a3366f09e95c 20 #endif
Sissors 1:a3366f09e95c 21
Sissors 0:6757f7363a9f 22 #ifdef MPU6050_ES
Sissors 0:6757f7363a9f 23 #define DOUBLE_ACCELERO
Sissors 0:6757f7363a9f 24 #endif
Sissors 0:6757f7363a9f 25
Sissors 0:6757f7363a9f 26 /**
Sissors 0:6757f7363a9f 27 * Registers
Sissors 0:6757f7363a9f 28 */
Sissors 0:6757f7363a9f 29 #define MPU6050_CONFIG_REG 0x1A
Sissors 0:6757f7363a9f 30 #define MPU6050_GYRO_CONFIG_REG 0x1B
Sissors 0:6757f7363a9f 31 #define MPU6050_ACCELERO_CONFIG_REG 0x1C
Sissors 0:6757f7363a9f 32
Sissors 0:6757f7363a9f 33 #define MPU6050_INT_PIN_CFG 0x37
Sissors 0:6757f7363a9f 34
Sissors 0:6757f7363a9f 35 #define MPU6050_ACCEL_XOUT_H_REG 0x3B
Sissors 0:6757f7363a9f 36 #define MPU6050_ACCEL_YOUT_H_REG 0x3D
Sissors 0:6757f7363a9f 37 #define MPU6050_ACCEL_ZOUT_H_REG 0x3F
Sissors 0:6757f7363a9f 38
Sissors 0:6757f7363a9f 39 #define MPU6050_TEMP_H_REG 0x41
Sissors 0:6757f7363a9f 40
Sissors 0:6757f7363a9f 41 #define MPU6050_GYRO_XOUT_H_REG 0x43
Sissors 0:6757f7363a9f 42 #define MPU6050_GYRO_YOUT_H_REG 0x45
Sissors 0:6757f7363a9f 43 #define MPU6050_GYRO_ZOUT_H_REG 0x47
Sissors 0:6757f7363a9f 44
Sissors 0:6757f7363a9f 45
Sissors 0:6757f7363a9f 46
Sissors 0:6757f7363a9f 47 #define MPU6050_PWR_MGMT_1_REG 0x6B
Sissors 0:6757f7363a9f 48 #define MPU6050_WHO_AM_I_REG 0x75
Sissors 0:6757f7363a9f 49
Sissors 0:6757f7363a9f 50
Sissors 0:6757f7363a9f 51
Sissors 0:6757f7363a9f 52 /**
Sissors 0:6757f7363a9f 53 * Definitions
Sissors 0:6757f7363a9f 54 */
Sissors 0:6757f7363a9f 55 #define MPU6050_SLP_BIT 6
Sissors 0:6757f7363a9f 56 #define MPU6050_BYPASS_BIT 1
Sissors 0:6757f7363a9f 57
Sissors 0:6757f7363a9f 58 #define MPU6050_BW_256 0
Sissors 0:6757f7363a9f 59 #define MPU6050_BW_188 1
Sissors 0:6757f7363a9f 60 #define MPU6050_BW_98 2
Sissors 0:6757f7363a9f 61 #define MPU6050_BW_42 3
Sissors 0:6757f7363a9f 62 #define MPU6050_BW_20 4
Sissors 0:6757f7363a9f 63 #define MPU6050_BW_10 5
Sissors 0:6757f7363a9f 64 #define MPU6050_BW_5 6
Sissors 0:6757f7363a9f 65
Sissors 0:6757f7363a9f 66 #define MPU6050_ACCELERO_RANGE_2G 0
Sissors 0:6757f7363a9f 67 #define MPU6050_ACCELERO_RANGE_4G 1
Sissors 0:6757f7363a9f 68 #define MPU6050_ACCELERO_RANGE_8G 2
Sissors 0:6757f7363a9f 69 #define MPU6050_ACCELERO_RANGE_16G 3
Sissors 0:6757f7363a9f 70
Sissors 0:6757f7363a9f 71 #define MPU6050_GYRO_RANGE_250 0
Sissors 0:6757f7363a9f 72 #define MPU6050_GYRO_RANGE_500 1
Sissors 0:6757f7363a9f 73 #define MPU6050_GYRO_RANGE_1000 2
Sissors 0:6757f7363a9f 74 #define MPU6050_GYRO_RANGE_2000 3
Sissors 0:6757f7363a9f 75
moklumbys 10:bd9665d14241 76 //interrupt address
moklumbys 10:bd9665d14241 77 #define MPU6050_RA_INT_ENABLE 0x38
moklumbys 4:268d3fcb92ba 78 //define how the accelerometer is placed on surface
moklumbys 4:268d3fcb92ba 79 #define X_AXIS 1
moklumbys 4:268d3fcb92ba 80 #define Y_AXIS 2
moklumbys 4:268d3fcb92ba 81 #define Z_AXIS 0
moklumbys 10:bd9665d14241 82 //translation from radians to degrees
moklumbys 6:502448484f91 83 #define RADIANS_TO_DEGREES 180/3.1415926536
moklumbys 10:bd9665d14241 84 //constant used for the complementary filter, which ranges usually from 0.9 to 1.0
moklumbys 9:898effccce30 85 #define ALPHA 0.96 //filter constant
moklumbys 10:bd9665d14241 86 //scale the gyro
moklumbys 10:bd9665d14241 87 #define GYRO_SCALE 2.7176
moklumbys 7:56e591a74939 88
Sissors 2:5c63e20c50f3 89 /** MPU6050 IMU library.
Sissors 2:5c63e20c50f3 90 *
Sissors 2:5c63e20c50f3 91 * Example:
Sissors 2:5c63e20c50f3 92 * @code
Sissors 2:5c63e20c50f3 93 * Later, maybe
Sissors 2:5c63e20c50f3 94 * @endcode
Sissors 2:5c63e20c50f3 95 */
Sissors 0:6757f7363a9f 96 class MPU6050 {
Sissors 0:6757f7363a9f 97 public:
Sissors 0:6757f7363a9f 98 /**
Sissors 0:6757f7363a9f 99 * Constructor.
Sissors 0:6757f7363a9f 100 *
Sissors 0:6757f7363a9f 101 * Sleep mode of MPU6050 is immediatly disabled
Sissors 0:6757f7363a9f 102 *
Sissors 0:6757f7363a9f 103 * @param sda - mbed pin to use for the SDA I2C line.
Sissors 0:6757f7363a9f 104 * @param scl - mbed pin to use for the SCL I2C line.
Sissors 0:6757f7363a9f 105 */
Sissors 0:6757f7363a9f 106 MPU6050(PinName sda, PinName scl);
Sissors 0:6757f7363a9f 107
Sissors 0:6757f7363a9f 108
Sissors 0:6757f7363a9f 109 /**
Sissors 0:6757f7363a9f 110 * Tests the I2C connection by reading the WHO_AM_I register.
Sissors 0:6757f7363a9f 111 *
Sissors 0:6757f7363a9f 112 * @return True for a working connection, false for an error
Sissors 0:6757f7363a9f 113 */
Sissors 0:6757f7363a9f 114 bool testConnection( void );
Sissors 0:6757f7363a9f 115
Sissors 0:6757f7363a9f 116 /**
Sissors 0:6757f7363a9f 117 * Sets the bandwidth of the digital low-pass filter
Sissors 0:6757f7363a9f 118 *
Sissors 0:6757f7363a9f 119 * Macros: MPU6050_BW_256 - MPU6050_BW_188 - MPU6050_BW_98 - MPU6050_BW_42 - MPU6050_BW_20 - MPU6050_BW_10 - MPU6050_BW_5
Sissors 0:6757f7363a9f 120 * Last number is the gyro's BW in Hz (accelero BW is virtually identical)
Sissors 0:6757f7363a9f 121 *
Sissors 0:6757f7363a9f 122 * @param BW - The three bits that set the bandwidth (use the predefined macros)
Sissors 0:6757f7363a9f 123 */
Sissors 0:6757f7363a9f 124 void setBW( char BW );
Sissors 0:6757f7363a9f 125
Sissors 0:6757f7363a9f 126 /**
Sissors 0:6757f7363a9f 127 * 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)
Sissors 0:6757f7363a9f 128 *
Sissors 0:6757f7363a9f 129 * @param state - Enables/disables the I2C bypass mode
Sissors 0:6757f7363a9f 130 */
Sissors 0:6757f7363a9f 131 void setI2CBypass ( bool state );
Sissors 0:6757f7363a9f 132
Sissors 0:6757f7363a9f 133 /**
Sissors 0:6757f7363a9f 134 * Sets the Accelero full-scale range
Sissors 0:6757f7363a9f 135 *
Sissors 0:6757f7363a9f 136 * Macros: MPU6050_ACCELERO_RANGE_2G - MPU6050_ACCELERO_RANGE_4G - MPU6050_ACCELERO_RANGE_8G - MPU6050_ACCELERO_RANGE_16G
Sissors 0:6757f7363a9f 137 *
Sissors 0:6757f7363a9f 138 * @param range - The two bits that set the full-scale range (use the predefined macros)
Sissors 0:6757f7363a9f 139 */
Sissors 0:6757f7363a9f 140 void setAcceleroRange(char range);
Sissors 0:6757f7363a9f 141
Sissors 0:6757f7363a9f 142 /**
Sissors 0:6757f7363a9f 143 * Reads the accelero x-axis.
Sissors 0:6757f7363a9f 144 *
Sissors 0:6757f7363a9f 145 * @return 16-bit signed integer x-axis accelero data
Sissors 0:6757f7363a9f 146 */
Sissors 0:6757f7363a9f 147 int getAcceleroRawX( void );
Sissors 0:6757f7363a9f 148
Sissors 0:6757f7363a9f 149 /**
Sissors 0:6757f7363a9f 150 * Reads the accelero y-axis.
Sissors 0:6757f7363a9f 151 *
Sissors 0:6757f7363a9f 152 * @return 16-bit signed integer y-axis accelero data
Sissors 0:6757f7363a9f 153 */
Sissors 0:6757f7363a9f 154 int getAcceleroRawY( void );
Sissors 0:6757f7363a9f 155
Sissors 0:6757f7363a9f 156 /**
Sissors 0:6757f7363a9f 157 * Reads the accelero z-axis.
Sissors 0:6757f7363a9f 158 *
Sissors 0:6757f7363a9f 159 * @return 16-bit signed integer z-axis accelero data
Sissors 0:6757f7363a9f 160 */
Sissors 0:6757f7363a9f 161 int getAcceleroRawZ( void );
Sissors 0:6757f7363a9f 162
Sissors 0:6757f7363a9f 163 /**
Sissors 0:6757f7363a9f 164 * Reads all accelero data.
Sissors 0:6757f7363a9f 165 *
Sissors 0:6757f7363a9f 166 * @param data - pointer to signed integer array with length three: data[0] = X, data[1] = Y, data[2] = Z
Sissors 0:6757f7363a9f 167 */
Sissors 0:6757f7363a9f 168 void getAcceleroRaw( int *data );
Sissors 0:6757f7363a9f 169
Sissors 0:6757f7363a9f 170 /**
Sissors 0:6757f7363a9f 171 * Reads all accelero data, gives the acceleration in m/s2
Sissors 0:6757f7363a9f 172 *
Sissors 0:6757f7363a9f 173 * Function uses the last setup value of the full scale range, if you manually set in another range, this won't work.
Sissors 0:6757f7363a9f 174 *
Sissors 0:6757f7363a9f 175 * @param data - pointer to float array with length three: data[0] = X, data[1] = Y, data[2] = Z
Sissors 0:6757f7363a9f 176 */
Sissors 0:6757f7363a9f 177 void getAccelero( float *data );
Sissors 0:6757f7363a9f 178
Sissors 0:6757f7363a9f 179 /**
Sissors 0:6757f7363a9f 180 * Sets the Gyro full-scale range
Sissors 0:6757f7363a9f 181 *
Sissors 0:6757f7363a9f 182 * Macros: MPU6050_GYRO_RANGE_250 - MPU6050_GYRO_RANGE_500 - MPU6050_GYRO_RANGE_1000 - MPU6050_GYRO_RANGE_2000
Sissors 0:6757f7363a9f 183 *
Sissors 0:6757f7363a9f 184 * @param range - The two bits that set the full-scale range (use the predefined macros)
Sissors 0:6757f7363a9f 185 */
Sissors 0:6757f7363a9f 186 void setGyroRange(char range);
Sissors 0:6757f7363a9f 187
Sissors 0:6757f7363a9f 188 /**
Sissors 0:6757f7363a9f 189 * Reads the gyro x-axis.
Sissors 0:6757f7363a9f 190 *
Sissors 0:6757f7363a9f 191 * @return 16-bit signed integer x-axis gyro data
Sissors 0:6757f7363a9f 192 */
Sissors 0:6757f7363a9f 193 int getGyroRawX( void );
Sissors 0:6757f7363a9f 194
Sissors 0:6757f7363a9f 195 /**
Sissors 0:6757f7363a9f 196 * Reads the gyro y-axis.
Sissors 0:6757f7363a9f 197 *
Sissors 0:6757f7363a9f 198 * @return 16-bit signed integer y-axis gyro data
Sissors 0:6757f7363a9f 199 */
Sissors 0:6757f7363a9f 200 int getGyroRawY( void );
Sissors 0:6757f7363a9f 201
Sissors 0:6757f7363a9f 202 /**
Sissors 0:6757f7363a9f 203 * Reads the gyro z-axis.
Sissors 0:6757f7363a9f 204 *
Sissors 0:6757f7363a9f 205 * @return 16-bit signed integer z-axis gyro data
Sissors 0:6757f7363a9f 206 */
Sissors 0:6757f7363a9f 207 int getGyroRawZ( void );
Sissors 0:6757f7363a9f 208
Sissors 0:6757f7363a9f 209 /**
Sissors 0:6757f7363a9f 210 * Reads all gyro data.
Sissors 0:6757f7363a9f 211 *
Sissors 0:6757f7363a9f 212 * @param data - pointer to signed integer array with length three: data[0] = X, data[1] = Y, data[2] = Z
Sissors 0:6757f7363a9f 213 */
Sissors 0:6757f7363a9f 214 void getGyroRaw( int *data );
Sissors 0:6757f7363a9f 215
Sissors 0:6757f7363a9f 216 /**
Sissors 0:6757f7363a9f 217 * Reads all gyro data, gives the gyro in rad/s
Sissors 0:6757f7363a9f 218 *
Sissors 0:6757f7363a9f 219 * Function uses the last setup value of the full scale range, if you manually set in another range, this won't work.
Sissors 0:6757f7363a9f 220 *
Sissors 0:6757f7363a9f 221 * @param data - pointer to float array with length three: data[0] = X, data[1] = Y, data[2] = Z
Sissors 0:6757f7363a9f 222 */
Sissors 0:6757f7363a9f 223 void getGyro( float *data);
Sissors 0:6757f7363a9f 224
Sissors 0:6757f7363a9f 225 /**
Sissors 0:6757f7363a9f 226 * Reads temperature data.
Sissors 0:6757f7363a9f 227 *
Sissors 0:6757f7363a9f 228 * @return 16 bit signed integer with the raw temperature register value
Sissors 0:6757f7363a9f 229 */
Sissors 0:6757f7363a9f 230 int getTempRaw( void );
Sissors 0:6757f7363a9f 231
Sissors 0:6757f7363a9f 232 /**
Sissors 0:6757f7363a9f 233 * Returns current temperature
Sissors 0:6757f7363a9f 234 *
Sissors 0:6757f7363a9f 235 * @returns float with the current temperature
Sissors 0:6757f7363a9f 236 */
Sissors 0:6757f7363a9f 237 float getTemp( void );
Sissors 0:6757f7363a9f 238
Sissors 0:6757f7363a9f 239 /**
Sissors 0:6757f7363a9f 240 * Sets the sleep mode of the MPU6050
Sissors 0:6757f7363a9f 241 *
Sissors 0:6757f7363a9f 242 * @param state - true for sleeping, false for wake up
Sissors 0:6757f7363a9f 243 */
Sissors 0:6757f7363a9f 244 void setSleepMode( bool state );
Sissors 0:6757f7363a9f 245
Sissors 0:6757f7363a9f 246
Sissors 0:6757f7363a9f 247 /**
Sissors 0:6757f7363a9f 248 * Writes data to the device, could be private, but public is handy so you can transmit directly to the MPU.
Sissors 0:6757f7363a9f 249 *
Sissors 0:6757f7363a9f 250 * @param adress - register address to write to
Sissors 0:6757f7363a9f 251 * @param data - data to write
Sissors 0:6757f7363a9f 252 */
Sissors 0:6757f7363a9f 253 void write( char address, char data);
Sissors 0:6757f7363a9f 254
Sissors 0:6757f7363a9f 255 /**
Sissors 0:6757f7363a9f 256 * Read data from the device, could be private, but public is handy so you can transmit directly to the MPU.
Sissors 0:6757f7363a9f 257 *
Sissors 0:6757f7363a9f 258 * @param adress - register address to write to
Sissors 0:6757f7363a9f 259 * @return - data from the register specified by RA
Sissors 0:6757f7363a9f 260 */
Sissors 0:6757f7363a9f 261 char read( char adress);
Sissors 0:6757f7363a9f 262
Sissors 0:6757f7363a9f 263 /**
Sissors 0:6757f7363a9f 264 * Read multtiple regigsters from the device, more efficient than using multiple normal reads.
Sissors 0:6757f7363a9f 265 *
Sissors 0:6757f7363a9f 266 * @param adress - register address to write to
Sissors 0:6757f7363a9f 267 * @param length - number of bytes to read
Sissors 0:6757f7363a9f 268 * @param data - pointer where the data needs to be written to
Sissors 0:6757f7363a9f 269 */
Sissors 0:6757f7363a9f 270 void read( char adress, char *data, int length);
Sissors 0:6757f7363a9f 271
moklumbys 11:9b414412b09e 272 /**
moklumbys 11:9b414412b09e 273 * function for calculating the angle from the accelerometer.
moklumbys 11:9b414412b09e 274 * it takes 3 values which correspond acceleration in X, Y and Z direction and calculates angles in degrees
moklumbys 11:9b414412b09e 275 * for pitch, roll and one more direction.. (NOT YAW!)
moklumbys 11:9b414412b09e 276 *
moklumbys 11:9b414412b09e 277 * @param data - angle calculated using only accelerometer
moklumbys 11:9b414412b09e 278 *
moklumbys 11:9b414412b09e 279 */
moklumbys 11:9b414412b09e 280 void getAcceleroAngle( float *data );
moklumbys 10:bd9665d14241 281
moklumbys 10:bd9665d14241 282
moklumbys 11:9b414412b09e 283 /**function which allows to produce the offset values for gyro and accelerometer.
moklumbys 11:9b414412b09e 284 * offset for gyro is simply a value, which needs to be substracted from original gyro rad/sec speed
moklumbys 11:9b414412b09e 285 * but offset for accelerometer is calculated in angles... later on might change that
moklumbys 11:9b414412b09e 286 * function simply takes the number of samples to be taken and calculated the average
moklumbys 11:9b414412b09e 287 *
moklumbys 11:9b414412b09e 288 * @param accOffset - accelerometer offset in angle
moklumbys 11:9b414412b09e 289 * @param gyroOffset - gyroscope offset in rad/s
moklumbys 11:9b414412b09e 290 * @param sampleSize - number of samples to be taken for calculating offsets
moklumbys 11:9b414412b09e 291 *
moklumbys 11:9b414412b09e 292 */
moklumbys 5:5873df1e58be 293 void getOffset(float *accOffset, float *gyroOffset, int sampleSize);
moklumbys 10:bd9665d14241 294
moklumbys 11:9b414412b09e 295 /**
moklumbys 11:9b414412b09e 296 * function for computing the angle, when accelerometer angle offset and gyro offset are both known.
moklumbys 11:9b414412b09e 297 * we also need to know how much time passed from previous calculation to now
moklumbys 11:9b414412b09e 298 * it produces the angle in degrees. However angles are produced from -90.0 to 90.0 degrees
moklumbys 11:9b414412b09e 299 * if anyone need smth different, they can update this library...
moklumbys 11:9b414412b09e 300 *
moklumbys 11:9b414412b09e 301 * @param angle - calculated accurate angle from complemetary filter
moklumbys 11:9b414412b09e 302 * @param accOffset - offset in angle for the accelerometer
moklumbys 11:9b414412b09e 303 * @param gyroOffset - offset in rad/s for the gyroscope
moklumbys 11:9b414412b09e 304 * @param interval - time before previous angle calculation and now
moklumbys 11:9b414412b09e 305 *
moklumbys 11:9b414412b09e 306 */
moklumbys 11:9b414412b09e 307 void computeAngle (float *angle, float *accOffset, float *gyroOffset, float interval);
moklumbys 10:bd9665d14241 308
moklumbys 11:9b414412b09e 309 ///function, which enables interrupts on MPU6050 INT pin
moklumbys 8:b1570b99df9e 310 void enableInt( void );
moklumbys 10:bd9665d14241 311
moklumbys 11:9b414412b09e 312 ///disables interrupts
moklumbys 8:b1570b99df9e 313 void disableInt( void );
moklumbys 10:bd9665d14241 314
moklumbys 11:9b414412b09e 315 /**function which sets the alpha value - constant for the complementary filter. default alpha = 0.97
moklumbys 11:9b414412b09e 316 *
moklumbys 11:9b414412b09e 317 * @param val - value the alpha (complementary filter constant) should be set to
moklumbys 11:9b414412b09e 318 *
moklumbys 11:9b414412b09e 319 */
moklumbys 9:898effccce30 320 void setAlpha(float val);
moklumbys 8:b1570b99df9e 321
Sissors 0:6757f7363a9f 322 private:
Sissors 0:6757f7363a9f 323
Sissors 1:a3366f09e95c 324 I2C connection;
Sissors 0:6757f7363a9f 325 char currentAcceleroRange;
Sissors 0:6757f7363a9f 326 char currentGyroRange;
moklumbys 10:bd9665d14241 327 float alpha;
Sissors 0:6757f7363a9f 328
Sissors 0:6757f7363a9f 329 };
Sissors 0:6757f7363a9f 330
Sissors 0:6757f7363a9f 331
Sissors 0:6757f7363a9f 332
Sissors 0:6757f7363a9f 333 #endif