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
Diff: MPU9250.cpp
- Revision:
- 0:3d8a882699ef
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPU9250.cpp Tue Apr 17 23:07:50 2018 +0000 @@ -0,0 +1,129 @@ +#include "MPU9250.h" + +// Class constructor +MPU9250::MPU9250(PinName sda, PinName scl) : i2c(sda, scl) +{ + // +} + +// Setup accelerometer, gyroscope and magnetometer +void MPU9250::setup(accel_scale a_scale, gyro_scale g_scale) +{ + setup_accel(a_scale); + setup_gyro(g_scale); +} + +// Setup accelerometer +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 | 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) accordingly to chosed scale (g) + switch (a_scale) + { + case ACCEL_SCALE_2G: + a.res = (2.0/32768.0)*GRAVITY; + break; + case ACCEL_SCALE_4G: + a.res = (4.0/32768.0)*GRAVITY; + break; + case ACCEL_SCALE_8G: + a.res = (8.0/32768.0)*GRAVITY; + break; + case ACCEL_SCALE_16G: + a.res = (16.0/32768.0)*GRAVITY; + break; + } +} + +// Setup gyroscope +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 | g_scale << 3 | 0b000}; + + // Point to register address and write data into this address + i2c.write(address, reg_data, 2); + + // Adjust resolution (rad/s) accordingly to chosed scale (º/s) + switch (g_scale) + { + case GYRO_SCALE_250DPS: + g.res = (250.0/32768.0)*(PI/180); + break; + case GYRO_SCALE_500DPS: + g.res = (500.0/32768.0)*(PI/180); + break; + case GYRO_SCALE_1000DPS: + g.res = (1000.0/32768.0)*(PI/180); + break; + case GYRO_SCALE_2000DPS: + g.res = (2000.0/32768.0)*(PI/180); + break; + } +} + +// Read accelerometer, gyroscope and magnetometer output data +void MPU9250::read() +{ + read_accel(); + read_gyro(); +} + +// 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) + a.x_raw = (data[0] << 8 ) | data[1]; + a.y_raw = (data[2] << 8 ) | data[3]; + a.z_raw = (data[4] << 8 ) | data[5]; + // Convert to SI units + a.x = a.x_raw * a.res; + a.y = a.y_raw * a.res; + a.z = a.z_raw * a.res; +} + +// Read gyroscope 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) + g.x_raw = (data[0] << 8 ) | data[1]; + g.y_raw = (data[2] << 8 ) | data[3]; + g.z_raw = (data[4] << 8 ) | data[5]; + // Convert to SI units + g.x = g.x_raw * g.res; + g.y = g.y_raw * g.res; + g.z = g.z_raw * g.res; +} \ No newline at end of file