Fabio Bobrow / CrazyflieSensors

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?

UserRevisionLine numberNew 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 }