mpu9250のライブラリ、I2Cを利用。開発段階のため微妙
mpu9250_i2c.cpp@1:6a4c2f84180b, 2017-01-28 (annotated)
- Committer:
- Gaku0606
- Date:
- Sat Jan 28 19:52:32 2017 +0000
- Revision:
- 1:6a4c2f84180b
- Parent:
- 0:d36bfb8300a2
??????????
Who changed what in which revision?
User | Revision | Line number | New 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 |