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
- Committer:
- fbob
- Date:
- 2018-04-19
- Revision:
- 3:2f2b8e863991
- Parent:
- 2:3d5f4b1c7bdb
- Child:
- 5:1ef8b91a0318
File content as of revision 3:2f2b8e863991:
#include "MPU9250.h" /** Class constructor */ MPU9250::MPU9250(PinName sda, PinName scl) : i2c(sda, scl) { } /** Initialize accelerometer and gyroscope */ void MPU9250::init() { setup_aux_i2c(); setup_accel(); setup_gyro(); } /** Read accelerometer and gyroscope */ void MPU9250::read() { read_accel(); read_gyro(); } /** Setup auxiliary I2C */ void MPU9250::setup_aux_i2c() { // MPU9250 I2C address char address = MPU9250_ADDRESS; // Register address and data that we're going to write char reg_data[2] = {INT_PIN_CFG, 0b00000010}; // Point to register address and write data into this address i2c.write(address, reg_data, 2); } /** Setup accelerometer with given output data rate and full-scale range*/ void MPU9250::setup_accel(accel_scale a_scale) { // MPU9250 I2C address char address = MPU9250_ADDRESS; // Register address and data that we're going to write char reg_data[2] = {ACCEL_CONFIG_1, 0b000 << 5 | a_scale << 3 | 0b000}; // Point to register address and write data into this address i2c.write(address, reg_data, 2); // Adjust resolution [m/s^2 / bit] accordingly to chosed scale (g) switch (a_scale) { case ACCEL_SCALE_2G: a_res = (2.0*GRAVITY)/32768.0; break; case ACCEL_SCALE_4G: a_res = (4.0*GRAVITY)/32768.0; break; case ACCEL_SCALE_8G: a_res = (8.0*GRAVITY)/32768.0; break; case ACCEL_SCALE_16G: a_res = (16.0*GRAVITY)/32768.0; break; } } /** Setup gyroscope with given output data rate and full-scale range*/ void MPU9250::setup_gyro(gyro_scale g_scale) { // MPU9250 I2C address char address = MPU9250_ADDRESS; // Register address and data that we're going to write char reg_data[2] = {GYRO_CONFIG, 0b000 << 5 | g_scale << 3 | 0b000}; // Point to register address and write data into this address i2c.write(address, reg_data, 2); // Adjust resolution [rad/s / bit] accordingly to chosed scale switch (g_scale) { case GYRO_SCALE_250DPS: g_res = (250.0*(PI/180.0))/32768.0; break; case GYRO_SCALE_500DPS: g_res = (500.0*(PI/180.0))/32768.0; break; case GYRO_SCALE_1000DPS: g_res = (1000.0*(PI/180))/32768.0; break; case GYRO_SCALE_2000DPS: g_res = (2000.0*(PI/180.0))/32768.0; break; } } /** Read accelerometer output data */ void MPU9250::read_accel() { // MPU9250 I2C address char address = MPU9250_ADDRESS; // Register address char reg[1] = {ACCEL_XOUT_H}; // Data that we're going to read char data[6]; // Point to register address i2c.write(address, reg, 1); // Read data from this address (register address will auto-increment and all three axis information (two 8 bit data each) will be read) i2c.read(address, data, 6); // Reassemble the data (two 8 bit data into one 16 bit data) int16_t ax_raw = (data[0] << 8 ) | data[1]; int16_t ay_raw = (data[2] << 8 ) | data[3]; int16_t az_raw = (data[4] << 8 ) | data[5]; // Convert to SI units [m/s^2] ax = ax_raw * a_res; ay = ay_raw * a_res; az = az_raw * a_res; } /** Read accelerometer output data */ void MPU9250::read_gyro() { // MPU9250 I2C address char address = MPU9250_ADDRESS; // Register address char reg[1] = {GYRO_XOUT_H}; // Data that we're going to read char data[6]; // Point to register address i2c.write(address, reg, 1); // Read data from this address (register address will auto-increment and all three axis information (two 8 bit data each) will be read) i2c.read(address, data, 6); // Reassemble the data (two 8 bit data into one 16 bit data) int16_t gx_raw = (data[0] << 8 ) | data[1]; int16_t gy_raw = (data[2] << 8 ) | data[3]; int16_t gz_raw = (data[4] << 8 ) | data[5]; // Convert to SI units [rad/s] gx = gx_raw * g_res; gy = gy_raw * g_res; gz = gz_raw * g_res; }