Franz Wolf / MPU6050

MPU6050 library that is kinda beta and will probably never leave beta but it might help some people.

Dependents:   NUClight_TEST_ALL_V2 NUClight-V3-HW-BORAD-TEST

MPU6050.cpp@3:2bf425b00c93, 2019-02-13 (annotated)

Committer:
wf
Date:
Wed Feb 13 11:33:19 2019 +0000
Revision:
3:2bf425b00c93
Parent:
1:a3366f09e95c 
Testprogram for nucLIGHT

Who changed what in which revision?

UserRevisionLine numberNew contents of line
wf 3:2bf425b00c93 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.
wf 3:2bf425b00c93 2 If it is half of what you expected, and you still are on the correct planet, you got an engineering sample
wf 3:2bf425b00c93 3 */
wf 3:2bf425b00c93 4
wf 3:2bf425b00c93 5
wf 3:2bf425b00c93 6 #ifndef MPU6050_H
wf 3:2bf425b00c93 7 #define MPU6050_H
wf 3:2bf425b00c93 8
Sissors 0:6757f7363a9f 9 /**
Sissors 0:6757f7363a9f 10 * Includes
Sissors 0:6757f7363a9f 11 */
wf 3:2bf425b00c93 12 #include "mbed.h"
wf 3:2bf425b00c93 13
wf 3:2bf425b00c93 14
wf 3:2bf425b00c93 15 /**
wf 3:2bf425b00c93 16 * Defines
wf 3:2bf425b00c93 17 */
wf 3:2bf425b00c93 18 #ifndef MPU6050_ADDRESS
wf 3:2bf425b00c93 19 #define MPU6050_ADDRESS 0x68 // address pin low (GND), default for InvenSense evaluation board
wf 3:2bf425b00c93 20 #endif
wf 3:2bf425b00c93 21
wf 3:2bf425b00c93 22 #ifdef MPU6050_ES
wf 3:2bf425b00c93 23 #define DOUBLE_ACCELERO
wf 3:2bf425b00c93 24 #endif
wf 3:2bf425b00c93 25
wf 3:2bf425b00c93 26 /**
wf 3:2bf425b00c93 27 * Registers
wf 3:2bf425b00c93 28 */
wf 3:2bf425b00c93 29 #define MPU6050_CONFIG_REG 0x1A
wf 3:2bf425b00c93 30 #define MPU6050_GYRO_CONFIG_REG 0x1B
wf 3:2bf425b00c93 31 #define MPU6050_ACCELERO_CONFIG_REG 0x1C
wf 3:2bf425b00c93 32
wf 3:2bf425b00c93 33 #define MPU6050_INT_PIN_CFG 0x37
wf 3:2bf425b00c93 34
wf 3:2bf425b00c93 35 #define MPU6050_ACCEL_XOUT_H_REG 0x3B
wf 3:2bf425b00c93 36 #define MPU6050_ACCEL_YOUT_H_REG 0x3D
wf 3:2bf425b00c93 37 #define MPU6050_ACCEL_ZOUT_H_REG 0x3F
wf 3:2bf425b00c93 38
wf 3:2bf425b00c93 39 #define MPU6050_TEMP_H_REG 0x41
wf 3:2bf425b00c93 40
wf 3:2bf425b00c93 41 #define MPU6050_GYRO_XOUT_H_REG 0x43
wf 3:2bf425b00c93 42 #define MPU6050_GYRO_YOUT_H_REG 0x45
wf 3:2bf425b00c93 43 #define MPU6050_GYRO_ZOUT_H_REG 0x47
wf 3:2bf425b00c93 44
wf 3:2bf425b00c93 45
wf 3:2bf425b00c93 46
wf 3:2bf425b00c93 47 #define MPU6050_PWR_MGMT_1_REG 0x6B
wf 3:2bf425b00c93 48 #define MPU6050_WHO_AM_I_REG 0x75
wf 3:2bf425b00c93 49
wf 3:2bf425b00c93 50
wf 3:2bf425b00c93 51
wf 3:2bf425b00c93 52 /**
wf 3:2bf425b00c93 53 * Definitions
wf 3:2bf425b00c93 54 */
wf 3:2bf425b00c93 55 #define MPU6050_SLP_BIT 6
wf 3:2bf425b00c93 56 #define MPU6050_BYPASS_BIT 1
wf 3:2bf425b00c93 57
wf 3:2bf425b00c93 58 #define MPU6050_BW_256 0
wf 3:2bf425b00c93 59 #define MPU6050_BW_188 1
wf 3:2bf425b00c93 60 #define MPU6050_BW_98 2
wf 3:2bf425b00c93 61 #define MPU6050_BW_42 3
wf 3:2bf425b00c93 62 #define MPU6050_BW_20 4
wf 3:2bf425b00c93 63 #define MPU6050_BW_10 5
wf 3:2bf425b00c93 64 #define MPU6050_BW_5 6
wf 3:2bf425b00c93 65
wf 3:2bf425b00c93 66 #define MPU6050_ACCELERO_RANGE_2G 0
wf 3:2bf425b00c93 67 #define MPU6050_ACCELERO_RANGE_4G 1
wf 3:2bf425b00c93 68 #define MPU6050_ACCELERO_RANGE_8G 2
wf 3:2bf425b00c93 69 #define MPU6050_ACCELERO_RANGE_16G 3
wf 3:2bf425b00c93 70
wf 3:2bf425b00c93 71 #define MPU6050_GYRO_RANGE_250 0
wf 3:2bf425b00c93 72 #define MPU6050_GYRO_RANGE_500 1
wf 3:2bf425b00c93 73 #define MPU6050_GYRO_RANGE_1000 2
wf 3:2bf425b00c93 74 #define MPU6050_GYRO_RANGE_2000 3
wf 3:2bf425b00c93 75
wf 3:2bf425b00c93 76 //define how the accelerometer is placed on surface
wf 3:2bf425b00c93 77 #define X_AXIS 1
wf 3:2bf425b00c93 78 #define Y_AXIS 2
wf 3:2bf425b00c93 79 #define Z_AXIS 0
wf 3:2bf425b00c93 80
wf 3:2bf425b00c93 81 #define RADIANS_TO_DEGREES 180/3.1415926536
wf 3:2bf425b00c93 82
wf 3:2bf425b00c93 83 #define ALPHA 0.97 //filter constant
wf 3:2bf425b00c93 84
wf 3:2bf425b00c93 85 #define GYRO_SCALE 2.31 //scale the gyro
wf 3:2bf425b00c93 86
wf 3:2bf425b00c93 87 /** MPU6050 IMU library.
wf 3:2bf425b00c93 88 *
wf 3:2bf425b00c93 89 * Example:
wf 3:2bf425b00c93 90 * @code
wf 3:2bf425b00c93 91 * Later, maybe
wf 3:2bf425b00c93 92 * @endcode
wf 3:2bf425b00c93 93 */
wf 3:2bf425b00c93 94 class MPU6050 {
wf 3:2bf425b00c93 95 public:
wf 3:2bf425b00c93 96 /**
wf 3:2bf425b00c93 97 * Constructor.
wf 3:2bf425b00c93 98 *
wf 3:2bf425b00c93 99 * Sleep mode of MPU6050 is immediatly disabled
wf 3:2bf425b00c93 100 *
wf 3:2bf425b00c93 101 * @param sda - mbed pin to use for the SDA I2C line.
wf 3:2bf425b00c93 102 * @param scl - mbed pin to use for the SCL I2C line.
wf 3:2bf425b00c93 103 */
wf 3:2bf425b00c93 104 MPU6050(PinName sda, PinName scl);
wf 3:2bf425b00c93 105
wf 3:2bf425b00c93 106
wf 3:2bf425b00c93 107 /**
wf 3:2bf425b00c93 108 * Tests the I2C connection by reading the WHO_AM_I register.
wf 3:2bf425b00c93 109 *
wf 3:2bf425b00c93 110 * @return True for a working connection, false for an error
wf 3:2bf425b00c93 111 */
wf 3:2bf425b00c93 112 bool testConnection( void );
wf 3:2bf425b00c93 113
wf 3:2bf425b00c93 114 /**
wf 3:2bf425b00c93 115 * Sets the bandwidth of the digital low-pass filter
wf 3:2bf425b00c93 116 *
wf 3:2bf425b00c93 117 * Macros: MPU6050_BW_256 - MPU6050_BW_188 - MPU6050_BW_98 - MPU6050_BW_42 - MPU6050_BW_20 - MPU6050_BW_10 - MPU6050_BW_5
wf 3:2bf425b00c93 118 * Last number is the gyro's BW in Hz (accelero BW is virtually identical)
wf 3:2bf425b00c93 119 *
wf 3:2bf425b00c93 120 * @param BW - The three bits that set the bandwidth (use the predefined macros)
wf 3:2bf425b00c93 121 */
wf 3:2bf425b00c93 122 void setBW( char BW );
wf 3:2bf425b00c93 123
wf 3:2bf425b00c93 124 /**
wf 3:2bf425b00c93 125 * 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)
wf 3:2bf425b00c93 126 *
wf 3:2bf425b00c93 127 * @param state - Enables/disables the I2C bypass mode
wf 3:2bf425b00c93 128 */
wf 3:2bf425b00c93 129 void setI2CBypass ( bool state );
wf 3:2bf425b00c93 130
wf 3:2bf425b00c93 131 /**
wf 3:2bf425b00c93 132 * Sets the Accelero full-scale range
wf 3:2bf425b00c93 133 *
wf 3:2bf425b00c93 134 * Macros: MPU6050_ACCELERO_RANGE_2G - MPU6050_ACCELERO_RANGE_4G - MPU6050_ACCELERO_RANGE_8G - MPU6050_ACCELERO_RANGE_16G
wf 3:2bf425b00c93 135 *
wf 3:2bf425b00c93 136 * @param range - The two bits that set the full-scale range (use the predefined macros)
wf 3:2bf425b00c93 137 */
wf 3:2bf425b00c93 138 void setAcceleroRange(char range);
wf 3:2bf425b00c93 139
wf 3:2bf425b00c93 140 /**
wf 3:2bf425b00c93 141 * Reads the accelero x-axis.
wf 3:2bf425b00c93 142 *
wf 3:2bf425b00c93 143 * @return 16-bit signed integer x-axis accelero data
wf 3:2bf425b00c93 144 */
wf 3:2bf425b00c93 145 int getAcceleroRawX( void );
wf 3:2bf425b00c93 146
wf 3:2bf425b00c93 147 /**
wf 3:2bf425b00c93 148 * Reads the accelero y-axis.
wf 3:2bf425b00c93 149 *
wf 3:2bf425b00c93 150 * @return 16-bit signed integer y-axis accelero data
wf 3:2bf425b00c93 151 */
wf 3:2bf425b00c93 152 int getAcceleroRawY( void );
wf 3:2bf425b00c93 153
wf 3:2bf425b00c93 154 /**
wf 3:2bf425b00c93 155 * Reads the accelero z-axis.
wf 3:2bf425b00c93 156 *
wf 3:2bf425b00c93 157 * @return 16-bit signed integer z-axis accelero data
wf 3:2bf425b00c93 158 */
wf 3:2bf425b00c93 159 int getAcceleroRawZ( void );
wf 3:2bf425b00c93 160
wf 3:2bf425b00c93 161 /**
wf 3:2bf425b00c93 162 * Reads all accelero data.
wf 3:2bf425b00c93 163 *
wf 3:2bf425b00c93 164 * @param data - pointer to signed integer array with length three: data[0] = X, data[1] = Y, data[2] = Z
wf 3:2bf425b00c93 165 */
wf 3:2bf425b00c93 166 void getAcceleroRaw( int *data );
wf 3:2bf425b00c93 167
wf 3:2bf425b00c93 168 /**
wf 3:2bf425b00c93 169 * Reads all accelero data, gives the acceleration in m/s2
wf 3:2bf425b00c93 170 *
wf 3:2bf425b00c93 171 * Function uses the last setup value of the full scale range, if you manually set in another range, this won't work.
wf 3:2bf425b00c93 172 *
wf 3:2bf425b00c93 173 * @param data - pointer to float array with length three: data[0] = X, data[1] = Y, data[2] = Z
wf 3:2bf425b00c93 174 */
wf 3:2bf425b00c93 175 void getAccelero( float *data );
wf 3:2bf425b00c93 176
wf 3:2bf425b00c93 177 /**
wf 3:2bf425b00c93 178 * Sets the Gyro full-scale range
wf 3:2bf425b00c93 179 *
wf 3:2bf425b00c93 180 * Macros: MPU6050_GYRO_RANGE_250 - MPU6050_GYRO_RANGE_500 - MPU6050_GYRO_RANGE_1000 - MPU6050_GYRO_RANGE_2000
wf 3:2bf425b00c93 181 *
wf 3:2bf425b00c93 182 * @param range - The two bits that set the full-scale range (use the predefined macros)
wf 3:2bf425b00c93 183 */
wf 3:2bf425b00c93 184 void setGyroRange(char range);
wf 3:2bf425b00c93 185
wf 3:2bf425b00c93 186 /**
wf 3:2bf425b00c93 187 * Reads the gyro x-axis.
wf 3:2bf425b00c93 188 *
wf 3:2bf425b00c93 189 * @return 16-bit signed integer x-axis gyro data
wf 3:2bf425b00c93 190 */
wf 3:2bf425b00c93 191 int getGyroRawX( void );
wf 3:2bf425b00c93 192
wf 3:2bf425b00c93 193 /**
wf 3:2bf425b00c93 194 * Reads the gyro y-axis.
wf 3:2bf425b00c93 195 *
wf 3:2bf425b00c93 196 * @return 16-bit signed integer y-axis gyro data
wf 3:2bf425b00c93 197 */
wf 3:2bf425b00c93 198 int getGyroRawY( void );
wf 3:2bf425b00c93 199
wf 3:2bf425b00c93 200 /**
wf 3:2bf425b00c93 201 * Reads the gyro z-axis.
wf 3:2bf425b00c93 202 *
wf 3:2bf425b00c93 203 * @return 16-bit signed integer z-axis gyro data
wf 3:2bf425b00c93 204 */
wf 3:2bf425b00c93 205 int getGyroRawZ( void );
wf 3:2bf425b00c93 206
wf 3:2bf425b00c93 207 /**
wf 3:2bf425b00c93 208 * Reads all gyro data.
wf 3:2bf425b00c93 209 *
wf 3:2bf425b00c93 210 * @param data - pointer to signed integer array with length three: data[0] = X, data[1] = Y, data[2] = Z
wf 3:2bf425b00c93 211 */
wf 3:2bf425b00c93 212 void getGyroRaw( int *data );
wf 3:2bf425b00c93 213
wf 3:2bf425b00c93 214 /**
wf 3:2bf425b00c93 215 * Reads all gyro data, gives the gyro in rad/s
wf 3:2bf425b00c93 216 *
wf 3:2bf425b00c93 217 * Function uses the last setup value of the full scale range, if you manually set in another range, this won't work.
wf 3:2bf425b00c93 218 *
wf 3:2bf425b00c93 219 * @param data - pointer to float array with length three: data[0] = X, data[1] = Y, data[2] = Z
wf 3:2bf425b00c93 220 */
wf 3:2bf425b00c93 221 void getGyro( float *data);
wf 3:2bf425b00c93 222
wf 3:2bf425b00c93 223 /**
wf 3:2bf425b00c93 224 * Reads temperature data.
wf 3:2bf425b00c93 225 *
wf 3:2bf425b00c93 226 * @return 16 bit signed integer with the raw temperature register value
wf 3:2bf425b00c93 227 */
wf 3:2bf425b00c93 228 int getTempRaw( void );
wf 3:2bf425b00c93 229
wf 3:2bf425b00c93 230 /**
wf 3:2bf425b00c93 231 * Returns current temperature
wf 3:2bf425b00c93 232 *
wf 3:2bf425b00c93 233 * @returns float with the current temperature
wf 3:2bf425b00c93 234 */
wf 3:2bf425b00c93 235 float getTemp( void );
wf 3:2bf425b00c93 236
wf 3:2bf425b00c93 237 /**
wf 3:2bf425b00c93 238 * Sets the sleep mode of the MPU6050
wf 3:2bf425b00c93 239 *
wf 3:2bf425b00c93 240 * @param state - true for sleeping, false for wake up
wf 3:2bf425b00c93 241 */
wf 3:2bf425b00c93 242 void setSleepMode( bool state );
wf 3:2bf425b00c93 243
wf 3:2bf425b00c93 244
wf 3:2bf425b00c93 245 /**
wf 3:2bf425b00c93 246 * Writes data to the device, could be private, but public is handy so you can transmit directly to the MPU.
wf 3:2bf425b00c93 247 *
wf 3:2bf425b00c93 248 * @param adress - register address to write to
wf 3:2bf425b00c93 249 * @param data - data to write
wf 3:2bf425b00c93 250 */
wf 3:2bf425b00c93 251 void write( char address, char data);
wf 3:2bf425b00c93 252
wf 3:2bf425b00c93 253 /**
wf 3:2bf425b00c93 254 * Read data from the device, could be private, but public is handy so you can transmit directly to the MPU.
wf 3:2bf425b00c93 255 *
wf 3:2bf425b00c93 256 * @param adress - register address to write to
wf 3:2bf425b00c93 257 * @return - data from the register specified by RA
wf 3:2bf425b00c93 258 */
wf 3:2bf425b00c93 259 char read( char adress);
wf 3:2bf425b00c93 260
wf 3:2bf425b00c93 261 /**
wf 3:2bf425b00c93 262 * Read multtiple regigsters from the device, more efficient than using multiple normal reads.
wf 3:2bf425b00c93 263 *
wf 3:2bf425b00c93 264 * @param adress - register address to write to
wf 3:2bf425b00c93 265 * @param length - number of bytes to read
wf 3:2bf425b00c93 266 * @param data - pointer where the data needs to be written to
wf 3:2bf425b00c93 267 */
wf 3:2bf425b00c93 268 void read( char adress, char *data, int length);
wf 3:2bf425b00c93 269
wf 3:2bf425b00c93 270 //added aditional functions
wf 3:2bf425b00c93 271 void getAcceleroAngle( float *data );
wf 3:2bf425b00c93 272 void getOffset(float *accOffset, float *gyroOffset, int sampleSize);
wf 3:2bf425b00c93 273 void computeAngle (float *angle, float *accOffset, float *gyroOffset, float *currTime, float *prevTime);
wf 3:2bf425b00c93 274
wf 3:2bf425b00c93 275 private:
wf 3:2bf425b00c93 276
wf 3:2bf425b00c93 277 I2C connection;
wf 3:2bf425b00c93 278 char currentAcceleroRange;
wf 3:2bf425b00c93 279 char currentGyroRange;
wf 3:2bf425b00c93 280
wf 3:2bf425b00c93 281
wf 3:2bf425b00c93 282 };
wf 3:2bf425b00c93 283
wf 3:2bf425b00c93 284
wf 3:2bf425b00c93 285
wf 3:2bf425b00c93 286 #endif/**
wf 3:2bf425b00c93 287 * Includes
wf 3:2bf425b00c93 288 */
Sissors 0:6757f7363a9f 289 #include "MPU6050.h"
Sissors 0:6757f7363a9f 290
Sissors 0:6757f7363a9f 291 MPU6050::MPU6050(PinName sda, PinName scl) : connection(sda, scl) {
Sissors 0:6757f7363a9f 292 this->setSleepMode(false);
Sissors 0:6757f7363a9f 293
Sissors 0:6757f7363a9f 294 //Initializations:
Sissors 0:6757f7363a9f 295 currentGyroRange = 0;
Sissors 0:6757f7363a9f 296 currentAcceleroRange=0;
Sissors 0:6757f7363a9f 297 }
Sissors 0:6757f7363a9f 298
Sissors 0:6757f7363a9f 299 //--------------------------------------------------
Sissors 0:6757f7363a9f 300 //-------------------General------------------------
Sissors 0:6757f7363a9f 301 //--------------------------------------------------
Sissors 0:6757f7363a9f 302
Sissors 0:6757f7363a9f 303 void MPU6050::write(char address, char data) {
Sissors 0:6757f7363a9f 304 char temp[2];
Sissors 0:6757f7363a9f 305 temp[0]=address;
Sissors 0:6757f7363a9f 306 temp[1]=data;
Sissors 0:6757f7363a9f 307
Sissors 0:6757f7363a9f 308 connection.write(MPU6050_ADDRESS * 2,temp,2);
Sissors 0:6757f7363a9f 309 }
Sissors 0:6757f7363a9f 310
Sissors 0:6757f7363a9f 311 char MPU6050::read(char address) {
Sissors 0:6757f7363a9f 312 char retval;
Sissors 0:6757f7363a9f 313 connection.write(MPU6050_ADDRESS * 2, &address, 1, true);
Sissors 0:6757f7363a9f 314 connection.read(MPU6050_ADDRESS * 2, &retval, 1);
Sissors 0:6757f7363a9f 315 return retval;
Sissors 0:6757f7363a9f 316 }
Sissors 0:6757f7363a9f 317
Sissors 0:6757f7363a9f 318 void MPU6050::read(char address, char *data, int length) {
Sissors 0:6757f7363a9f 319 connection.write(MPU6050_ADDRESS * 2, &address, 1, true);
Sissors 0:6757f7363a9f 320 connection.read(MPU6050_ADDRESS * 2, data, length);
Sissors 0:6757f7363a9f 321 }
Sissors 0:6757f7363a9f 322
Sissors 0:6757f7363a9f 323 void MPU6050::setSleepMode(bool state) {
Sissors 0:6757f7363a9f 324 char temp;
Sissors 0:6757f7363a9f 325 temp = this->read(MPU6050_PWR_MGMT_1_REG);
Sissors 0:6757f7363a9f 326 if (state == true)
Sissors 0:6757f7363a9f 327 temp |= 1<<MPU6050_SLP_BIT;
Sissors 0:6757f7363a9f 328 if (state == false)
Sissors 0:6757f7363a9f 329 temp &= ~(1<<MPU6050_SLP_BIT);
Sissors 0:6757f7363a9f 330 this->write(MPU6050_PWR_MGMT_1_REG, temp);
Sissors 0:6757f7363a9f 331 }
Sissors 0:6757f7363a9f 332
Sissors 0:6757f7363a9f 333 bool MPU6050::testConnection( void ) {
Sissors 0:6757f7363a9f 334 char temp;
Sissors 0:6757f7363a9f 335 temp = this->read(MPU6050_WHO_AM_I_REG);
Sissors 0:6757f7363a9f 336 return (temp == (MPU6050_ADDRESS & 0xFE));
Sissors 0:6757f7363a9f 337 }
Sissors 0:6757f7363a9f 338
Sissors 0:6757f7363a9f 339 void MPU6050::setBW(char BW) {
Sissors 0:6757f7363a9f 340 char temp;
Sissors 0:6757f7363a9f 341 BW=BW & 0x07;
Sissors 0:6757f7363a9f 342 temp = this->read(MPU6050_CONFIG_REG);
Sissors 0:6757f7363a9f 343 temp &= 0xF8;
Sissors 0:6757f7363a9f 344 temp = temp + BW;
Sissors 0:6757f7363a9f 345 this->write(MPU6050_CONFIG_REG, temp);
Sissors 0:6757f7363a9f 346 }
Sissors 0:6757f7363a9f 347
Sissors 0:6757f7363a9f 348 void MPU6050::setI2CBypass(bool state) {
Sissors 0:6757f7363a9f 349 char temp;
Sissors 0:6757f7363a9f 350 temp = this->read(MPU6050_INT_PIN_CFG);
Sissors 0:6757f7363a9f 351 if (state == true)
Sissors 0:6757f7363a9f 352 temp |= 1<<MPU6050_BYPASS_BIT;
Sissors 0:6757f7363a9f 353 if (state == false)
Sissors 0:6757f7363a9f 354 temp &= ~(1<<MPU6050_BYPASS_BIT);
Sissors 0:6757f7363a9f 355 this->write(MPU6050_INT_PIN_CFG, temp);
Sissors 0:6757f7363a9f 356 }
Sissors 0:6757f7363a9f 357
Sissors 0:6757f7363a9f 358 //--------------------------------------------------
Sissors 0:6757f7363a9f 359 //----------------Accelerometer---------------------
Sissors 0:6757f7363a9f 360 //--------------------------------------------------
Sissors 0:6757f7363a9f 361
Sissors 0:6757f7363a9f 362 void MPU6050::setAcceleroRange( char range ) {
Sissors 0:6757f7363a9f 363 char temp;
Sissors 0:6757f7363a9f 364 range = range & 0x03;
Sissors 0:6757f7363a9f 365 currentAcceleroRange = range;
Sissors 0:6757f7363a9f 366
Sissors 0:6757f7363a9f 367 temp = this->read(MPU6050_ACCELERO_CONFIG_REG);
Sissors 0:6757f7363a9f 368 temp &= ~(3<<3);
Sissors 0:6757f7363a9f 369 temp = temp + (range<<3);
Sissors 0:6757f7363a9f 370 this->write(MPU6050_ACCELERO_CONFIG_REG, temp);
Sissors 0:6757f7363a9f 371 }
Sissors 0:6757f7363a9f 372
Sissors 0:6757f7363a9f 373 int MPU6050::getAcceleroRawX( void ) {
Sissors 0:6757f7363a9f 374 short retval;
Sissors 0:6757f7363a9f 375 char data[2];
Sissors 0:6757f7363a9f 376 this->read(MPU6050_ACCEL_XOUT_H_REG, data, 2);
Sissors 0:6757f7363a9f 377 retval = (data[0]<<8) + data[1];
Sissors 0:6757f7363a9f 378 return (int)retval;
Sissors 0:6757f7363a9f 379 }
Sissors 0:6757f7363a9f 380
Sissors 0:6757f7363a9f 381 int MPU6050::getAcceleroRawY( void ) {
Sissors 0:6757f7363a9f 382 short retval;
Sissors 0:6757f7363a9f 383 char data[2];
Sissors 0:6757f7363a9f 384 this->read(MPU6050_ACCEL_YOUT_H_REG, data, 2);
Sissors 0:6757f7363a9f 385 retval = (data[0]<<8) + data[1];
Sissors 0:6757f7363a9f 386 return (int)retval;
Sissors 0:6757f7363a9f 387 }
Sissors 0:6757f7363a9f 388
Sissors 0:6757f7363a9f 389 int MPU6050::getAcceleroRawZ( void ) {
Sissors 0:6757f7363a9f 390 short retval;
Sissors 0:6757f7363a9f 391 char data[2];
Sissors 0:6757f7363a9f 392 this->read(MPU6050_ACCEL_ZOUT_H_REG, data, 2);
Sissors 0:6757f7363a9f 393 retval = (data[0]<<8) + data[1];
Sissors 0:6757f7363a9f 394 return (int)retval;
Sissors 0:6757f7363a9f 395 }
Sissors 0:6757f7363a9f 396
Sissors 0:6757f7363a9f 397 void MPU6050::getAcceleroRaw( int *data ) {
Sissors 0:6757f7363a9f 398 char temp[6];
Sissors 0:6757f7363a9f 399 this->read(MPU6050_ACCEL_XOUT_H_REG, temp, 6);
Sissors 0:6757f7363a9f 400 data[0] = (int)(short)((temp[0]<<8) + temp[1]);
Sissors 0:6757f7363a9f 401 data[1] = (int)(short)((temp[2]<<8) + temp[3]);
Sissors 0:6757f7363a9f 402 data[2] = (int)(short)((temp[4]<<8) + temp[5]);
Sissors 0:6757f7363a9f 403 }
Sissors 0:6757f7363a9f 404
Sissors 0:6757f7363a9f 405 void MPU6050::getAccelero( float *data ) {
Sissors 0:6757f7363a9f 406 int temp[3];
Sissors 0:6757f7363a9f 407 this->getAcceleroRaw(temp);
Sissors 0:6757f7363a9f 408 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_2G) {
Sissors 0:6757f7363a9f 409 data[0]=(float)temp[0] / 16384.0 * 9.81;
Sissors 0:6757f7363a9f 410 data[1]=(float)temp[1] / 16384.0 * 9.81;
Sissors 0:6757f7363a9f 411 data[2]=(float)temp[2] / 16384.0 * 9.81;
Sissors 0:6757f7363a9f 412 }
Sissors 0:6757f7363a9f 413 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_4G){
Sissors 0:6757f7363a9f 414 data[0]=(float)temp[0] / 8192.0 * 9.81;
Sissors 0:6757f7363a9f 415 data[1]=(float)temp[1] / 8192.0 * 9.81;
Sissors 0:6757f7363a9f 416 data[2]=(float)temp[2] / 8192.0 * 9.81;
Sissors 0:6757f7363a9f 417 }
Sissors 0:6757f7363a9f 418 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_8G){
Sissors 0:6757f7363a9f 419 data[0]=(float)temp[0] / 4096.0 * 9.81;
Sissors 0:6757f7363a9f 420 data[1]=(float)temp[1] / 4096.0 * 9.81;
Sissors 0:6757f7363a9f 421 data[2]=(float)temp[2] / 4096.0 * 9.81;
Sissors 0:6757f7363a9f 422 }
Sissors 0:6757f7363a9f 423 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_16G){
Sissors 0:6757f7363a9f 424 data[0]=(float)temp[0] / 2048.0 * 9.81;
Sissors 0:6757f7363a9f 425 data[1]=(float)temp[1] / 2048.0 * 9.81;
Sissors 0:6757f7363a9f 426 data[2]=(float)temp[2] / 2048.0 * 9.81;
Sissors 0:6757f7363a9f 427 }
Sissors 0:6757f7363a9f 428
Sissors 0:6757f7363a9f 429 #ifdef DOUBLE_ACCELERO
Sissors 0:6757f7363a9f 430 data[0]*=2;
Sissors 0:6757f7363a9f 431 data[1]*=2;
Sissors 0:6757f7363a9f 432 data[2]*=2;
Sissors 0:6757f7363a9f 433 #endif
Sissors 0:6757f7363a9f 434 }
Sissors 0:6757f7363a9f 435
Sissors 0:6757f7363a9f 436 //--------------------------------------------------
Sissors 0:6757f7363a9f 437 //------------------Gyroscope-----------------------
Sissors 0:6757f7363a9f 438 //--------------------------------------------------
Sissors 0:6757f7363a9f 439 void MPU6050::setGyroRange( char range ) {
Sissors 0:6757f7363a9f 440 char temp;
Sissors 0:6757f7363a9f 441 currentGyroRange = range;
Sissors 0:6757f7363a9f 442 range = range & 0x03;
Sissors 0:6757f7363a9f 443 temp = this->read(MPU6050_GYRO_CONFIG_REG);
Sissors 0:6757f7363a9f 444 temp &= ~(3<<3);
Sissors 0:6757f7363a9f 445 temp = temp + range<<3;
Sissors 0:6757f7363a9f 446 this->write(MPU6050_GYRO_CONFIG_REG, temp);
Sissors 0:6757f7363a9f 447 }
Sissors 0:6757f7363a9f 448
Sissors 0:6757f7363a9f 449 int MPU6050::getGyroRawX( void ) {
Sissors 0:6757f7363a9f 450 short retval;
Sissors 0:6757f7363a9f 451 char data[2];
Sissors 0:6757f7363a9f 452 this->read(MPU6050_GYRO_XOUT_H_REG, data, 2);
Sissors 0:6757f7363a9f 453 retval = (data[0]<<8) + data[1];
Sissors 0:6757f7363a9f 454 return (int)retval;
Sissors 0:6757f7363a9f 455 }
Sissors 0:6757f7363a9f 456
Sissors 0:6757f7363a9f 457 int MPU6050::getGyroRawY( void ) {
Sissors 0:6757f7363a9f 458 short retval;
Sissors 0:6757f7363a9f 459 char data[2];
Sissors 0:6757f7363a9f 460 this->read(MPU6050_GYRO_YOUT_H_REG, data, 2);
Sissors 0:6757f7363a9f 461 retval = (data[0]<<8) + data[1];
Sissors 0:6757f7363a9f 462 return (int)retval;
Sissors 0:6757f7363a9f 463 }
Sissors 0:6757f7363a9f 464
Sissors 0:6757f7363a9f 465 int MPU6050::getGyroRawZ( void ) {
Sissors 0:6757f7363a9f 466 short retval;
Sissors 0:6757f7363a9f 467 char data[2];
Sissors 0:6757f7363a9f 468 this->read(MPU6050_GYRO_ZOUT_H_REG, data, 2);
Sissors 0:6757f7363a9f 469 retval = (data[0]<<8) + data[1];
Sissors 0:6757f7363a9f 470 return (int)retval;
Sissors 0:6757f7363a9f 471 }
Sissors 0:6757f7363a9f 472
Sissors 0:6757f7363a9f 473 void MPU6050::getGyroRaw( int *data ) {
Sissors 0:6757f7363a9f 474 char temp[6];
Sissors 0:6757f7363a9f 475 this->read(MPU6050_GYRO_XOUT_H_REG, temp, 6);
Sissors 0:6757f7363a9f 476 data[0] = (int)(short)((temp[0]<<8) + temp[1]);
Sissors 0:6757f7363a9f 477 data[1] = (int)(short)((temp[2]<<8) + temp[3]);
Sissors 0:6757f7363a9f 478 data[2] = (int)(short)((temp[4]<<8) + temp[5]);
Sissors 0:6757f7363a9f 479 }
Sissors 0:6757f7363a9f 480
Sissors 0:6757f7363a9f 481 void MPU6050::getGyro( float *data ) {
Sissors 0:6757f7363a9f 482 int temp[3];
Sissors 0:6757f7363a9f 483 this->getGyroRaw(temp);
Sissors 1:a3366f09e95c 484 if (currentGyroRange == MPU6050_GYRO_RANGE_250) {
Sissors 0:6757f7363a9f 485 data[0]=(float)temp[0] / 7505.7;
Sissors 0:6757f7363a9f 486 data[1]=(float)temp[1] / 7505.7;
Sissors 0:6757f7363a9f 487 data[2]=(float)temp[2] / 7505.7;
Sissors 0:6757f7363a9f 488 }
Sissors 1:a3366f09e95c 489 if (currentGyroRange == MPU6050_GYRO_RANGE_500){
Sissors 0:6757f7363a9f 490 data[0]=(float)temp[0] / 3752.9;
Sissors 0:6757f7363a9f 491 data[1]=(float)temp[1] / 3752.9;
Sissors 0:6757f7363a9f 492 data[2]=(float)temp[2] / 3752.9;
Sissors 0:6757f7363a9f 493 }
Sissors 1:a3366f09e95c 494 if (currentGyroRange == MPU6050_GYRO_RANGE_1000){
Sissors 0:6757f7363a9f 495 data[0]=(float)temp[0] / 1879.3;;
Sissors 0:6757f7363a9f 496 data[1]=(float)temp[1] / 1879.3;
Sissors 0:6757f7363a9f 497 data[2]=(float)temp[2] / 1879.3;
Sissors 0:6757f7363a9f 498 }
Sissors 1:a3366f09e95c 499 if (currentGyroRange == MPU6050_GYRO_RANGE_2000){
Sissors 0:6757f7363a9f 500 data[0]=(float)temp[0] / 939.7;
Sissors 0:6757f7363a9f 501 data[1]=(float)temp[1] / 939.7;
Sissors 0:6757f7363a9f 502 data[2]=(float)temp[2] / 939.7;
Sissors 0:6757f7363a9f 503 }
Sissors 0:6757f7363a9f 504 }
Sissors 0:6757f7363a9f 505 //--------------------------------------------------
Sissors 0:6757f7363a9f 506 //-------------------Temperature--------------------
Sissors 0:6757f7363a9f 507 //--------------------------------------------------
Sissors 0:6757f7363a9f 508 int MPU6050::getTempRaw( void ) {
Sissors 0:6757f7363a9f 509 short retval;
Sissors 0:6757f7363a9f 510 char data[2];
Sissors 0:6757f7363a9f 511 this->read(MPU6050_TEMP_H_REG, data, 2);
Sissors 0:6757f7363a9f 512 retval = (data[0]<<8) + data[1];
Sissors 0:6757f7363a9f 513 return (int)retval;
Sissors 0:6757f7363a9f 514 }
Sissors 0:6757f7363a9f 515
Sissors 0:6757f7363a9f 516 float MPU6050::getTemp( void ) {
Sissors 0:6757f7363a9f 517 float retval;
Sissors 0:6757f7363a9f 518 retval=(float)this->getTempRaw();
Sissors 0:6757f7363a9f 519 retval=(retval+521.0)/340.0+35.0;
Sissors 0:6757f7363a9f 520 return retval;
Sissors 0:6757f7363a9f 521 }
Sissors 0:6757f7363a9f 522