mpu9250のライブラリ、I2Cを利用。開発段階のため微妙
Diff: mpu9250_i2c.cpp
- Revision:
- 0:d36bfb8300a2
- Child:
- 1:6a4c2f84180b
diff -r 000000000000 -r d36bfb8300a2 mpu9250_i2c.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mpu9250_i2c.cpp Mon Jan 09 06:28:04 2017 +0000 @@ -0,0 +1,185 @@ +#include "mbed.h" +#include "mpu9250_i2c.h" + +char mpu9250::_addr = SLAVE_ADDR_HIGH; +double mpu9250::acc_coef = ACC_LSB; +double mpu9250::gyro_coef = GYRO_LSB; +double mpu9250::mag_coef = MAG_LSB; +double mpu9250::acc_offset[3] = {0,0,0}; +double mpu9250::gyro_offset[3] = {0,0,0}; +double mpu9250::mag_offset[3] = {0,0,0}; + +mpu9250::mpu9250(I2C &_i2c, AD0 celect){ + + _nine = &_i2c; + if(celect == AD0_HIGH) _addr = SLAVE_ADDR_HIGH; + else _addr = SLAVE_ADDR_LOW; + + _nine->frequency(400000);//400kHz + init(); +} +void mpu9250::frequency(int Hz){ + _nine->frequency(Hz); +} +bool mpu9250::senserTest(){ + if(readReg(_addr, WHO_AM_I) == 0x71){ + return true; + } + else return false; +} + +bool mpu9250::mag_senserTest(){ + if(readReg(MAG_ADDR, WIA) == 0x48){ + return true; + } + else return false; +} + +void mpu9250::setOffset(double gx, double gy, double gz, double ax, double ay, double az, double mx, double my, double mz){ + gyro_offset[0] = gx; + gyro_offset[1] = gy; + gyro_offset[2] = gz; + acc_offset[0] = ax; + acc_offset[1] = ay; + acc_offset[2] = az; + mag_offset[0] = mx; + mag_offset[1] = my; + mag_offset[2] = mz; +} + +void mpu9250::setGyro(GYRO_RANGE g_range){ + char fchoice = readReg(_addr, GYRO_CONFIG) & 0x03; + char send; + if(g_range == _250DPS){ + send = 0x00 | fchoice; + gyro_coef = GYRO_LSB; + } + else if(g_range == _500DPS){ + send = 0x08 | fchoice; + gyro_coef = GYRO_LSB * 2.0; + } + else if(g_range == _1000DPS){ + send = 0x10 | fchoice; + gyro_coef = GYRO_LSB * 4.0; + } + else if(g_range == _2000DPS){ + send = 0x18 | fchoice; + gyro_coef = GYRO_LSB * 8.0; + } + writeReg(_addr, GYRO_CONFIG, send); +} +void mpu9250::setAcc(ACC_RANGE a_range){ + char send; + if(a_range == _2G){ + send = 0x00; + acc_coef = ACC_LSB; + } + else if(a_range == _4G){ + send = 0x08; + acc_coef = ACC_LSB * 2.0; + } + else if(a_range == _8G){ + send = 0x10; + acc_coef = ACC_LSB * 4.0; + } + else if(a_range == _16G){ + send = 0x18; + acc_coef = ACC_LSB * 8.0; + } + writeReg(_addr, ACCEL_CONFIG, send); +} + +void mpu9250::init(){ + + acc_coef = ACC_LSB; + gyro_coef = GYRO_LSB; + mag_coef = MAG_LSB; + + writeReg(MAG_ADDR, 0x6B, 0x00); + wait_us(70); + writeReg(_addr, 0x37, 0x02); + wait_us(70); + writeReg(MAG_ADDR, CNTL1, 0x16); + wait_us(70); + setAcc(_4G); + wait_us(70); + setGyro(_500DPS); +} + +void mpu9250::getAcc(double *acc){ + char data[6]; + short sign; + readReg(_addr, ACCEL_XOUT_H, data, 6); + sign = ((short)data[0] << 8) | (short)data[1]; + acc[0] = (double)sign * acc_coef - acc_offset[0]; + sign = ((short)data[2] << 8) | (short)data[3]; + acc[1] = (double)sign * acc_coef - acc_offset[1]; + sign = ((short)data[4] << 8) | (short)data[5]; + acc[2] = (double)sign * acc_coef - acc_offset[2]; +} +void mpu9250::getAcc(double *ax, double *ay, double *az){ + double acc[3]; + getAcc(acc); + *ax = acc[0]; + *ay = acc[1]; + *az = acc[2]; +} +void mpu9250::getGyro(double *gyro){ + char data[6]; + short sign; + readReg(_addr, GYRO_XOUT_H, data, 6); + sign = ((short)data[0] << 8) | (short)data[1]; + gyro[0] = (double)sign * gyro_coef - gyro_offset[0]; + sign = ((short)data[2] << 8) | (short)data[3]; + gyro[1] = (double)sign * gyro_coef - gyro_offset[1]; + sign = ((short)data[4] << 8) | (short)data[5]; + gyro[2] = (double)sign * gyro_coef - gyro_offset[2]; +} +void mpu9250::getGyro(double *gx, double *gy, double *gz){ + double gyro[3]; + getGyro(gyro); + *gx = gyro[0]; + *gy = gyro[1]; + *gz = gyro[2]; +} +void mpu9250::getMag(double *mag){ + char data[8]; + short sign; + readReg(MAG_ADDR, ST1, data, 8); + sign = ((short)data[2] << 8) | (short)data[1]; + mag[1] = (double)sign * mag_coef - mag_offset[1]; + sign = ((short)data[4] << 8) | (short)data[3]; + mag[0] = (double)sign * mag_coef - mag_offset[0]; + sign = ((short)data[6] << 8) | (short)data[5]; + mag[2] = (double)sign * -mag_coef - mag_offset[2]; +} +void mpu9250::getMag(double *mx, double *my, double *mz){ + double mag[3]; + getMag(mag); + *mx = mag[0]; + *my = mag[1]; + *mz = mag[2]; +} +void mpu9250::getGyroAcc(double *imu){ + char data[14]; + short sign; + readReg(_addr, ACCEL_XOUT_H, data, 14); + sign = ((short)data[0] << 8) | (short)data[1]; + imu[3] = (double)sign * acc_coef - acc_offset[0]; + sign = ((short)data[2] << 8) | (short)data[3]; + imu[4] = (double)sign * acc_coef - acc_offset[1]; + sign = ((short)data[4] << 8) | (short)data[5]; + imu[5] = (double)sign * acc_coef - acc_offset[2]; + + sign = ((short)data[8] << 8) | (short)data[9]; + imu[0] = (double)sign * gyro_coef - gyro_offset[0]; + sign = ((short)data[10] << 8) | (short)data[11]; + imu[1] = (double)sign * gyro_coef - gyro_offset[1]; + sign = ((short)data[12] << 8) | (short)data[13]; + imu[2] = (double)sign * gyro_coef - gyro_offset[2]; +} + +void mpu9250::setAccLPF(A_BAND_WIDTH band){ + writeReg(_addr, ACCEL_CONFIG2, band); + wait_us(70); +} \ No newline at end of file