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@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?

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