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/MPU9250.cpp
- Revision:
- 10:237258f2d05e
- Parent:
- 9:350a1d643ef1
--- a/MPU9250/MPU9250.cpp Fri Jul 20 20:40:10 2018 +0000 +++ b/MPU9250/MPU9250.cpp Sun Aug 26 17:17:46 2018 +0000 @@ -36,7 +36,7 @@ void MPU9250::setup_i2c() { // Setup I2C bus frequency to 100kHz - i2c.frequency(400000); + i2c.frequency(100000); } /** Test I2C bus */ @@ -109,6 +109,7 @@ /** Read accelerometer output data */ void MPU9250::read_accel() { + /* // Read raw data (two 8 bit data for each axis) uint8_t ax_raw_h = read_reg(ACCEL_XOUT_H); uint8_t ax_raw_l = read_reg(ACCEL_XOUT_L); @@ -126,11 +127,34 @@ ax = ay_raw * a_res; ay = -ax_raw * a_res; az = -az_raw * a_res; + */ + + // MPU9250 I2C bus 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 = ay_raw * a_res; + ay = -ax_raw * a_res; + az = -az_raw * a_res; } /** Read accelerometer data */ void MPU9250::read_gyro() { + /* // Read raw data (two 8 bit data for each axis) uint8_t gx_raw_h = read_reg(GYRO_XOUT_H); uint8_t gx_raw_l = read_reg(GYRO_XOUT_L); @@ -148,6 +172,27 @@ gx = -gy_raw * g_res; gy = gx_raw * g_res; gz = gz_raw * g_res; + */ + // MPU9250 I2C bus 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 = -gy_raw * g_res; + gy = gx_raw * g_res; + gz = gz_raw * g_res; } /** Write data into register on MPU9250 I2C bus address */