Gaku Matsumoto / mpu9250_i2c

mpu9250のライブラリ、I2Cを利用。開発段階のため微妙

Dependents:   library

mpu9250_i2c.cpp@10:fad6675651c1, 2017-01-28 (annotated)

Committer:
Gaku0606
Date:
Sat Jan 28 21:00:46 2017 +0000
Revision:
10:fad6675651c1
Parent:
1:6a4c2f84180b 
ggg

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Gaku0606 0:d36bfb8300a2 1 #include "mbed.h"
Gaku0606 0:d36bfb8300a2 2 #include "mpu9250_i2c.h"
Gaku0606 0:d36bfb8300a2 3
Gaku0606 0:d36bfb8300a2 4 char mpu9250::_addr = SLAVE_ADDR_HIGH;
Gaku0606 0:d36bfb8300a2 5 double mpu9250::acc_coef = ACC_LSB;
Gaku0606 0:d36bfb8300a2 6 double mpu9250::gyro_coef = GYRO_LSB;
Gaku0606 0:d36bfb8300a2 7 double mpu9250::mag_coef = MAG_LSB;
Gaku0606 0:d36bfb8300a2 8 double mpu9250::acc_offset[3] = {0,0,0};
Gaku0606 0:d36bfb8300a2 9 double mpu9250::gyro_offset[3] = {0,0,0};
Gaku0606 0:d36bfb8300a2 10 double mpu9250::mag_offset[3] = {0,0,0};
Gaku0606 0:d36bfb8300a2 11
Gaku0606 0:d36bfb8300a2 12 mpu9250::mpu9250(I2C &_i2c, AD0 celect){
Gaku0606 0:d36bfb8300a2 13
Gaku0606 0:d36bfb8300a2 14 _nine = &_i2c;
Gaku0606 0:d36bfb8300a2 15 if(celect == AD0_HIGH) _addr = SLAVE_ADDR_HIGH;
Gaku0606 0:d36bfb8300a2 16 else _addr = SLAVE_ADDR_LOW;
Gaku0606 0:d36bfb8300a2 17
Gaku0606 0:d36bfb8300a2 18 _nine->frequency(400000);//400kHz
Gaku0606 0:d36bfb8300a2 19 init();
Gaku0606 0:d36bfb8300a2 20 }
Gaku0606 0:d36bfb8300a2 21 void mpu9250::frequency(int Hz){
Gaku0606 0:d36bfb8300a2 22 _nine->frequency(Hz);
Gaku0606 0:d36bfb8300a2 23 }
Gaku0606 0:d36bfb8300a2 24 bool mpu9250::senserTest(){
Gaku0606 1:6a4c2f84180b 25 if(readReg(_addr, WHO_AM_I_MPU9250) == 0x71){
Gaku0606 0:d36bfb8300a2 26 return true;
Gaku0606 0:d36bfb8300a2 27 }
Gaku0606 0:d36bfb8300a2 28 else return false;
Gaku0606 0:d36bfb8300a2 29 }
Gaku0606 0:d36bfb8300a2 30
Gaku0606 0:d36bfb8300a2 31 bool mpu9250::mag_senserTest(){
Gaku0606 0:d36bfb8300a2 32 if(readReg(MAG_ADDR, WIA) == 0x48){
Gaku0606 0:d36bfb8300a2 33 return true;
Gaku0606 0:d36bfb8300a2 34 }
Gaku0606 0:d36bfb8300a2 35 else return false;
Gaku0606 0:d36bfb8300a2 36 }
Gaku0606 0:d36bfb8300a2 37
Gaku0606 0:d36bfb8300a2 38 void mpu9250::setOffset(double gx, double gy, double gz, double ax, double ay, double az, double mx, double my, double mz){
Gaku0606 0:d36bfb8300a2 39 gyro_offset[0] = gx;
Gaku0606 0:d36bfb8300a2 40 gyro_offset[1] = gy;
Gaku0606 0:d36bfb8300a2 41 gyro_offset[2] = gz;
Gaku0606 0:d36bfb8300a2 42 acc_offset[0] = ax;
Gaku0606 0:d36bfb8300a2 43 acc_offset[1] = ay;
Gaku0606 0:d36bfb8300a2 44 acc_offset[2] = az;
Gaku0606 0:d36bfb8300a2 45 mag_offset[0] = mx;
Gaku0606 0:d36bfb8300a2 46 mag_offset[1] = my;
Gaku0606 0:d36bfb8300a2 47 mag_offset[2] = mz;
Gaku0606 0:d36bfb8300a2 48 }
Gaku0606 0:d36bfb8300a2 49
Gaku0606 0:d36bfb8300a2 50 void mpu9250::setGyro(GYRO_RANGE g_range){
Gaku0606 0:d36bfb8300a2 51 char fchoice = readReg(_addr, GYRO_CONFIG) & 0x03;
Gaku0606 0:d36bfb8300a2 52 char send;
Gaku0606 0:d36bfb8300a2 53 if(g_range == _250DPS){
Gaku0606 0:d36bfb8300a2 54 send = 0x00 | fchoice;
Gaku0606 0:d36bfb8300a2 55 gyro_coef = GYRO_LSB;
Gaku0606 0:d36bfb8300a2 56 }
Gaku0606 0:d36bfb8300a2 57 else if(g_range == _500DPS){
Gaku0606 0:d36bfb8300a2 58 send = 0x08 | fchoice;
Gaku0606 0:d36bfb8300a2 59 gyro_coef = GYRO_LSB * 2.0;
Gaku0606 0:d36bfb8300a2 60 }
Gaku0606 0:d36bfb8300a2 61 else if(g_range == _1000DPS){
Gaku0606 0:d36bfb8300a2 62 send = 0x10 | fchoice;
Gaku0606 0:d36bfb8300a2 63 gyro_coef = GYRO_LSB * 4.0;
Gaku0606 0:d36bfb8300a2 64 }
Gaku0606 0:d36bfb8300a2 65 else if(g_range == _2000DPS){
Gaku0606 0:d36bfb8300a2 66 send = 0x18 | fchoice;
Gaku0606 0:d36bfb8300a2 67 gyro_coef = GYRO_LSB * 8.0;
Gaku0606 0:d36bfb8300a2 68 }
Gaku0606 0:d36bfb8300a2 69 writeReg(_addr, GYRO_CONFIG, send);
Gaku0606 0:d36bfb8300a2 70 }
Gaku0606 0:d36bfb8300a2 71 void mpu9250::setAcc(ACC_RANGE a_range){
Gaku0606 0:d36bfb8300a2 72 char send;
Gaku0606 0:d36bfb8300a2 73 if(a_range == _2G){
Gaku0606 0:d36bfb8300a2 74 send = 0x00;
Gaku0606 0:d36bfb8300a2 75 acc_coef = ACC_LSB;
Gaku0606 0:d36bfb8300a2 76 }
Gaku0606 0:d36bfb8300a2 77 else if(a_range == _4G){
Gaku0606 0:d36bfb8300a2 78 send = 0x08;
Gaku0606 0:d36bfb8300a2 79 acc_coef = ACC_LSB * 2.0;
Gaku0606 0:d36bfb8300a2 80 }
Gaku0606 0:d36bfb8300a2 81 else if(a_range == _8G){
Gaku0606 0:d36bfb8300a2 82 send = 0x10;
Gaku0606 0:d36bfb8300a2 83 acc_coef = ACC_LSB * 4.0;
Gaku0606 0:d36bfb8300a2 84 }
Gaku0606 0:d36bfb8300a2 85 else if(a_range == _16G){
Gaku0606 0:d36bfb8300a2 86 send = 0x18;
Gaku0606 0:d36bfb8300a2 87 acc_coef = ACC_LSB * 8.0;
Gaku0606 0:d36bfb8300a2 88 }
Gaku0606 0:d36bfb8300a2 89 writeReg(_addr, ACCEL_CONFIG, send);
Gaku0606 0:d36bfb8300a2 90 }
Gaku0606 0:d36bfb8300a2 91
Gaku0606 0:d36bfb8300a2 92 void mpu9250::init(){
Gaku0606 0:d36bfb8300a2 93
Gaku0606 0:d36bfb8300a2 94 acc_coef = ACC_LSB;
Gaku0606 0:d36bfb8300a2 95 gyro_coef = GYRO_LSB;
Gaku0606 0:d36bfb8300a2 96 mag_coef = MAG_LSB;
Gaku0606 0:d36bfb8300a2 97
Gaku0606 0:d36bfb8300a2 98 writeReg(MAG_ADDR, 0x6B, 0x00);
Gaku0606 0:d36bfb8300a2 99 wait_us(70);
Gaku0606 0:d36bfb8300a2 100 writeReg(_addr, 0x37, 0x02);
Gaku0606 0:d36bfb8300a2 101 wait_us(70);
Gaku0606 0:d36bfb8300a2 102 writeReg(MAG_ADDR, CNTL1, 0x16);
Gaku0606 0:d36bfb8300a2 103 wait_us(70);
Gaku0606 0:d36bfb8300a2 104 setAcc(_4G);
Gaku0606 0:d36bfb8300a2 105 wait_us(70);
Gaku0606 0:d36bfb8300a2 106 setGyro(_500DPS);
Gaku0606 0:d36bfb8300a2 107 }
Gaku0606 0:d36bfb8300a2 108
Gaku0606 1:6a4c2f84180b 109 template<typename T>void mpu9250::getAcc(T *acc){
Gaku0606 0:d36bfb8300a2 110 char data[6];
Gaku0606 0:d36bfb8300a2 111 short sign;
Gaku0606 0:d36bfb8300a2 112 readReg(_addr, ACCEL_XOUT_H, data, 6);
Gaku0606 0:d36bfb8300a2 113 sign = ((short)data[0] << 8) | (short)data[1];
Gaku0606 0:d36bfb8300a2 114 acc[0] = (double)sign * acc_coef - acc_offset[0];
Gaku0606 0:d36bfb8300a2 115 sign = ((short)data[2] << 8) | (short)data[3];
Gaku0606 0:d36bfb8300a2 116 acc[1] = (double)sign * acc_coef - acc_offset[1];
Gaku0606 0:d36bfb8300a2 117 sign = ((short)data[4] << 8) | (short)data[5];
Gaku0606 0:d36bfb8300a2 118 acc[2] = (double)sign * acc_coef - acc_offset[2];
Gaku0606 0:d36bfb8300a2 119 }
Gaku0606 1:6a4c2f84180b 120 template<typename T>void mpu9250::getAcc(T *ax, T *ay, T *az){
Gaku0606 0:d36bfb8300a2 121 double acc[3];
Gaku0606 0:d36bfb8300a2 122 getAcc(acc);
Gaku0606 0:d36bfb8300a2 123 *ax = acc[0];
Gaku0606 0:d36bfb8300a2 124 *ay = acc[1];
Gaku0606 0:d36bfb8300a2 125 *az = acc[2];
Gaku0606 0:d36bfb8300a2 126 }
Gaku0606 1:6a4c2f84180b 127 template<typename T>void mpu9250::getGyro(T *gyro){
Gaku0606 0:d36bfb8300a2 128 char data[6];
Gaku0606 0:d36bfb8300a2 129 short sign;
Gaku0606 0:d36bfb8300a2 130 readReg(_addr, GYRO_XOUT_H, data, 6);
Gaku0606 0:d36bfb8300a2 131 sign = ((short)data[0] << 8) | (short)data[1];
Gaku0606 0:d36bfb8300a2 132 gyro[0] = (double)sign * gyro_coef - gyro_offset[0];
Gaku0606 0:d36bfb8300a2 133 sign = ((short)data[2] << 8) | (short)data[3];
Gaku0606 0:d36bfb8300a2 134 gyro[1] = (double)sign * gyro_coef - gyro_offset[1];
Gaku0606 0:d36bfb8300a2 135 sign = ((short)data[4] << 8) | (short)data[5];
Gaku0606 0:d36bfb8300a2 136 gyro[2] = (double)sign * gyro_coef - gyro_offset[2];
Gaku0606 0:d36bfb8300a2 137 }
Gaku0606 1:6a4c2f84180b 138 template<typename T>void mpu9250::getGyro(T *gx, T *gy, T *gz){
Gaku0606 0:d36bfb8300a2 139 double gyro[3];
Gaku0606 0:d36bfb8300a2 140 getGyro(gyro);
Gaku0606 0:d36bfb8300a2 141 *gx = gyro[0];
Gaku0606 0:d36bfb8300a2 142 *gy = gyro[1];
Gaku0606 0:d36bfb8300a2 143 *gz = gyro[2];
Gaku0606 0:d36bfb8300a2 144 }
Gaku0606 1:6a4c2f84180b 145 template<typename T>void mpu9250::getMag(T *mag){
Gaku0606 0:d36bfb8300a2 146 char data[8];
Gaku0606 0:d36bfb8300a2 147 short sign;
Gaku0606 0:d36bfb8300a2 148 readReg(MAG_ADDR, ST1, data, 8);
Gaku0606 0:d36bfb8300a2 149 sign = ((short)data[2] << 8) | (short)data[1];
Gaku0606 0:d36bfb8300a2 150 mag[1] = (double)sign * mag_coef - mag_offset[1];
Gaku0606 0:d36bfb8300a2 151 sign = ((short)data[4] << 8) | (short)data[3];
Gaku0606 0:d36bfb8300a2 152 mag[0] = (double)sign * mag_coef - mag_offset[0];
Gaku0606 0:d36bfb8300a2 153 sign = ((short)data[6] << 8) | (short)data[5];
Gaku0606 0:d36bfb8300a2 154 mag[2] = (double)sign * -mag_coef - mag_offset[2];
Gaku0606 0:d36bfb8300a2 155 }
Gaku0606 1:6a4c2f84180b 156 template<typename T>void mpu9250::getMag(T *mx, T *my, T *mz){
Gaku0606 0:d36bfb8300a2 157 double mag[3];
Gaku0606 0:d36bfb8300a2 158 getMag(mag);
Gaku0606 0:d36bfb8300a2 159 *mx = mag[0];
Gaku0606 0:d36bfb8300a2 160 *my = mag[1];
Gaku0606 0:d36bfb8300a2 161 *mz = mag[2];
Gaku0606 0:d36bfb8300a2 162 }
Gaku0606 1:6a4c2f84180b 163 template<typename T>void mpu9250::getGyroAcc(T *imu){
Gaku0606 0:d36bfb8300a2 164 char data[14];
Gaku0606 0:d36bfb8300a2 165 short sign;
Gaku0606 0:d36bfb8300a2 166 readReg(_addr, ACCEL_XOUT_H, data, 14);
Gaku0606 0:d36bfb8300a2 167 sign = ((short)data[0] << 8) | (short)data[1];
Gaku0606 0:d36bfb8300a2 168 imu[3] = (double)sign * acc_coef - acc_offset[0];
Gaku0606 0:d36bfb8300a2 169 sign = ((short)data[2] << 8) | (short)data[3];
Gaku0606 0:d36bfb8300a2 170 imu[4] = (double)sign * acc_coef - acc_offset[1];
Gaku0606 0:d36bfb8300a2 171 sign = ((short)data[4] << 8) | (short)data[5];
Gaku0606 0:d36bfb8300a2 172 imu[5] = (double)sign * acc_coef - acc_offset[2];
Gaku0606 0:d36bfb8300a2 173
Gaku0606 0:d36bfb8300a2 174 sign = ((short)data[8] << 8) | (short)data[9];
Gaku0606 0:d36bfb8300a2 175 imu[0] = (double)sign * gyro_coef - gyro_offset[0];
Gaku0606 0:d36bfb8300a2 176 sign = ((short)data[10] << 8) | (short)data[11];
Gaku0606 0:d36bfb8300a2 177 imu[1] = (double)sign * gyro_coef - gyro_offset[1];
Gaku0606 0:d36bfb8300a2 178 sign = ((short)data[12] << 8) | (short)data[13];
Gaku0606 0:d36bfb8300a2 179 imu[2] = (double)sign * gyro_coef - gyro_offset[2];
Gaku0606 0:d36bfb8300a2 180 }
Gaku0606 0:d36bfb8300a2 181
Gaku0606 0:d36bfb8300a2 182 void mpu9250::setAccLPF(A_BAND_WIDTH band){
Gaku0606 0:d36bfb8300a2 183 writeReg(_addr, ACCEL_CONFIG2, band);
Gaku0606 0:d36bfb8300a2 184 wait_us(70);
Gaku0606 1:6a4c2f84180b 185 }
Gaku0606 1:6a4c2f84180b 186
Gaku0606 1:6a4c2f84180b 187 template void mpu9250::getAcc<double>(double *ax, double *ay, double *az);
Gaku0606 1:6a4c2f84180b 188 template void mpu9250::getAcc<float>(float *ax, float *ay, float *az);
Gaku0606 1:6a4c2f84180b 189 template void mpu9250::getAcc<double>(double *acc);
Gaku0606 1:6a4c2f84180b 190 template void mpu9250::getAcc<float>(float *acc);
Gaku0606 1:6a4c2f84180b 191
Gaku0606 1:6a4c2f84180b 192 template void mpu9250::getGyro<double>(double *gx, double *gy, double *gz);
Gaku0606 1:6a4c2f84180b 193 template void mpu9250::getGyro<float>(float *gx, float *gy, float *gz);
Gaku0606 1:6a4c2f84180b 194 template void mpu9250::getGyro<double>(double *gyro);
Gaku0606 1:6a4c2f84180b 195 template void mpu9250::getGyro<float>(float *gyro);
Gaku0606 1:6a4c2f84180b 196
Gaku0606 1:6a4c2f84180b 197 template void mpu9250::getMag<double>(double *mx, double *my, double *mz);
Gaku0606 1:6a4c2f84180b 198 template void mpu9250::getMag<float>(float *mx, float *my, float *mz);
Gaku0606 1:6a4c2f84180b 199 template void mpu9250::getMag<double>(double *mag);
Gaku0606 1:6a4c2f84180b 200 template void mpu9250::getMag<float>(float *mag);
Gaku0606 1:6a4c2f84180b 201
Gaku0606 1:6a4c2f84180b 202 template void mpu9250::getGyroAcc<double>(double *imu);//gx,gy,gz,ax,ay,az
Gaku0606 1:6a4c2f84180b 203 template void mpu9250::getGyroAcc<float>(float *imu);//gx,gy,gz,ax,ay,az
Gaku0606 1:6a4c2f84180b 204