Luis Silvio Cordova Rivadeneira
/
mpu9250
Ejemplo de uso del MPU9250 por puerto I2C
myMPU9250.h@0:cecf4940adf1, 2015-10-25 (annotated)
- Committer:
- a0074639
- Date:
- Sun Oct 25 15:28:55 2015 +0000
- Revision:
- 0:cecf4940adf1
- Child:
- 1:ad5417f813f4
comfirmed it works well
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
a0074639 | 0:cecf4940adf1 | 1 | #ifndef MYMPU9250_H |
a0074639 | 0:cecf4940adf1 | 2 | #define MYMPU9250_H |
a0074639 | 0:cecf4940adf1 | 3 | |
a0074639 | 0:cecf4940adf1 | 4 | #include "mpu9250Define.h" |
a0074639 | 0:cecf4940adf1 | 5 | |
a0074639 | 0:cecf4940adf1 | 6 | class MyMPU9250 |
a0074639 | 0:cecf4940adf1 | 7 | { |
a0074639 | 0:cecf4940adf1 | 8 | public: |
a0074639 | 0:cecf4940adf1 | 9 | MyMPU9250() |
a0074639 | 0:cecf4940adf1 | 10 | : pc_(USBTX, USBRX), i2c_(p28, p27) { |
a0074639 | 0:cecf4940adf1 | 11 | accelLSB2MG_ = 10.0; |
a0074639 | 0:cecf4940adf1 | 12 | gyroLSB2DegPerSec_ = 0.0; |
a0074639 | 0:cecf4940adf1 | 13 | }; |
a0074639 | 0:cecf4940adf1 | 14 | |
a0074639 | 0:cecf4940adf1 | 15 | bool initialize() { |
a0074639 | 0:cecf4940adf1 | 16 | i2c_.frequency(400000); |
a0074639 | 0:cecf4940adf1 | 17 | i2c_.start(); |
a0074639 | 0:cecf4940adf1 | 18 | |
a0074639 | 0:cecf4940adf1 | 19 | wait(0.5); |
a0074639 | 0:cecf4940adf1 | 20 | |
a0074639 | 0:cecf4940adf1 | 21 | char cmd[2] = { 0x00 }; |
a0074639 | 0:cecf4940adf1 | 22 | cmd[0] = PWR_MGMT_1; // 0x6B |
a0074639 | 0:cecf4940adf1 | 23 | cmd[1] = (PWR_MGMT_1_CLKSEL_0); // 0x00 |
a0074639 | 0:cecf4940adf1 | 24 | this->write(cmd, BIT_2); |
a0074639 | 0:cecf4940adf1 | 25 | |
a0074639 | 0:cecf4940adf1 | 26 | wait(0.5); |
a0074639 | 0:cecf4940adf1 | 27 | |
a0074639 | 0:cecf4940adf1 | 28 | char i_am[1] = { 0x00 }; |
a0074639 | 0:cecf4940adf1 | 29 | if(! this->read(WHO_AM_I, i_am, BIT_1)) { |
a0074639 | 0:cecf4940adf1 | 30 | pc_.printf("Fail to read! I am %x\n", i_am[0]); |
a0074639 | 0:cecf4940adf1 | 31 | return false; |
a0074639 | 0:cecf4940adf1 | 32 | } else if(!(i_am[0] == 0x71)) { |
a0074639 | 0:cecf4940adf1 | 33 | pc_.printf("Fail to identify! I am %x\n", i_am[0]); |
a0074639 | 0:cecf4940adf1 | 34 | return false; |
a0074639 | 0:cecf4940adf1 | 35 | } |
a0074639 | 0:cecf4940adf1 | 36 | return true; |
a0074639 | 0:cecf4940adf1 | 37 | }; |
a0074639 | 0:cecf4940adf1 | 38 | |
a0074639 | 0:cecf4940adf1 | 39 | void setup(AccelSensitivity accel_mode=AFS_SEL_2, |
a0074639 | 0:cecf4940adf1 | 40 | GyroSensitivity gyro_mode=FS_SEL_250) { |
a0074639 | 0:cecf4940adf1 | 41 | char cmd[2] = { 0x00 }; |
a0074639 | 0:cecf4940adf1 | 42 | |
a0074639 | 0:cecf4940adf1 | 43 | cmd[0] = ACCEL_CONFIG; |
a0074639 | 0:cecf4940adf1 | 44 | cmd[1] = ACCEL_CONFIG_AFS_SEL_2;//(0x18); |
a0074639 | 0:cecf4940adf1 | 45 | this->write(cmd, BIT_2); |
a0074639 | 0:cecf4940adf1 | 46 | accelLSB2MG_ = getAccelLSB2MG(accel_mode); |
a0074639 | 0:cecf4940adf1 | 47 | pc_.printf("accelLSB2MG: %f, %x", accelLSB2MG_, cmd[1]); |
a0074639 | 0:cecf4940adf1 | 48 | |
a0074639 | 0:cecf4940adf1 | 49 | cmd[0] = GYRO_CONFIG; |
a0074639 | 0:cecf4940adf1 | 50 | cmd[1] = getGyroSensitivity(gyro_mode);//(0x18); |
a0074639 | 0:cecf4940adf1 | 51 | this->write(cmd, BIT_2); |
a0074639 | 0:cecf4940adf1 | 52 | gyroLSB2DegPerSec_ = getGyroLSB2DegPerSec(gyro_mode); |
a0074639 | 0:cecf4940adf1 | 53 | |
a0074639 | 0:cecf4940adf1 | 54 | cmd[0] = INT_PIN_CFG; // 0x37 |
a0074639 | 0:cecf4940adf1 | 55 | cmd[1] = (INT_PIN_DFG_I2C_BYPASS_EN); // 0x02 |
a0074639 | 0:cecf4940adf1 | 56 | this->write(cmd, BIT_2); |
a0074639 | 0:cecf4940adf1 | 57 | }; |
a0074639 | 0:cecf4940adf1 | 58 | |
a0074639 | 0:cecf4940adf1 | 59 | bool updateAccelAllAxes() { |
a0074639 | 0:cecf4940adf1 | 60 | return this->read(ACCEL_XOUT_H, accel_all_, BIT_6); |
a0074639 | 0:cecf4940adf1 | 61 | }; |
a0074639 | 0:cecf4940adf1 | 62 | |
a0074639 | 0:cecf4940adf1 | 63 | int accelXRaw() { |
a0074639 | 0:cecf4940adf1 | 64 | return (int16_t)(accel_all_[0]*0x100 + accel_all_[1]) ; |
a0074639 | 0:cecf4940adf1 | 65 | }; |
a0074639 | 0:cecf4940adf1 | 66 | |
a0074639 | 0:cecf4940adf1 | 67 | double accelX() { |
a0074639 | 0:cecf4940adf1 | 68 | return accelLSB2MG_ * accelXRaw() ; |
a0074639 | 0:cecf4940adf1 | 69 | }; |
a0074639 | 0:cecf4940adf1 | 70 | |
a0074639 | 0:cecf4940adf1 | 71 | int accelYRaw() { |
a0074639 | 0:cecf4940adf1 | 72 | return (int16_t)(accel_all_[2]*0x100 + accel_all_[3]) ; |
a0074639 | 0:cecf4940adf1 | 73 | }; |
a0074639 | 0:cecf4940adf1 | 74 | |
a0074639 | 0:cecf4940adf1 | 75 | double accelY() { |
a0074639 | 0:cecf4940adf1 | 76 | return accelLSB2MG_ * accelYRaw() ; |
a0074639 | 0:cecf4940adf1 | 77 | }; |
a0074639 | 0:cecf4940adf1 | 78 | |
a0074639 | 0:cecf4940adf1 | 79 | int accelZRaw() { |
a0074639 | 0:cecf4940adf1 | 80 | return (int16_t)(accel_all_[4]*0x100 + accel_all_[5]) ; |
a0074639 | 0:cecf4940adf1 | 81 | }; |
a0074639 | 0:cecf4940adf1 | 82 | |
a0074639 | 0:cecf4940adf1 | 83 | double accelZ() { |
a0074639 | 0:cecf4940adf1 | 84 | return accelLSB2MG_ * accelZRaw() ; |
a0074639 | 0:cecf4940adf1 | 85 | }; |
a0074639 | 0:cecf4940adf1 | 86 | |
a0074639 | 0:cecf4940adf1 | 87 | |
a0074639 | 0:cecf4940adf1 | 88 | bool updateGyroAllAxes() { |
a0074639 | 0:cecf4940adf1 | 89 | return this->read(GYRO_XOUT_H, gyro_all_, BIT_6); |
a0074639 | 0:cecf4940adf1 | 90 | }; |
a0074639 | 0:cecf4940adf1 | 91 | |
a0074639 | 0:cecf4940adf1 | 92 | int gyroXRaw() { |
a0074639 | 0:cecf4940adf1 | 93 | return (int16_t)(gyro_all_[0]*0x100 + gyro_all_[1]) ; |
a0074639 | 0:cecf4940adf1 | 94 | }; |
a0074639 | 0:cecf4940adf1 | 95 | |
a0074639 | 0:cecf4940adf1 | 96 | double gyroX() { |
a0074639 | 0:cecf4940adf1 | 97 | return accelLSB2MG_ * gyroXRaw() ; |
a0074639 | 0:cecf4940adf1 | 98 | }; |
a0074639 | 0:cecf4940adf1 | 99 | |
a0074639 | 0:cecf4940adf1 | 100 | int gyroYRaw() { |
a0074639 | 0:cecf4940adf1 | 101 | return (int16_t)(gyro_all_[2]*0x100 + gyro_all_[3]) ; |
a0074639 | 0:cecf4940adf1 | 102 | }; |
a0074639 | 0:cecf4940adf1 | 103 | |
a0074639 | 0:cecf4940adf1 | 104 | double gyroY() { |
a0074639 | 0:cecf4940adf1 | 105 | return accelLSB2MG_ * gyroYRaw() ; |
a0074639 | 0:cecf4940adf1 | 106 | }; |
a0074639 | 0:cecf4940adf1 | 107 | |
a0074639 | 0:cecf4940adf1 | 108 | int gyroZRaw() { |
a0074639 | 0:cecf4940adf1 | 109 | return (int16_t)(gyro_all_[4]*0x100 + gyro_all_[5]) ; |
a0074639 | 0:cecf4940adf1 | 110 | }; |
a0074639 | 0:cecf4940adf1 | 111 | |
a0074639 | 0:cecf4940adf1 | 112 | double gyroZ() { |
a0074639 | 0:cecf4940adf1 | 113 | return accelLSB2MG_ * gyroZRaw() ; |
a0074639 | 0:cecf4940adf1 | 114 | }; |
a0074639 | 0:cecf4940adf1 | 115 | |
a0074639 | 0:cecf4940adf1 | 116 | |
a0074639 | 0:cecf4940adf1 | 117 | private: |
a0074639 | 0:cecf4940adf1 | 118 | Serial pc_; |
a0074639 | 0:cecf4940adf1 | 119 | I2C i2c_; |
a0074639 | 0:cecf4940adf1 | 120 | float accelLSB2MG_; |
a0074639 | 0:cecf4940adf1 | 121 | float gyroLSB2DegPerSec_; |
a0074639 | 0:cecf4940adf1 | 122 | char accel_all_[6]; |
a0074639 | 0:cecf4940adf1 | 123 | char gyro_all_[6]; |
a0074639 | 0:cecf4940adf1 | 124 | |
a0074639 | 0:cecf4940adf1 | 125 | bool write(char* _cmd, int _length) { |
a0074639 | 0:cecf4940adf1 | 126 | int w_status = i2c_.write(MPU9250_ADDRESS, _cmd, _length, false); |
a0074639 | 0:cecf4940adf1 | 127 | if(w_status == 0) { |
a0074639 | 0:cecf4940adf1 | 128 | return true; |
a0074639 | 0:cecf4940adf1 | 129 | } else { |
a0074639 | 0:cecf4940adf1 | 130 | pc_.printf("write register: %x \n", _cmd[0]); |
a0074639 | 0:cecf4940adf1 | 131 | pc_.printf(" write: %d \n", w_status); |
a0074639 | 0:cecf4940adf1 | 132 | return false; |
a0074639 | 0:cecf4940adf1 | 133 | } |
a0074639 | 0:cecf4940adf1 | 134 | } |
a0074639 | 0:cecf4940adf1 | 135 | |
a0074639 | 0:cecf4940adf1 | 136 | bool read(char _register, char* _data, int _length) { |
a0074639 | 0:cecf4940adf1 | 137 | char cmd[1] = { _register }; |
a0074639 | 0:cecf4940adf1 | 138 | int w_status = i2c_.write(MPU9250_ADDRESS, cmd, BIT_1, false); |
a0074639 | 0:cecf4940adf1 | 139 | int r_status = i2c_.read(MPU9250_ADDRESS, _data, _length, false); |
a0074639 | 0:cecf4940adf1 | 140 | if(w_status + r_status == 0) { |
a0074639 | 0:cecf4940adf1 | 141 | return true; |
a0074639 | 0:cecf4940adf1 | 142 | } else { |
a0074639 | 0:cecf4940adf1 | 143 | pc_.printf("write register: %x \n", cmd[0]); |
a0074639 | 0:cecf4940adf1 | 144 | pc_.printf("read register: %x \n", _register); |
a0074639 | 0:cecf4940adf1 | 145 | pc_.printf(" write: %d | read: %d \n", w_status, r_status); |
a0074639 | 0:cecf4940adf1 | 146 | return false; |
a0074639 | 0:cecf4940adf1 | 147 | } |
a0074639 | 0:cecf4940adf1 | 148 | }; |
a0074639 | 0:cecf4940adf1 | 149 | }; |
a0074639 | 0:cecf4940adf1 | 150 | |
a0074639 | 0:cecf4940adf1 | 151 | #endif /* MYMPU9250_H */ |