PQMPU9250.cpp

00001 #include "mbed.h"
00002 #include "PQMPU9250.h"
00003 
00004 MPU9250::MPU9250 (I2C &i2c, AD0_t AD0)
00005 {
00006     _i2c = &i2c;
00007     _addr = AD0;
00008     _i2c->frequency(400000);
00009 }
00010 
00011 void MPU9250::begin()
00012 {
00013     cmd[0] = MPU9250_PWR_MGMT_1;
00014     cmd[1] = 0x00;
00015     _i2c->write(_addr, cmd, 2);
00016     cmd[0] = MPU9250_INT_PIN_CFG;
00017     cmd[1] = 0x02;
00018     _i2c->write(_addr, cmd, 2);
00019     set(_2G, _250DPS, _16B_8HZ);
00020 }
00021 
00022 bool MPU9250::test()
00023 {
00024     cmd[0] = MPU9250_WHO_AM_I;
00025     _i2c->write(_addr, cmd, 1);
00026     _i2c->read(_addr, buff, 1);
00027     if (buff[0] == 0x71) {
00028         return true;
00029     } else {
00030         return false;
00031     }
00032 }
00033 
00034 bool MPU9250::test_AK8963()
00035 {
00036     cmd[0] = MPU9250_WIA;
00037     _i2c->write(MPU9250_AK8963_ADDR, cmd, 1);
00038     _i2c->read(MPU9250_AK8963_ADDR, buff, 1);
00039     if (buff[0] == 0x48) {
00040         return true;
00041     } else {
00042         return false;
00043     }
00044 }
00045 
00046 void MPU9250::set(AccelConfig_t accel_config, GyroConfig_t gyro_config, MagConfig_t mag_config)
00047 {
00048     set_accel(accel_config);
00049     set_gyro(gyro_config);
00050     set_mag(mag_config);
00051 }
00052 
00053 void MPU9250::set_accel(AccelConfig_t accel_config)
00054 {
00055     cmd[0] = MPU9250_ACCEL_CONFIG;
00056     cmd[1] = accel_config;
00057     _i2c->write(_addr, cmd, 2);
00058     if (accel_config == _2G) {
00059         accel_LSB = MPU9250_ACCEL_LSB;
00060     } else if (accel_config == _4G) {
00061         accel_LSB = MPU9250_ACCEL_LSB * 2;
00062     } else if (accel_config == _8G) {
00063         accel_LSB = MPU9250_ACCEL_LSB * 4;
00064     } else if (accel_config == _16G) {
00065         accel_LSB = MPU9250_ACCEL_LSB * 8;
00066     }
00067 }
00068 
00069 void MPU9250::set_gyro(GyroConfig_t gyro_config)
00070 {
00071     cmd[0] = MPU9250_GYRO_CONFIG;
00072     cmd[1] = gyro_config;
00073     _i2c->write(_addr, cmd, 2);
00074     if (gyro_config == _250DPS) {
00075         gyro_LSB = MPU9250_GYRO_LSB;
00076     } else if (gyro_config == _500DPS) {
00077         gyro_LSB = MPU9250_GYRO_LSB * 2;
00078     } else if (gyro_config == _1000DPS) {
00079         gyro_LSB = MPU9250_GYRO_LSB * 4;
00080     } else if (gyro_config == _2000DPS) {
00081         gyro_LSB = MPU9250_GYRO_LSB * 8;
00082     }
00083 }
00084 
00085 void MPU9250::set_mag(MagConfig_t mag_config)
00086 {
00087     cmd[0] = MPU9250_CNTL1;
00088     cmd[1] = mag_config;
00089     _i2c->write(MPU9250_AK8963_ADDR, cmd, 2);
00090     mag_LSB = MPU9250_MAG_LSB;
00091 }
00092 
00093 void MPU9250::offset(float *accel, float *gyro, float *mag)
00094 {
00095     offset_accel(accel);
00096     offset_gyro(gyro);
00097     offset_mag(mag);
00098 }
00099 
00100 void MPU9250::offset_accel(float *accel)
00101 {
00102     for (int i = 0; i < 3; i++) {
00103         accel_offset[i] = accel[i];
00104     }
00105 }
00106 
00107 void MPU9250::offset_gyro(float *gyro)
00108 {
00109     for (int i = 0; i < 3; i++) {
00110         gyro_offset[i] = gyro[i];
00111     }
00112 }
00113 
00114 void MPU9250::offset_mag(float *mag)
00115 {
00116     for (int i = 0; i < 3; i++) {
00117         mag_offset[i] = mag[i];
00118     }
00119 }
00120 
00121 void MPU9250::read(float *accel, float *gyro, float *mag)
00122 {
00123     read_accel(accel);
00124     read_gyro(gyro);
00125     read_mag(mag);
00126 }
00127 
00128 void MPU9250::read_accel(float *accel)
00129 {
00130     cmd[0] = MPU9250_ACCEL_XOUT_H;
00131     _i2c->write(_addr, cmd, 1);
00132     _i2c->read(_addr, buff, 6);
00133     accel[0] = (short)(buff[0] << 8 | buff[1]) * accel_LSB - accel_offset[0];
00134     accel[1] = (short)(buff[2] << 8 | buff[3]) * accel_LSB - accel_offset[1];
00135     accel[2] = (short)(buff[4] << 8 | buff[5]) * accel_LSB - accel_offset[2];
00136 }
00137 
00138 void MPU9250::read_gyro(float *gyro)
00139 {
00140     cmd[0] = MPU9250_GYRO_XOUT_H;
00141     _i2c->write(_addr, cmd, 1);
00142     _i2c->read(_addr, buff, 6);
00143     gyro[0] = (short)(buff[0] << 8 | buff[1]) * gyro_LSB - gyro_offset[0];
00144     gyro[1] = (short)(buff[2] << 8 | buff[3]) * gyro_LSB - gyro_offset[1];
00145     gyro[2] = (short)(buff[4] << 8 | buff[5]) * gyro_LSB - gyro_offset[2];
00146 }
00147 
00148 void MPU9250::read_mag(float *mag)
00149 {
00150     cmd[0] = MPU9250_HXL;
00151     _i2c->write(MPU9250_AK8963_ADDR, cmd, 1);
00152     _i2c->read(MPU9250_AK8963_ADDR, buff, 7);
00153     mag[0] = (short)(buff[0] | buff[1] << 8) * mag_LSB - mag_offset[0];
00154     mag[1] = (short)(buff[2] | buff[3] << 8) * mag_LSB - mag_offset[1];
00155     mag[2] = (short)(buff[4] | buff[5] << 8) * mag_LSB - mag_offset[2];
00156 }