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@3:2f2b8e863991, 2018-04-19 (annotated)
- Committer:
- fbob
- Date:
- Thu Apr 19 19:41:02 2018 +0000
- Revision:
- 3:2f2b8e863991
- Parent:
- 2:3d5f4b1c7bdb
- Child:
- 5:1ef8b91a0318
Included barometer class
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
fbob | 0:3d8a882699ef | 1 | #include "MPU9250.h" |
fbob | 0:3d8a882699ef | 2 | |
fbob | 3:2f2b8e863991 | 3 | /** Class constructor */ |
fbob | 0:3d8a882699ef | 4 | MPU9250::MPU9250(PinName sda, PinName scl) : i2c(sda, scl) |
fbob | 0:3d8a882699ef | 5 | { |
fbob | 0:3d8a882699ef | 6 | } |
fbob | 0:3d8a882699ef | 7 | |
fbob | 3:2f2b8e863991 | 8 | /** Initialize accelerometer and gyroscope */ |
fbob | 3:2f2b8e863991 | 9 | void MPU9250::init() |
fbob | 0:3d8a882699ef | 10 | { |
fbob | 3:2f2b8e863991 | 11 | setup_aux_i2c(); |
fbob | 3:2f2b8e863991 | 12 | setup_accel(); |
fbob | 3:2f2b8e863991 | 13 | setup_gyro(); |
fbob | 3:2f2b8e863991 | 14 | } |
fbob | 3:2f2b8e863991 | 15 | |
fbob | 3:2f2b8e863991 | 16 | /** Read accelerometer and gyroscope */ |
fbob | 3:2f2b8e863991 | 17 | void MPU9250::read() |
fbob | 3:2f2b8e863991 | 18 | { |
fbob | 3:2f2b8e863991 | 19 | read_accel(); |
fbob | 3:2f2b8e863991 | 20 | read_gyro(); |
fbob | 0:3d8a882699ef | 21 | } |
fbob | 0:3d8a882699ef | 22 | |
fbob | 3:2f2b8e863991 | 23 | /** Setup auxiliary I2C */ |
fbob | 3:2f2b8e863991 | 24 | void MPU9250::setup_aux_i2c() |
fbob | 3:2f2b8e863991 | 25 | { |
fbob | 3:2f2b8e863991 | 26 | // MPU9250 I2C address |
fbob | 3:2f2b8e863991 | 27 | char address = MPU9250_ADDRESS; |
fbob | 3:2f2b8e863991 | 28 | // Register address and data that we're going to write |
fbob | 3:2f2b8e863991 | 29 | char reg_data[2] = {INT_PIN_CFG, 0b00000010}; |
fbob | 3:2f2b8e863991 | 30 | |
fbob | 3:2f2b8e863991 | 31 | // Point to register address and write data into this address |
fbob | 3:2f2b8e863991 | 32 | i2c.write(address, reg_data, 2); |
fbob | 3:2f2b8e863991 | 33 | } |
fbob | 3:2f2b8e863991 | 34 | |
fbob | 3:2f2b8e863991 | 35 | /** Setup accelerometer with given output data rate and full-scale range*/ |
fbob | 0:3d8a882699ef | 36 | void MPU9250::setup_accel(accel_scale a_scale) |
fbob | 0:3d8a882699ef | 37 | { |
fbob | 0:3d8a882699ef | 38 | // MPU9250 I2C address |
fbob | 0:3d8a882699ef | 39 | char address = MPU9250_ADDRESS; |
fbob | 0:3d8a882699ef | 40 | // Register address and data that we're going to write |
fbob | 3:2f2b8e863991 | 41 | char reg_data[2] = {ACCEL_CONFIG_1, 0b000 << 5 | a_scale << 3 | 0b000}; |
fbob | 0:3d8a882699ef | 42 | |
fbob | 0:3d8a882699ef | 43 | // Point to register address and write data into this address |
fbob | 0:3d8a882699ef | 44 | i2c.write(address, reg_data, 2); |
fbob | 0:3d8a882699ef | 45 | |
fbob | 3:2f2b8e863991 | 46 | // Adjust resolution [m/s^2 / bit] accordingly to chosed scale (g) |
fbob | 0:3d8a882699ef | 47 | switch (a_scale) |
fbob | 0:3d8a882699ef | 48 | { |
fbob | 0:3d8a882699ef | 49 | case ACCEL_SCALE_2G: |
fbob | 3:2f2b8e863991 | 50 | a_res = (2.0*GRAVITY)/32768.0; |
fbob | 0:3d8a882699ef | 51 | break; |
fbob | 0:3d8a882699ef | 52 | case ACCEL_SCALE_4G: |
fbob | 3:2f2b8e863991 | 53 | a_res = (4.0*GRAVITY)/32768.0; |
fbob | 0:3d8a882699ef | 54 | break; |
fbob | 0:3d8a882699ef | 55 | case ACCEL_SCALE_8G: |
fbob | 3:2f2b8e863991 | 56 | a_res = (8.0*GRAVITY)/32768.0; |
fbob | 0:3d8a882699ef | 57 | break; |
fbob | 0:3d8a882699ef | 58 | case ACCEL_SCALE_16G: |
fbob | 3:2f2b8e863991 | 59 | a_res = (16.0*GRAVITY)/32768.0; |
fbob | 0:3d8a882699ef | 60 | break; |
fbob | 0:3d8a882699ef | 61 | } |
fbob | 0:3d8a882699ef | 62 | } |
fbob | 0:3d8a882699ef | 63 | |
fbob | 3:2f2b8e863991 | 64 | /** Setup gyroscope with given output data rate and full-scale range*/ |
fbob | 0:3d8a882699ef | 65 | void MPU9250::setup_gyro(gyro_scale g_scale) |
fbob | 0:3d8a882699ef | 66 | { |
fbob | 0:3d8a882699ef | 67 | // MPU9250 I2C address |
fbob | 0:3d8a882699ef | 68 | char address = MPU9250_ADDRESS; |
fbob | 0:3d8a882699ef | 69 | // Register address and data that we're going to write |
fbob | 3:2f2b8e863991 | 70 | char reg_data[2] = {GYRO_CONFIG, 0b000 << 5 | g_scale << 3 | 0b000}; |
fbob | 0:3d8a882699ef | 71 | |
fbob | 0:3d8a882699ef | 72 | // Point to register address and write data into this address |
fbob | 0:3d8a882699ef | 73 | i2c.write(address, reg_data, 2); |
fbob | 0:3d8a882699ef | 74 | |
fbob | 3:2f2b8e863991 | 75 | // Adjust resolution [rad/s / bit] accordingly to chosed scale |
fbob | 0:3d8a882699ef | 76 | switch (g_scale) |
fbob | 0:3d8a882699ef | 77 | { |
fbob | 0:3d8a882699ef | 78 | case GYRO_SCALE_250DPS: |
fbob | 3:2f2b8e863991 | 79 | g_res = (250.0*(PI/180.0))/32768.0; |
fbob | 0:3d8a882699ef | 80 | break; |
fbob | 0:3d8a882699ef | 81 | case GYRO_SCALE_500DPS: |
fbob | 3:2f2b8e863991 | 82 | g_res = (500.0*(PI/180.0))/32768.0; |
fbob | 0:3d8a882699ef | 83 | break; |
fbob | 0:3d8a882699ef | 84 | case GYRO_SCALE_1000DPS: |
fbob | 3:2f2b8e863991 | 85 | g_res = (1000.0*(PI/180))/32768.0; |
fbob | 0:3d8a882699ef | 86 | break; |
fbob | 0:3d8a882699ef | 87 | case GYRO_SCALE_2000DPS: |
fbob | 3:2f2b8e863991 | 88 | g_res = (2000.0*(PI/180.0))/32768.0; |
fbob | 0:3d8a882699ef | 89 | break; |
fbob | 0:3d8a882699ef | 90 | } |
fbob | 0:3d8a882699ef | 91 | } |
fbob | 0:3d8a882699ef | 92 | |
fbob | 3:2f2b8e863991 | 93 | /** Read accelerometer output data */ |
fbob | 0:3d8a882699ef | 94 | void MPU9250::read_accel() |
fbob | 0:3d8a882699ef | 95 | { |
fbob | 0:3d8a882699ef | 96 | // MPU9250 I2C address |
fbob | 0:3d8a882699ef | 97 | char address = MPU9250_ADDRESS; |
fbob | 0:3d8a882699ef | 98 | // Register address |
fbob | 0:3d8a882699ef | 99 | char reg[1] = {ACCEL_XOUT_H}; |
fbob | 0:3d8a882699ef | 100 | // Data that we're going to read |
fbob | 0:3d8a882699ef | 101 | char data[6]; |
fbob | 0:3d8a882699ef | 102 | |
fbob | 0:3d8a882699ef | 103 | // Point to register address |
fbob | 0:3d8a882699ef | 104 | i2c.write(address, reg, 1); |
fbob | 0:3d8a882699ef | 105 | // Read data from this address (register address will auto-increment and all three axis information (two 8 bit data each) will be read) |
fbob | 0:3d8a882699ef | 106 | i2c.read(address, data, 6); |
fbob | 0:3d8a882699ef | 107 | |
fbob | 0:3d8a882699ef | 108 | // Reassemble the data (two 8 bit data into one 16 bit data) |
fbob | 3:2f2b8e863991 | 109 | int16_t ax_raw = (data[0] << 8 ) | data[1]; |
fbob | 3:2f2b8e863991 | 110 | int16_t ay_raw = (data[2] << 8 ) | data[3]; |
fbob | 3:2f2b8e863991 | 111 | int16_t az_raw = (data[4] << 8 ) | data[5]; |
fbob | 3:2f2b8e863991 | 112 | // Convert to SI units [m/s^2] |
fbob | 3:2f2b8e863991 | 113 | ax = ax_raw * a_res; |
fbob | 3:2f2b8e863991 | 114 | ay = ay_raw * a_res; |
fbob | 3:2f2b8e863991 | 115 | az = az_raw * a_res; |
fbob | 0:3d8a882699ef | 116 | } |
fbob | 0:3d8a882699ef | 117 | |
fbob | 3:2f2b8e863991 | 118 | /** Read accelerometer output data */ |
fbob | 0:3d8a882699ef | 119 | void MPU9250::read_gyro() |
fbob | 0:3d8a882699ef | 120 | { |
fbob | 0:3d8a882699ef | 121 | // MPU9250 I2C address |
fbob | 0:3d8a882699ef | 122 | char address = MPU9250_ADDRESS; |
fbob | 0:3d8a882699ef | 123 | // Register address |
fbob | 0:3d8a882699ef | 124 | char reg[1] = {GYRO_XOUT_H}; |
fbob | 0:3d8a882699ef | 125 | // Data that we're going to read |
fbob | 0:3d8a882699ef | 126 | char data[6]; |
fbob | 0:3d8a882699ef | 127 | |
fbob | 0:3d8a882699ef | 128 | // Point to register address |
fbob | 0:3d8a882699ef | 129 | i2c.write(address, reg, 1); |
fbob | 0:3d8a882699ef | 130 | // Read data from this address (register address will auto-increment and all three axis information (two 8 bit data each) will be read) |
fbob | 0:3d8a882699ef | 131 | i2c.read(address, data, 6); |
fbob | 0:3d8a882699ef | 132 | |
fbob | 0:3d8a882699ef | 133 | // Reassemble the data (two 8 bit data into one 16 bit data) |
fbob | 3:2f2b8e863991 | 134 | int16_t gx_raw = (data[0] << 8 ) | data[1]; |
fbob | 3:2f2b8e863991 | 135 | int16_t gy_raw = (data[2] << 8 ) | data[3]; |
fbob | 3:2f2b8e863991 | 136 | int16_t gz_raw = (data[4] << 8 ) | data[5]; |
fbob | 3:2f2b8e863991 | 137 | // Convert to SI units [rad/s] |
fbob | 3:2f2b8e863991 | 138 | gx = gx_raw * g_res; |
fbob | 3:2f2b8e863991 | 139 | gy = gy_raw * g_res; |
fbob | 3:2f2b8e863991 | 140 | gz = gz_raw * g_res; |
fbob | 0:3d8a882699ef | 141 | } |