Library containing Crazyflie 2.0 sensors drivers: - LPS25H (barometer) - MPU9250 (IMU) - PMW3901 (optical flow) - VL53L0X (range)
Dependents: Drones-Controlador controladoatitude_cteste Drone_Controlador_Atitude optical_test
MPU9250/MPU9250.cpp@15:e07de535b86f, 2018-10-17 (annotated)
- Committer:
- fbob
- Date:
- Wed Oct 17 13:26:29 2018 +0000
- Revision:
- 15:e07de535b86f
- Parent:
- 10:237258f2d05e
Changed range and optical flow parameters names
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
fbob | 8:930aa9d0f5ae | 1 | #include "MPU9250.h" |
fbob | 8:930aa9d0f5ae | 2 | |
fbob | 8:930aa9d0f5ae | 3 | /** Class constructor */ |
fbob | 8:930aa9d0f5ae | 4 | MPU9250::MPU9250(PinName sda, PinName scl) : i2c(sda, scl) |
fbob | 8:930aa9d0f5ae | 5 | { |
fbob | 8:930aa9d0f5ae | 6 | } |
fbob | 8:930aa9d0f5ae | 7 | |
fbob | 8:930aa9d0f5ae | 8 | /** Try to initialize sensor (return true if succeed and false if failed) */ |
fbob | 8:930aa9d0f5ae | 9 | bool MPU9250::init() |
fbob | 8:930aa9d0f5ae | 10 | { |
fbob | 8:930aa9d0f5ae | 11 | // Setup I2C bus |
fbob | 8:930aa9d0f5ae | 12 | setup_i2c(); |
fbob | 8:930aa9d0f5ae | 13 | |
fbob | 8:930aa9d0f5ae | 14 | // Test I2C bus |
fbob | 8:930aa9d0f5ae | 15 | if (test_i2c()) { |
fbob | 8:930aa9d0f5ae | 16 | // Setup accelerometer and gyroscope configurations |
fbob | 8:930aa9d0f5ae | 17 | setup_accel(); |
fbob | 8:930aa9d0f5ae | 18 | setup_gyro(); |
fbob | 8:930aa9d0f5ae | 19 | // Setup auxiliary I2C bus pins |
fbob | 8:930aa9d0f5ae | 20 | setup_aux_i2c(); |
fbob | 8:930aa9d0f5ae | 21 | return true; |
fbob | 8:930aa9d0f5ae | 22 | } else { |
fbob | 8:930aa9d0f5ae | 23 | return false; |
fbob | 8:930aa9d0f5ae | 24 | } |
fbob | 8:930aa9d0f5ae | 25 | } |
fbob | 8:930aa9d0f5ae | 26 | |
fbob | 8:930aa9d0f5ae | 27 | /** Read sensor data */ |
fbob | 8:930aa9d0f5ae | 28 | void MPU9250::read() |
fbob | 8:930aa9d0f5ae | 29 | { |
fbob | 8:930aa9d0f5ae | 30 | // Read accelerometer and gyroscope data |
fbob | 8:930aa9d0f5ae | 31 | read_accel(); |
fbob | 8:930aa9d0f5ae | 32 | read_gyro(); |
fbob | 8:930aa9d0f5ae | 33 | } |
fbob | 8:930aa9d0f5ae | 34 | |
fbob | 8:930aa9d0f5ae | 35 | /** Setup I2C bus */ |
fbob | 8:930aa9d0f5ae | 36 | void MPU9250::setup_i2c() |
fbob | 8:930aa9d0f5ae | 37 | { |
fbob | 8:930aa9d0f5ae | 38 | // Setup I2C bus frequency to 100kHz |
fbob | 10:237258f2d05e | 39 | i2c.frequency(100000); |
fbob | 8:930aa9d0f5ae | 40 | } |
fbob | 8:930aa9d0f5ae | 41 | |
fbob | 8:930aa9d0f5ae | 42 | /** Test I2C bus */ |
fbob | 8:930aa9d0f5ae | 43 | bool MPU9250::test_i2c() |
fbob | 8:930aa9d0f5ae | 44 | { |
fbob | 8:930aa9d0f5ae | 45 | // Read device identity |
fbob | 8:930aa9d0f5ae | 46 | uint8_t device_id = read_reg(WHO_AM_I); |
fbob | 8:930aa9d0f5ae | 47 | |
fbob | 8:930aa9d0f5ae | 48 | // Check if device identity is 0x71 |
fbob | 8:930aa9d0f5ae | 49 | if (device_id == 0x71) { |
fbob | 8:930aa9d0f5ae | 50 | return true; |
fbob | 8:930aa9d0f5ae | 51 | } else { |
fbob | 8:930aa9d0f5ae | 52 | return false; |
fbob | 8:930aa9d0f5ae | 53 | } |
fbob | 8:930aa9d0f5ae | 54 | } |
fbob | 8:930aa9d0f5ae | 55 | |
fbob | 8:930aa9d0f5ae | 56 | /** Setup auxiliary I2C bus pins */ |
fbob | 8:930aa9d0f5ae | 57 | void MPU9250::setup_aux_i2c() |
fbob | 8:930aa9d0f5ae | 58 | { |
fbob | 8:930aa9d0f5ae | 59 | // Write |
fbob | 8:930aa9d0f5ae | 60 | write_reg(INT_PIN_CFG, 0b00000010); |
fbob | 8:930aa9d0f5ae | 61 | } |
fbob | 8:930aa9d0f5ae | 62 | |
fbob | 8:930aa9d0f5ae | 63 | /** Setup accelerometer configurations (full-scale range) */ |
fbob | 8:930aa9d0f5ae | 64 | void MPU9250::setup_accel(accel_scale a_scale) |
fbob | 8:930aa9d0f5ae | 65 | { |
fbob | 8:930aa9d0f5ae | 66 | // Write configuration data |
fbob | 8:930aa9d0f5ae | 67 | write_reg(ACCEL_CONFIG_1, (0b000 << 5) | (a_scale << 3) | 0b000); |
fbob | 8:930aa9d0f5ae | 68 | |
fbob | 8:930aa9d0f5ae | 69 | // Adjust resolution [m/s^2 / bit] accordingly to choose scale (g) |
fbob | 8:930aa9d0f5ae | 70 | switch (a_scale) { |
fbob | 8:930aa9d0f5ae | 71 | case ACCEL_SCALE_2G: |
fbob | 8:930aa9d0f5ae | 72 | a_res = (2.0f*GRAVITY)/32768.0f; |
fbob | 8:930aa9d0f5ae | 73 | break; |
fbob | 8:930aa9d0f5ae | 74 | case ACCEL_SCALE_4G: |
fbob | 8:930aa9d0f5ae | 75 | a_res = (4.0f*GRAVITY)/32768.0f; |
fbob | 8:930aa9d0f5ae | 76 | break; |
fbob | 8:930aa9d0f5ae | 77 | case ACCEL_SCALE_8G: |
fbob | 8:930aa9d0f5ae | 78 | a_res = (8.0f*GRAVITY)/32768.0f; |
fbob | 8:930aa9d0f5ae | 79 | break; |
fbob | 8:930aa9d0f5ae | 80 | case ACCEL_SCALE_16G: |
fbob | 8:930aa9d0f5ae | 81 | a_res = (16.0f*GRAVITY)/32768.0f; |
fbob | 8:930aa9d0f5ae | 82 | break; |
fbob | 8:930aa9d0f5ae | 83 | } |
fbob | 8:930aa9d0f5ae | 84 | } |
fbob | 8:930aa9d0f5ae | 85 | |
fbob | 8:930aa9d0f5ae | 86 | /** Setup gyroscope configurations (full-scale range) */ |
fbob | 8:930aa9d0f5ae | 87 | void MPU9250::setup_gyro(gyro_scale g_scale) |
fbob | 8:930aa9d0f5ae | 88 | { |
fbob | 8:930aa9d0f5ae | 89 | // Write configuration data |
fbob | 8:930aa9d0f5ae | 90 | write_reg(GYRO_CONFIG, (0b000 << 5) | (g_scale << 3) | 0b000); |
fbob | 8:930aa9d0f5ae | 91 | |
fbob | 8:930aa9d0f5ae | 92 | // Adjust resolution [rad/s / bit] accordingly to choose scale |
fbob | 8:930aa9d0f5ae | 93 | switch (g_scale) { |
fbob | 8:930aa9d0f5ae | 94 | case GYRO_SCALE_250DPS: |
fbob | 8:930aa9d0f5ae | 95 | g_res = (250.0f*(PI/180.0f))/32768.0f; |
fbob | 8:930aa9d0f5ae | 96 | break; |
fbob | 8:930aa9d0f5ae | 97 | case GYRO_SCALE_500DPS: |
fbob | 8:930aa9d0f5ae | 98 | g_res = (500.0f*(PI/180.0f))/32768.0f; |
fbob | 8:930aa9d0f5ae | 99 | break; |
fbob | 8:930aa9d0f5ae | 100 | case GYRO_SCALE_1000DPS: |
fbob | 8:930aa9d0f5ae | 101 | g_res = (1000.0f*(PI/180.0f))/32768.0f; |
fbob | 8:930aa9d0f5ae | 102 | break; |
fbob | 8:930aa9d0f5ae | 103 | case GYRO_SCALE_2000DPS: |
fbob | 8:930aa9d0f5ae | 104 | g_res = (2000.0f*(PI/180.0f))/32768.0f; |
fbob | 8:930aa9d0f5ae | 105 | break; |
fbob | 8:930aa9d0f5ae | 106 | } |
fbob | 8:930aa9d0f5ae | 107 | } |
fbob | 8:930aa9d0f5ae | 108 | |
fbob | 8:930aa9d0f5ae | 109 | /** Read accelerometer output data */ |
fbob | 8:930aa9d0f5ae | 110 | void MPU9250::read_accel() |
fbob | 8:930aa9d0f5ae | 111 | { |
fbob | 10:237258f2d05e | 112 | /* |
fbob | 8:930aa9d0f5ae | 113 | // Read raw data (two 8 bit data for each axis) |
fbob | 8:930aa9d0f5ae | 114 | uint8_t ax_raw_h = read_reg(ACCEL_XOUT_H); |
fbob | 8:930aa9d0f5ae | 115 | uint8_t ax_raw_l = read_reg(ACCEL_XOUT_L); |
fbob | 8:930aa9d0f5ae | 116 | uint8_t ay_raw_h = read_reg(ACCEL_YOUT_H); |
fbob | 8:930aa9d0f5ae | 117 | uint8_t ay_raw_l = read_reg(ACCEL_YOUT_L); |
fbob | 8:930aa9d0f5ae | 118 | uint8_t az_raw_h = read_reg(ACCEL_ZOUT_H); |
fbob | 8:930aa9d0f5ae | 119 | uint8_t az_raw_l = read_reg(ACCEL_ZOUT_L); |
fbob | 8:930aa9d0f5ae | 120 | |
fbob | 8:930aa9d0f5ae | 121 | // Reassemble raw data (one 16 bit data for each axis) |
fbob | 8:930aa9d0f5ae | 122 | int16_t ax_raw = (ax_raw_h << 8) | ax_raw_l; |
fbob | 8:930aa9d0f5ae | 123 | int16_t ay_raw = (ay_raw_h << 8) | ay_raw_l; |
fbob | 8:930aa9d0f5ae | 124 | int16_t az_raw = (az_raw_h << 8) | az_raw_l; |
fbob | 8:930aa9d0f5ae | 125 | |
fbob | 9:350a1d643ef1 | 126 | // Convert raw data to SI units [m/s^2] and also convert from sensor reference frame to drone reference frame |
fbob | 9:350a1d643ef1 | 127 | ax = ay_raw * a_res; |
fbob | 9:350a1d643ef1 | 128 | ay = -ax_raw * a_res; |
fbob | 9:350a1d643ef1 | 129 | az = -az_raw * a_res; |
fbob | 10:237258f2d05e | 130 | */ |
fbob | 10:237258f2d05e | 131 | |
fbob | 10:237258f2d05e | 132 | // MPU9250 I2C bus address |
fbob | 10:237258f2d05e | 133 | char address = MPU9250_ADDRESS; |
fbob | 10:237258f2d05e | 134 | // Register address |
fbob | 10:237258f2d05e | 135 | char reg[1] = {ACCEL_XOUT_H}; |
fbob | 10:237258f2d05e | 136 | // Data that we're going to read |
fbob | 10:237258f2d05e | 137 | char data[6]; |
fbob | 10:237258f2d05e | 138 | |
fbob | 10:237258f2d05e | 139 | // Point to register address |
fbob | 10:237258f2d05e | 140 | i2c.write(address, reg, 1); |
fbob | 10:237258f2d05e | 141 | // Read data from this address (register address will auto-increment and all three axis information (two 8 bit data each) will be read) |
fbob | 10:237258f2d05e | 142 | i2c.read(address, data, 6); |
fbob | 10:237258f2d05e | 143 | |
fbob | 10:237258f2d05e | 144 | // Reassemble the data (two 8 bit data into one 16 bit data) |
fbob | 10:237258f2d05e | 145 | int16_t ax_raw = (data[0] << 8 ) | data[1]; |
fbob | 10:237258f2d05e | 146 | int16_t ay_raw = (data[2] << 8 ) | data[3]; |
fbob | 10:237258f2d05e | 147 | int16_t az_raw = (data[4] << 8 ) | data[5]; |
fbob | 10:237258f2d05e | 148 | // Convert to SI units [m/s^2] |
fbob | 10:237258f2d05e | 149 | ax = ay_raw * a_res; |
fbob | 10:237258f2d05e | 150 | ay = -ax_raw * a_res; |
fbob | 10:237258f2d05e | 151 | az = -az_raw * a_res; |
fbob | 8:930aa9d0f5ae | 152 | } |
fbob | 8:930aa9d0f5ae | 153 | |
fbob | 8:930aa9d0f5ae | 154 | /** Read accelerometer data */ |
fbob | 8:930aa9d0f5ae | 155 | void MPU9250::read_gyro() |
fbob | 8:930aa9d0f5ae | 156 | { |
fbob | 10:237258f2d05e | 157 | /* |
fbob | 8:930aa9d0f5ae | 158 | // Read raw data (two 8 bit data for each axis) |
fbob | 8:930aa9d0f5ae | 159 | uint8_t gx_raw_h = read_reg(GYRO_XOUT_H); |
fbob | 8:930aa9d0f5ae | 160 | uint8_t gx_raw_l = read_reg(GYRO_XOUT_L); |
fbob | 8:930aa9d0f5ae | 161 | uint8_t gy_raw_h = read_reg(GYRO_YOUT_H); |
fbob | 8:930aa9d0f5ae | 162 | uint8_t gy_raw_l = read_reg(GYRO_YOUT_L); |
fbob | 8:930aa9d0f5ae | 163 | uint8_t gz_raw_h = read_reg(GYRO_ZOUT_H); |
fbob | 8:930aa9d0f5ae | 164 | uint8_t gz_raw_l = read_reg(GYRO_ZOUT_L); |
fbob | 8:930aa9d0f5ae | 165 | |
fbob | 8:930aa9d0f5ae | 166 | // Reassemble raw data (one 16 bit data for each axis) |
fbob | 8:930aa9d0f5ae | 167 | int16_t gx_raw = (gx_raw_h << 8 ) | gx_raw_l; |
fbob | 8:930aa9d0f5ae | 168 | int16_t gy_raw = (gy_raw_h << 8 ) | gy_raw_l; |
fbob | 8:930aa9d0f5ae | 169 | int16_t gz_raw = (gz_raw_h << 8 ) | gz_raw_l; |
fbob | 8:930aa9d0f5ae | 170 | |
fbob | 9:350a1d643ef1 | 171 | // Convert raw data to SI units [rad/s] and also convert from sensor reference frame to drone reference frame |
fbob | 9:350a1d643ef1 | 172 | gx = -gy_raw * g_res; |
fbob | 9:350a1d643ef1 | 173 | gy = gx_raw * g_res; |
fbob | 8:930aa9d0f5ae | 174 | gz = gz_raw * g_res; |
fbob | 10:237258f2d05e | 175 | */ |
fbob | 10:237258f2d05e | 176 | // MPU9250 I2C bus address |
fbob | 10:237258f2d05e | 177 | char address = MPU9250_ADDRESS; |
fbob | 10:237258f2d05e | 178 | // Register address |
fbob | 10:237258f2d05e | 179 | char reg[1] = {GYRO_XOUT_H}; |
fbob | 10:237258f2d05e | 180 | // Data that we're going to read |
fbob | 10:237258f2d05e | 181 | char data[6]; |
fbob | 10:237258f2d05e | 182 | |
fbob | 10:237258f2d05e | 183 | // Point to register address |
fbob | 10:237258f2d05e | 184 | i2c.write(address, reg, 1); |
fbob | 10:237258f2d05e | 185 | // Read data from this address (register address will auto-increment and all three axis information (two 8 bit data each) will be read) |
fbob | 10:237258f2d05e | 186 | i2c.read(address, data, 6); |
fbob | 10:237258f2d05e | 187 | |
fbob | 10:237258f2d05e | 188 | // Reassemble the data (two 8 bit data into one 16 bit data) |
fbob | 10:237258f2d05e | 189 | int16_t gx_raw = (data[0] << 8 ) | data[1]; |
fbob | 10:237258f2d05e | 190 | int16_t gy_raw = (data[2] << 8 ) | data[3]; |
fbob | 10:237258f2d05e | 191 | int16_t gz_raw = (data[4] << 8 ) | data[5]; |
fbob | 10:237258f2d05e | 192 | // Convert to SI units [rad/s] |
fbob | 10:237258f2d05e | 193 | gx = -gy_raw * g_res; |
fbob | 10:237258f2d05e | 194 | gy = gx_raw * g_res; |
fbob | 10:237258f2d05e | 195 | gz = gz_raw * g_res; |
fbob | 8:930aa9d0f5ae | 196 | } |
fbob | 8:930aa9d0f5ae | 197 | |
fbob | 8:930aa9d0f5ae | 198 | /** Write data into register on MPU9250 I2C bus address */ |
fbob | 8:930aa9d0f5ae | 199 | void MPU9250::write_reg(uint8_t reg, uint8_t data) |
fbob | 8:930aa9d0f5ae | 200 | { |
fbob | 8:930aa9d0f5ae | 201 | // Register address and data that will be writed |
fbob | 8:930aa9d0f5ae | 202 | char i2c_reg_data[2] = {reg, data}; |
fbob | 8:930aa9d0f5ae | 203 | |
fbob | 8:930aa9d0f5ae | 204 | // Point to register address and write data |
fbob | 8:930aa9d0f5ae | 205 | i2c.write(MPU9250_ADDRESS, i2c_reg_data, 2); |
fbob | 8:930aa9d0f5ae | 206 | } |
fbob | 8:930aa9d0f5ae | 207 | |
fbob | 8:930aa9d0f5ae | 208 | /** Read data from register on MPU9250 I2C bus address */ |
fbob | 8:930aa9d0f5ae | 209 | char MPU9250::read_reg(uint8_t reg) |
fbob | 8:930aa9d0f5ae | 210 | { |
fbob | 8:930aa9d0f5ae | 211 | // Register address |
fbob | 8:930aa9d0f5ae | 212 | char i2c_reg[1] = {reg}; |
fbob | 8:930aa9d0f5ae | 213 | // Data that will be readed |
fbob | 8:930aa9d0f5ae | 214 | char i2c_data[1]; |
fbob | 8:930aa9d0f5ae | 215 | |
fbob | 8:930aa9d0f5ae | 216 | // Point to register address |
fbob | 8:930aa9d0f5ae | 217 | i2c.write(MPU9250_ADDRESS, i2c_reg, 1); |
fbob | 8:930aa9d0f5ae | 218 | // Read data |
fbob | 8:930aa9d0f5ae | 219 | i2c.read(MPU9250_ADDRESS, i2c_data, 1); |
fbob | 8:930aa9d0f5ae | 220 | |
fbob | 8:930aa9d0f5ae | 221 | // Return readed data |
fbob | 8:930aa9d0f5ae | 222 | return i2c_data[0]; |
fbob | 8:930aa9d0f5ae | 223 | } |