mpu9250のライブラリ、I2Cを利用。
Dependents: Hybrid_AttitudeEstimation Hybrid_main_FirstEdtion rocket_logger_sinkan2018_v1 HYBRYD2018_IZU_ROCKET ... more
Fork of mpu9250_i2c by
Diff: mpu9250_i2c.cpp
- Revision:
- 15:d8b263a8138e
- Parent:
- 1:6a4c2f84180b
--- a/mpu9250_i2c.cpp Sat Jan 28 21:24:27 2017 +0000 +++ b/mpu9250_i2c.cpp Wed Dec 06 13:07:09 2017 +0000 @@ -1,19 +1,20 @@ #include "mbed.h" #include "mpu9250_i2c.h" -char mpu9250::_addr = SLAVE_ADDR_HIGH; +/* +char mpu9250::_addr = MPU9250_SLAVE_ADDR_HIGH; double mpu9250::acc_coef = ACC_LSB; double mpu9250::gyro_coef = GYRO_LSB; -double mpu9250::mag_coef = MAG_LSB; +double mpu9250::mag_coef = MPU9250_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}; +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; + if(celect == AD0_HIGH) _addr = MPU9250_SLAVE_ADDR_HIGH; + else _addr = MPU9250_SLAVE_ADDR_LOW; _nine->frequency(400000);//400kHz init(); @@ -21,21 +22,21 @@ void mpu9250::frequency(int Hz){ _nine->frequency(Hz); } -bool mpu9250::senserTest(){ +bool mpu9250::sensorTest(){ if(readReg(_addr, WHO_AM_I_MPU9250) == 0x71){ return true; } else return false; } -bool mpu9250::mag_senserTest(){ - if(readReg(MAG_ADDR, WIA) == 0x48){ +bool mpu9250::mag_sensorTest(){ + if(readReg(MPU9250_MAG_ADDR, MPU9250_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){ +void mpu9250::setOffset(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz){ gyro_offset[0] = gx; gyro_offset[1] = gy; gyro_offset[2] = gz; @@ -93,13 +94,13 @@ acc_coef = ACC_LSB; gyro_coef = GYRO_LSB; - mag_coef = MAG_LSB; + mag_coef = MPU9250_MAG_LSB; - writeReg(MAG_ADDR, 0x6B, 0x00); + writeReg(MPU9250_MAG_ADDR, 0x6B, 0x00); wait_us(70); writeReg(_addr, 0x37, 0x02); wait_us(70); - writeReg(MAG_ADDR, CNTL1, 0x16); + writeReg(MPU9250_MAG_ADDR, MPU9250_CNTL1, 0x16); wait_us(70); setAcc(_4G); wait_us(70); @@ -111,73 +112,138 @@ 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]; + acc[0] = (float)sign * acc_coef - acc_offset[0]; sign = ((short)data[2] << 8) | (short)data[3]; - acc[1] = (double)sign * acc_coef - acc_offset[1]; + acc[1] = (float)sign * acc_coef - acc_offset[1]; sign = ((short)data[4] << 8) | (short)data[5]; - acc[2] = (double)sign * acc_coef - acc_offset[2]; + acc[2] = (float)sign * acc_coef - acc_offset[2]; } + template<typename T>void mpu9250::getAcc(T *ax, T *ay, T *az){ - double acc[3]; + float acc[3]; getAcc(acc); *ax = acc[0]; *ay = acc[1]; *az = acc[2]; } + template<typename T>void mpu9250::getGyro(T *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]; + gyro[0] = (float)sign * gyro_coef - gyro_offset[0]; sign = ((short)data[2] << 8) | (short)data[3]; - gyro[1] = (double)sign * gyro_coef - gyro_offset[1]; + gyro[1] = (float)sign * gyro_coef - gyro_offset[1]; sign = ((short)data[4] << 8) | (short)data[5]; - gyro[2] = (double)sign * gyro_coef - gyro_offset[2]; + gyro[2] = (float)sign * gyro_coef - gyro_offset[2]; } + template<typename T>void mpu9250::getGyro(T *gx, T *gy, T *gz){ - double gyro[3]; + float gyro[3]; getGyro(gyro); *gx = gyro[0]; *gy = gyro[1]; *gz = gyro[2]; } + template<typename T>void mpu9250::getMag(T *mag){ char data[8]; short sign; - readReg(MAG_ADDR, ST1, data, 8); + readReg(MPU9250_MAG_ADDR, MPU9250_ST1, data, 8); sign = ((short)data[2] << 8) | (short)data[1]; - mag[1] = (double)sign * mag_coef - mag_offset[1]; + mag[1] = (float)sign * mag_coef - mag_offset[1]; sign = ((short)data[4] << 8) | (short)data[3]; - mag[0] = (double)sign * mag_coef - mag_offset[0]; + mag[0] = (float)sign * mag_coef - mag_offset[0]; sign = ((short)data[6] << 8) | (short)data[5]; - mag[2] = (double)sign * -mag_coef - mag_offset[2]; + mag[2] = (float)sign * -mag_coef - mag_offset[2]; } + template<typename T>void mpu9250::getMag(T *mx, T *my, T *mz){ - double mag[3]; + float mag[3]; getMag(mag); *mx = mag[0]; *my = mag[1]; *mz = mag[2]; } + template<typename T>void mpu9250::getGyroAcc(T *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]; + imu[3] = (float)sign * acc_coef - acc_offset[0]; sign = ((short)data[2] << 8) | (short)data[3]; - imu[4] = (double)sign * acc_coef - acc_offset[1]; + imu[4] = (float)sign * acc_coef - acc_offset[1]; sign = ((short)data[4] << 8) | (short)data[5]; - imu[5] = (double)sign * acc_coef - acc_offset[2]; + imu[5] = (float)sign * acc_coef - acc_offset[2]; + + sign = ((short)data[8] << 8) | (short)data[9]; + imu[0] = (float)sign * gyro_coef - gyro_offset[0]; + sign = ((short)data[10] << 8) | (short)data[11]; + imu[1] = (float)sign * gyro_coef - gyro_offset[1]; + sign = ((short)data[12] << 8) | (short)data[13]; + imu[2] = (float)sign * gyro_coef - gyro_offset[2]; +} + +template<typename T>void mpu9250::getAll(T *imu, T *mag){ + + char data[14]; + short sign; + readReg(_addr, ACCEL_XOUT_H, data, 14); + sign = ((short)data[0] << 8) | (short)data[1]; + imu[3] = (float)sign * acc_coef - acc_offset[0]; + sign = ((short)data[2] << 8) | (short)data[3]; + imu[4] = (float)sign * acc_coef - acc_offset[1]; + sign = ((short)data[4] << 8) | (short)data[5]; + imu[5] = (float)sign * acc_coef - acc_offset[2]; sign = ((short)data[8] << 8) | (short)data[9]; - imu[0] = (double)sign * gyro_coef - gyro_offset[0]; + imu[0] = (float)sign * gyro_coef - gyro_offset[0]; sign = ((short)data[10] << 8) | (short)data[11]; - imu[1] = (double)sign * gyro_coef - gyro_offset[1]; + imu[1] = (float)sign * gyro_coef - gyro_offset[1]; sign = ((short)data[12] << 8) | (short)data[13]; - imu[2] = (double)sign * gyro_coef - gyro_offset[2]; + imu[2] = (float)sign * gyro_coef - gyro_offset[2]; + + readReg(MPU9250_MAG_ADDR, MPU9250_ST1, data, 8); + sign = ((short)data[2] << 8) | (short)data[1]; + mag[1] = (float)sign * mag_coef - mag_offset[1]; + sign = ((short)data[4] << 8) | (short)data[3]; + mag[0] = (float)sign * mag_coef - mag_offset[0]; + sign = ((short)data[6] << 8) | (short)data[5]; + mag[2] = (float)sign * -mag_coef - mag_offset[2]; + } + +template<typename T>void mpu9250::getAll(T *all){ + + char data[14]; + short sign; + readReg(_addr, ACCEL_XOUT_H, data, 14); + sign = ((short)data[0] << 8) | (short)data[1]; + all[3] = (float)sign * acc_coef - acc_offset[0]; + sign = ((short)data[2] << 8) | (short)data[3]; + all[4] = (float)sign * acc_coef - acc_offset[1]; + sign = ((short)data[4] << 8) | (short)data[5]; + all[5] = (float)sign * acc_coef - acc_offset[2]; + + sign = ((short)data[8] << 8) | (short)data[9]; + all[0] = (float)sign * gyro_coef - gyro_offset[0]; + sign = ((short)data[10] << 8) | (short)data[11]; + all[1] = (float)sign * gyro_coef - gyro_offset[1]; + sign = ((short)data[12] << 8) | (short)data[13]; + all[2] = (float)sign * gyro_coef - gyro_offset[2]; + + readReg(MPU9250_MAG_ADDR, MPU9250_ST1, data, 8); + sign = ((short)data[2] << 8) | (short)data[1]; + all[7] = (float)sign * mag_coef - mag_offset[1]; + sign = ((short)data[4] << 8) | (short)data[3]; + all[6] = (float)sign * mag_coef - mag_offset[0]; + sign = ((short)data[6] << 8) | (short)data[5]; + all[8] = (float)sign * -mag_coef - mag_offset[2]; + return; +} + void mpu9250::setAccLPF(A_BAND_WIDTH band){ writeReg(_addr, ACCEL_CONFIG2, band); @@ -201,4 +267,9 @@ template void mpu9250::getGyroAcc<double>(double *imu);//gx,gy,gz,ax,ay,az template void mpu9250::getGyroAcc<float>(float *imu);//gx,gy,gz,ax,ay,az - \ No newline at end of file + +template void mpu9250::getAll<double>(double *all); +template void mpu9250::getAll<float>(float *all); + +template void mpu9250::getAll<float>(float *imu, float *mag); +template void mpu9250::getAll<double>(double *imu, double *mag); \ No newline at end of file