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.cpp@0:3d8a882699ef, 2018-04-17 (annotated)
- Committer:
- fbob
- Date:
- Tue Apr 17 23:07:50 2018 +0000
- Revision:
- 0:3d8a882699ef
1.0
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 | 0:3d8a882699ef | 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 | 0:3d8a882699ef | 8 | |
fbob | 0:3d8a882699ef | 9 | // Setup accelerometer, gyroscope and magnetometer |
fbob | 0:3d8a882699ef | 10 | void MPU9250::setup(accel_scale a_scale, gyro_scale g_scale) |
fbob | 0:3d8a882699ef | 11 | { |
fbob | 0:3d8a882699ef | 12 | setup_accel(a_scale); |
fbob | 0:3d8a882699ef | 13 | setup_gyro(g_scale); |
fbob | 0:3d8a882699ef | 14 | } |
fbob | 0:3d8a882699ef | 15 | |
fbob | 0:3d8a882699ef | 16 | // Setup accelerometer |
fbob | 0:3d8a882699ef | 17 | void MPU9250::setup_accel(accel_scale a_scale) |
fbob | 0:3d8a882699ef | 18 | { |
fbob | 0:3d8a882699ef | 19 | // MPU9250 I2C address |
fbob | 0:3d8a882699ef | 20 | char address = MPU9250_ADDRESS; |
fbob | 0:3d8a882699ef | 21 | // Register address and data that we're going to write |
fbob | 0:3d8a882699ef | 22 | char reg_data[2] = {ACCEL_CONFIG_1, 0b000 | a_scale << 3 | 0b000}; |
fbob | 0:3d8a882699ef | 23 | |
fbob | 0:3d8a882699ef | 24 | // Point to register address and write data into this address |
fbob | 0:3d8a882699ef | 25 | i2c.write(address, reg_data, 2); |
fbob | 0:3d8a882699ef | 26 | |
fbob | 0:3d8a882699ef | 27 | // Adjust resolution (m/s^2) accordingly to chosed scale (g) |
fbob | 0:3d8a882699ef | 28 | switch (a_scale) |
fbob | 0:3d8a882699ef | 29 | { |
fbob | 0:3d8a882699ef | 30 | case ACCEL_SCALE_2G: |
fbob | 0:3d8a882699ef | 31 | a.res = (2.0/32768.0)*GRAVITY; |
fbob | 0:3d8a882699ef | 32 | break; |
fbob | 0:3d8a882699ef | 33 | case ACCEL_SCALE_4G: |
fbob | 0:3d8a882699ef | 34 | a.res = (4.0/32768.0)*GRAVITY; |
fbob | 0:3d8a882699ef | 35 | break; |
fbob | 0:3d8a882699ef | 36 | case ACCEL_SCALE_8G: |
fbob | 0:3d8a882699ef | 37 | a.res = (8.0/32768.0)*GRAVITY; |
fbob | 0:3d8a882699ef | 38 | break; |
fbob | 0:3d8a882699ef | 39 | case ACCEL_SCALE_16G: |
fbob | 0:3d8a882699ef | 40 | a.res = (16.0/32768.0)*GRAVITY; |
fbob | 0:3d8a882699ef | 41 | break; |
fbob | 0:3d8a882699ef | 42 | } |
fbob | 0:3d8a882699ef | 43 | } |
fbob | 0:3d8a882699ef | 44 | |
fbob | 0:3d8a882699ef | 45 | // Setup gyroscope |
fbob | 0:3d8a882699ef | 46 | void MPU9250::setup_gyro(gyro_scale g_scale) |
fbob | 0:3d8a882699ef | 47 | { |
fbob | 0:3d8a882699ef | 48 | // MPU9250 I2C address |
fbob | 0:3d8a882699ef | 49 | char address = MPU9250_ADDRESS; |
fbob | 0:3d8a882699ef | 50 | // Register address and data that we're going to write |
fbob | 0:3d8a882699ef | 51 | char reg_data[2] = {GYRO_CONFIG, 0b000 | g_scale << 3 | 0b000}; |
fbob | 0:3d8a882699ef | 52 | |
fbob | 0:3d8a882699ef | 53 | // Point to register address and write data into this address |
fbob | 0:3d8a882699ef | 54 | i2c.write(address, reg_data, 2); |
fbob | 0:3d8a882699ef | 55 | |
fbob | 0:3d8a882699ef | 56 | // Adjust resolution (rad/s) accordingly to chosed scale (º/s) |
fbob | 0:3d8a882699ef | 57 | switch (g_scale) |
fbob | 0:3d8a882699ef | 58 | { |
fbob | 0:3d8a882699ef | 59 | case GYRO_SCALE_250DPS: |
fbob | 0:3d8a882699ef | 60 | g.res = (250.0/32768.0)*(PI/180); |
fbob | 0:3d8a882699ef | 61 | break; |
fbob | 0:3d8a882699ef | 62 | case GYRO_SCALE_500DPS: |
fbob | 0:3d8a882699ef | 63 | g.res = (500.0/32768.0)*(PI/180); |
fbob | 0:3d8a882699ef | 64 | break; |
fbob | 0:3d8a882699ef | 65 | case GYRO_SCALE_1000DPS: |
fbob | 0:3d8a882699ef | 66 | g.res = (1000.0/32768.0)*(PI/180); |
fbob | 0:3d8a882699ef | 67 | break; |
fbob | 0:3d8a882699ef | 68 | case GYRO_SCALE_2000DPS: |
fbob | 0:3d8a882699ef | 69 | g.res = (2000.0/32768.0)*(PI/180); |
fbob | 0:3d8a882699ef | 70 | break; |
fbob | 0:3d8a882699ef | 71 | } |
fbob | 0:3d8a882699ef | 72 | } |
fbob | 0:3d8a882699ef | 73 | |
fbob | 0:3d8a882699ef | 74 | // Read accelerometer, gyroscope and magnetometer output data |
fbob | 0:3d8a882699ef | 75 | void MPU9250::read() |
fbob | 0:3d8a882699ef | 76 | { |
fbob | 0:3d8a882699ef | 77 | read_accel(); |
fbob | 0:3d8a882699ef | 78 | read_gyro(); |
fbob | 0:3d8a882699ef | 79 | } |
fbob | 0:3d8a882699ef | 80 | |
fbob | 0:3d8a882699ef | 81 | // Read accelerometer output data |
fbob | 0:3d8a882699ef | 82 | void MPU9250::read_accel() |
fbob | 0:3d8a882699ef | 83 | { |
fbob | 0:3d8a882699ef | 84 | // MPU9250 I2C address |
fbob | 0:3d8a882699ef | 85 | char address = MPU9250_ADDRESS; |
fbob | 0:3d8a882699ef | 86 | // Register address |
fbob | 0:3d8a882699ef | 87 | char reg[1] = {ACCEL_XOUT_H}; |
fbob | 0:3d8a882699ef | 88 | // Data that we're going to read |
fbob | 0:3d8a882699ef | 89 | char data[6]; |
fbob | 0:3d8a882699ef | 90 | |
fbob | 0:3d8a882699ef | 91 | // Point to register address |
fbob | 0:3d8a882699ef | 92 | i2c.write(address, reg, 1); |
fbob | 0:3d8a882699ef | 93 | // 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 | 94 | i2c.read(address, data, 6); |
fbob | 0:3d8a882699ef | 95 | |
fbob | 0:3d8a882699ef | 96 | // Reassemble the data (two 8 bit data into one 16 bit data) |
fbob | 0:3d8a882699ef | 97 | a.x_raw = (data[0] << 8 ) | data[1]; |
fbob | 0:3d8a882699ef | 98 | a.y_raw = (data[2] << 8 ) | data[3]; |
fbob | 0:3d8a882699ef | 99 | a.z_raw = (data[4] << 8 ) | data[5]; |
fbob | 0:3d8a882699ef | 100 | // Convert to SI units |
fbob | 0:3d8a882699ef | 101 | a.x = a.x_raw * a.res; |
fbob | 0:3d8a882699ef | 102 | a.y = a.y_raw * a.res; |
fbob | 0:3d8a882699ef | 103 | a.z = a.z_raw * a.res; |
fbob | 0:3d8a882699ef | 104 | } |
fbob | 0:3d8a882699ef | 105 | |
fbob | 0:3d8a882699ef | 106 | // Read gyroscope output data |
fbob | 0:3d8a882699ef | 107 | void MPU9250::read_gyro() |
fbob | 0:3d8a882699ef | 108 | { |
fbob | 0:3d8a882699ef | 109 | // MPU9250 I2C address |
fbob | 0:3d8a882699ef | 110 | char address = MPU9250_ADDRESS; |
fbob | 0:3d8a882699ef | 111 | // Register address |
fbob | 0:3d8a882699ef | 112 | char reg[1] = {GYRO_XOUT_H}; |
fbob | 0:3d8a882699ef | 113 | // Data that we're going to read |
fbob | 0:3d8a882699ef | 114 | char data[6]; |
fbob | 0:3d8a882699ef | 115 | |
fbob | 0:3d8a882699ef | 116 | // Point to register address |
fbob | 0:3d8a882699ef | 117 | i2c.write(address, reg, 1); |
fbob | 0:3d8a882699ef | 118 | // 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 | 119 | i2c.read(address, data, 6); |
fbob | 0:3d8a882699ef | 120 | |
fbob | 0:3d8a882699ef | 121 | // Reassemble the data (two 8 bit data into one 16 bit data) |
fbob | 0:3d8a882699ef | 122 | g.x_raw = (data[0] << 8 ) | data[1]; |
fbob | 0:3d8a882699ef | 123 | g.y_raw = (data[2] << 8 ) | data[3]; |
fbob | 0:3d8a882699ef | 124 | g.z_raw = (data[4] << 8 ) | data[5]; |
fbob | 0:3d8a882699ef | 125 | // Convert to SI units |
fbob | 0:3d8a882699ef | 126 | g.x = g.x_raw * g.res; |
fbob | 0:3d8a882699ef | 127 | g.y = g.y_raw * g.res; |
fbob | 0:3d8a882699ef | 128 | g.z = g.z_raw * g.res; |
fbob | 0:3d8a882699ef | 129 | } |