Sheila Pham / Mbed OS IMU6050Ver1

Added for gyro and testing

MPU6050.h@14:365c1c1bf6ee, 2019-07-04 (annotated)

Committer:
sepham
Date:
Thu Jul 04 18:25:35 2019 +0000
Revision:
14:365c1c1bf6ee
Parent:
11:9b414412b09e 
IMU tof more gyro and testing

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