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:
- 3:2f2b8e863991
- Parent:
- 2:3d5f4b1c7bdb
- Child:
- 5:1ef8b91a0318
--- a/MPU9250/MPU9250.cpp Tue Apr 17 23:35:21 2018 +0000 +++ b/MPU9250/MPU9250.cpp Thu Apr 19 19:41:02 2018 +0000 @@ -1,84 +1,96 @@ #include "MPU9250.h" -// Class constructor +/** 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) +/** Initialize accelerometer and gyroscope */ +void MPU9250::init() { - setup_accel(a_scale); - setup_gyro(g_scale); + setup_aux_i2c(); + setup_accel(); + setup_gyro(); +} + +/** Read accelerometer and gyroscope */ +void MPU9250::read() +{ + read_accel(); + read_gyro(); } -// Setup accelerometer +/** 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 | a_scale << 3 | 0b000}; + 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) accordingly to chosed scale (g) + // Adjust resolution [m/s^2 / bit] accordingly to chosed scale (g) switch (a_scale) { case ACCEL_SCALE_2G: - a.res = (2.0/32768.0)*GRAVITY; + a_res = (2.0*GRAVITY)/32768.0; break; case ACCEL_SCALE_4G: - a.res = (4.0/32768.0)*GRAVITY; + a_res = (4.0*GRAVITY)/32768.0; break; case ACCEL_SCALE_8G: - a.res = (8.0/32768.0)*GRAVITY; + a_res = (8.0*GRAVITY)/32768.0; break; case ACCEL_SCALE_16G: - a.res = (16.0/32768.0)*GRAVITY; + a_res = (16.0*GRAVITY)/32768.0; break; } } -// Setup gyroscope +/** 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 | g_scale << 3 | 0b000}; + 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) accordingly to chosed scale (º/s) + // Adjust resolution [rad/s / bit] accordingly to chosed scale switch (g_scale) { case GYRO_SCALE_250DPS: - g.res = (250.0/32768.0)*(PI/180); + g_res = (250.0*(PI/180.0))/32768.0; break; case GYRO_SCALE_500DPS: - g.res = (500.0/32768.0)*(PI/180); + g_res = (500.0*(PI/180.0))/32768.0; break; case GYRO_SCALE_1000DPS: - g.res = (1000.0/32768.0)*(PI/180); + g_res = (1000.0*(PI/180))/32768.0; break; case GYRO_SCALE_2000DPS: - g.res = (2000.0/32768.0)*(PI/180); + g_res = (2000.0*(PI/180.0))/32768.0; break; } } - -// Read accelerometer, gyroscope and magnetometer output data -void MPU9250::read() -{ - read_accel(); - read_gyro(); -} -// Read accelerometer output data +/** Read accelerometer output data */ void MPU9250::read_accel() { // MPU9250 I2C address @@ -94,16 +106,16 @@ 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; + 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 gyroscope output data +/** Read accelerometer output data */ void MPU9250::read_gyro() { // MPU9250 I2C address @@ -119,11 +131,11 @@ 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; + 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; } \ No newline at end of file