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@2:3d5f4b1c7bdb, 2018-04-17 (annotated)

Committer:
fbob
Date:
Tue Apr 17 23:35:21 2018 +0000
Revision:
2:3d5f4b1c7bdb
Parent:
MPU9250.cpp@0:3d8a882699ef
Child:
3:2f2b8e863991
Updated name

Who changed what in which revision?

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