mpu9250のライブラリ、I2Cを利用。
Dependents: Hybrid_AttitudeEstimation Hybrid_main_FirstEdtion rocket_logger_sinkan2018_v1 HYBRYD2018_IZU_ROCKET ... more
Fork of mpu9250_i2c by
mpu9250_i2c.cpp@16:bea48f4ac712, 2017-12-06 (annotated)
- Committer:
- Gaku0606
- Date:
- Wed Dec 06 13:11:39 2017 +0000
- Revision:
- 16:bea48f4ac712
- Parent:
- 15:d8b263a8138e
setOffset?????
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 | 15:d8b263a8138e | 4 | /* |
Gaku0606 | 15:d8b263a8138e | 5 | char mpu9250::_addr = MPU9250_SLAVE_ADDR_HIGH; |
Gaku0606 | 0:d36bfb8300a2 | 6 | double mpu9250::acc_coef = ACC_LSB; |
Gaku0606 | 0:d36bfb8300a2 | 7 | double mpu9250::gyro_coef = GYRO_LSB; |
Gaku0606 | 15:d8b263a8138e | 8 | double mpu9250::mag_coef = MPU9250_MAG_LSB; |
Gaku0606 | 0:d36bfb8300a2 | 9 | double mpu9250::acc_offset[3] = {0,0,0}; |
Gaku0606 | 0:d36bfb8300a2 | 10 | double mpu9250::gyro_offset[3] = {0,0,0}; |
Gaku0606 | 15:d8b263a8138e | 11 | double mpu9250::mag_offset[3] = {0,0,0};*/ |
Gaku0606 | 0:d36bfb8300a2 | 12 | |
Gaku0606 | 0:d36bfb8300a2 | 13 | mpu9250::mpu9250(I2C &_i2c, AD0 celect){ |
Gaku0606 | 0:d36bfb8300a2 | 14 | |
Gaku0606 | 0:d36bfb8300a2 | 15 | _nine = &_i2c; |
Gaku0606 | 15:d8b263a8138e | 16 | if(celect == AD0_HIGH) _addr = MPU9250_SLAVE_ADDR_HIGH; |
Gaku0606 | 15:d8b263a8138e | 17 | else _addr = MPU9250_SLAVE_ADDR_LOW; |
Gaku0606 | 0:d36bfb8300a2 | 18 | |
Gaku0606 | 0:d36bfb8300a2 | 19 | _nine->frequency(400000);//400kHz |
Gaku0606 | 0:d36bfb8300a2 | 20 | init(); |
Gaku0606 | 0:d36bfb8300a2 | 21 | } |
Gaku0606 | 0:d36bfb8300a2 | 22 | void mpu9250::frequency(int Hz){ |
Gaku0606 | 0:d36bfb8300a2 | 23 | _nine->frequency(Hz); |
Gaku0606 | 0:d36bfb8300a2 | 24 | } |
Gaku0606 | 15:d8b263a8138e | 25 | bool mpu9250::sensorTest(){ |
Gaku0606 | 1:6a4c2f84180b | 26 | if(readReg(_addr, WHO_AM_I_MPU9250) == 0x71){ |
Gaku0606 | 0:d36bfb8300a2 | 27 | return true; |
Gaku0606 | 0:d36bfb8300a2 | 28 | } |
Gaku0606 | 0:d36bfb8300a2 | 29 | else return false; |
Gaku0606 | 0:d36bfb8300a2 | 30 | } |
Gaku0606 | 0:d36bfb8300a2 | 31 | |
Gaku0606 | 15:d8b263a8138e | 32 | bool mpu9250::mag_sensorTest(){ |
Gaku0606 | 15:d8b263a8138e | 33 | if(readReg(MPU9250_MAG_ADDR, MPU9250_WIA) == 0x48){ |
Gaku0606 | 0:d36bfb8300a2 | 34 | return true; |
Gaku0606 | 0:d36bfb8300a2 | 35 | } |
Gaku0606 | 0:d36bfb8300a2 | 36 | else return false; |
Gaku0606 | 0:d36bfb8300a2 | 37 | } |
Gaku0606 | 0:d36bfb8300a2 | 38 | |
Gaku0606 | 15:d8b263a8138e | 39 | void mpu9250::setOffset(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz){ |
Gaku0606 | 0:d36bfb8300a2 | 40 | gyro_offset[0] = gx; |
Gaku0606 | 0:d36bfb8300a2 | 41 | gyro_offset[1] = gy; |
Gaku0606 | 0:d36bfb8300a2 | 42 | gyro_offset[2] = gz; |
Gaku0606 | 0:d36bfb8300a2 | 43 | acc_offset[0] = ax; |
Gaku0606 | 0:d36bfb8300a2 | 44 | acc_offset[1] = ay; |
Gaku0606 | 0:d36bfb8300a2 | 45 | acc_offset[2] = az; |
Gaku0606 | 0:d36bfb8300a2 | 46 | mag_offset[0] = mx; |
Gaku0606 | 0:d36bfb8300a2 | 47 | mag_offset[1] = my; |
Gaku0606 | 0:d36bfb8300a2 | 48 | mag_offset[2] = mz; |
Gaku0606 | 0:d36bfb8300a2 | 49 | } |
Gaku0606 | 0:d36bfb8300a2 | 50 | |
Gaku0606 | 0:d36bfb8300a2 | 51 | void mpu9250::setGyro(GYRO_RANGE g_range){ |
Gaku0606 | 0:d36bfb8300a2 | 52 | char fchoice = readReg(_addr, GYRO_CONFIG) & 0x03; |
Gaku0606 | 0:d36bfb8300a2 | 53 | char send; |
Gaku0606 | 0:d36bfb8300a2 | 54 | if(g_range == _250DPS){ |
Gaku0606 | 0:d36bfb8300a2 | 55 | send = 0x00 | fchoice; |
Gaku0606 | 0:d36bfb8300a2 | 56 | gyro_coef = GYRO_LSB; |
Gaku0606 | 0:d36bfb8300a2 | 57 | } |
Gaku0606 | 0:d36bfb8300a2 | 58 | else if(g_range == _500DPS){ |
Gaku0606 | 0:d36bfb8300a2 | 59 | send = 0x08 | fchoice; |
Gaku0606 | 0:d36bfb8300a2 | 60 | gyro_coef = GYRO_LSB * 2.0; |
Gaku0606 | 0:d36bfb8300a2 | 61 | } |
Gaku0606 | 0:d36bfb8300a2 | 62 | else if(g_range == _1000DPS){ |
Gaku0606 | 0:d36bfb8300a2 | 63 | send = 0x10 | fchoice; |
Gaku0606 | 0:d36bfb8300a2 | 64 | gyro_coef = GYRO_LSB * 4.0; |
Gaku0606 | 0:d36bfb8300a2 | 65 | } |
Gaku0606 | 0:d36bfb8300a2 | 66 | else if(g_range == _2000DPS){ |
Gaku0606 | 0:d36bfb8300a2 | 67 | send = 0x18 | fchoice; |
Gaku0606 | 0:d36bfb8300a2 | 68 | gyro_coef = GYRO_LSB * 8.0; |
Gaku0606 | 0:d36bfb8300a2 | 69 | } |
Gaku0606 | 0:d36bfb8300a2 | 70 | writeReg(_addr, GYRO_CONFIG, send); |
Gaku0606 | 0:d36bfb8300a2 | 71 | } |
Gaku0606 | 0:d36bfb8300a2 | 72 | void mpu9250::setAcc(ACC_RANGE a_range){ |
Gaku0606 | 0:d36bfb8300a2 | 73 | char send; |
Gaku0606 | 0:d36bfb8300a2 | 74 | if(a_range == _2G){ |
Gaku0606 | 0:d36bfb8300a2 | 75 | send = 0x00; |
Gaku0606 | 0:d36bfb8300a2 | 76 | acc_coef = ACC_LSB; |
Gaku0606 | 0:d36bfb8300a2 | 77 | } |
Gaku0606 | 0:d36bfb8300a2 | 78 | else if(a_range == _4G){ |
Gaku0606 | 0:d36bfb8300a2 | 79 | send = 0x08; |
Gaku0606 | 0:d36bfb8300a2 | 80 | acc_coef = ACC_LSB * 2.0; |
Gaku0606 | 0:d36bfb8300a2 | 81 | } |
Gaku0606 | 0:d36bfb8300a2 | 82 | else if(a_range == _8G){ |
Gaku0606 | 0:d36bfb8300a2 | 83 | send = 0x10; |
Gaku0606 | 0:d36bfb8300a2 | 84 | acc_coef = ACC_LSB * 4.0; |
Gaku0606 | 0:d36bfb8300a2 | 85 | } |
Gaku0606 | 0:d36bfb8300a2 | 86 | else if(a_range == _16G){ |
Gaku0606 | 0:d36bfb8300a2 | 87 | send = 0x18; |
Gaku0606 | 0:d36bfb8300a2 | 88 | acc_coef = ACC_LSB * 8.0; |
Gaku0606 | 0:d36bfb8300a2 | 89 | } |
Gaku0606 | 0:d36bfb8300a2 | 90 | writeReg(_addr, ACCEL_CONFIG, send); |
Gaku0606 | 0:d36bfb8300a2 | 91 | } |
Gaku0606 | 0:d36bfb8300a2 | 92 | |
Gaku0606 | 0:d36bfb8300a2 | 93 | void mpu9250::init(){ |
Gaku0606 | 0:d36bfb8300a2 | 94 | |
Gaku0606 | 0:d36bfb8300a2 | 95 | acc_coef = ACC_LSB; |
Gaku0606 | 0:d36bfb8300a2 | 96 | gyro_coef = GYRO_LSB; |
Gaku0606 | 15:d8b263a8138e | 97 | mag_coef = MPU9250_MAG_LSB; |
Gaku0606 | 0:d36bfb8300a2 | 98 | |
Gaku0606 | 15:d8b263a8138e | 99 | writeReg(MPU9250_MAG_ADDR, 0x6B, 0x00); |
Gaku0606 | 0:d36bfb8300a2 | 100 | wait_us(70); |
Gaku0606 | 0:d36bfb8300a2 | 101 | writeReg(_addr, 0x37, 0x02); |
Gaku0606 | 0:d36bfb8300a2 | 102 | wait_us(70); |
Gaku0606 | 15:d8b263a8138e | 103 | writeReg(MPU9250_MAG_ADDR, MPU9250_CNTL1, 0x16); |
Gaku0606 | 0:d36bfb8300a2 | 104 | wait_us(70); |
Gaku0606 | 0:d36bfb8300a2 | 105 | setAcc(_4G); |
Gaku0606 | 0:d36bfb8300a2 | 106 | wait_us(70); |
Gaku0606 | 0:d36bfb8300a2 | 107 | setGyro(_500DPS); |
Gaku0606 | 0:d36bfb8300a2 | 108 | } |
Gaku0606 | 0:d36bfb8300a2 | 109 | |
Gaku0606 | 1:6a4c2f84180b | 110 | template<typename T>void mpu9250::getAcc(T *acc){ |
Gaku0606 | 0:d36bfb8300a2 | 111 | char data[6]; |
Gaku0606 | 0:d36bfb8300a2 | 112 | short sign; |
Gaku0606 | 0:d36bfb8300a2 | 113 | readReg(_addr, ACCEL_XOUT_H, data, 6); |
Gaku0606 | 0:d36bfb8300a2 | 114 | sign = ((short)data[0] << 8) | (short)data[1]; |
Gaku0606 | 15:d8b263a8138e | 115 | acc[0] = (float)sign * acc_coef - acc_offset[0]; |
Gaku0606 | 0:d36bfb8300a2 | 116 | sign = ((short)data[2] << 8) | (short)data[3]; |
Gaku0606 | 15:d8b263a8138e | 117 | acc[1] = (float)sign * acc_coef - acc_offset[1]; |
Gaku0606 | 0:d36bfb8300a2 | 118 | sign = ((short)data[4] << 8) | (short)data[5]; |
Gaku0606 | 15:d8b263a8138e | 119 | acc[2] = (float)sign * acc_coef - acc_offset[2]; |
Gaku0606 | 0:d36bfb8300a2 | 120 | } |
Gaku0606 | 15:d8b263a8138e | 121 | |
Gaku0606 | 1:6a4c2f84180b | 122 | template<typename T>void mpu9250::getAcc(T *ax, T *ay, T *az){ |
Gaku0606 | 15:d8b263a8138e | 123 | float acc[3]; |
Gaku0606 | 0:d36bfb8300a2 | 124 | getAcc(acc); |
Gaku0606 | 0:d36bfb8300a2 | 125 | *ax = acc[0]; |
Gaku0606 | 0:d36bfb8300a2 | 126 | *ay = acc[1]; |
Gaku0606 | 0:d36bfb8300a2 | 127 | *az = acc[2]; |
Gaku0606 | 0:d36bfb8300a2 | 128 | } |
Gaku0606 | 15:d8b263a8138e | 129 | |
Gaku0606 | 1:6a4c2f84180b | 130 | template<typename T>void mpu9250::getGyro(T *gyro){ |
Gaku0606 | 0:d36bfb8300a2 | 131 | char data[6]; |
Gaku0606 | 0:d36bfb8300a2 | 132 | short sign; |
Gaku0606 | 0:d36bfb8300a2 | 133 | readReg(_addr, GYRO_XOUT_H, data, 6); |
Gaku0606 | 0:d36bfb8300a2 | 134 | sign = ((short)data[0] << 8) | (short)data[1]; |
Gaku0606 | 15:d8b263a8138e | 135 | gyro[0] = (float)sign * gyro_coef - gyro_offset[0]; |
Gaku0606 | 0:d36bfb8300a2 | 136 | sign = ((short)data[2] << 8) | (short)data[3]; |
Gaku0606 | 15:d8b263a8138e | 137 | gyro[1] = (float)sign * gyro_coef - gyro_offset[1]; |
Gaku0606 | 0:d36bfb8300a2 | 138 | sign = ((short)data[4] << 8) | (short)data[5]; |
Gaku0606 | 15:d8b263a8138e | 139 | gyro[2] = (float)sign * gyro_coef - gyro_offset[2]; |
Gaku0606 | 0:d36bfb8300a2 | 140 | } |
Gaku0606 | 15:d8b263a8138e | 141 | |
Gaku0606 | 1:6a4c2f84180b | 142 | template<typename T>void mpu9250::getGyro(T *gx, T *gy, T *gz){ |
Gaku0606 | 15:d8b263a8138e | 143 | float gyro[3]; |
Gaku0606 | 0:d36bfb8300a2 | 144 | getGyro(gyro); |
Gaku0606 | 0:d36bfb8300a2 | 145 | *gx = gyro[0]; |
Gaku0606 | 0:d36bfb8300a2 | 146 | *gy = gyro[1]; |
Gaku0606 | 0:d36bfb8300a2 | 147 | *gz = gyro[2]; |
Gaku0606 | 0:d36bfb8300a2 | 148 | } |
Gaku0606 | 15:d8b263a8138e | 149 | |
Gaku0606 | 1:6a4c2f84180b | 150 | template<typename T>void mpu9250::getMag(T *mag){ |
Gaku0606 | 0:d36bfb8300a2 | 151 | char data[8]; |
Gaku0606 | 0:d36bfb8300a2 | 152 | short sign; |
Gaku0606 | 15:d8b263a8138e | 153 | readReg(MPU9250_MAG_ADDR, MPU9250_ST1, data, 8); |
Gaku0606 | 0:d36bfb8300a2 | 154 | sign = ((short)data[2] << 8) | (short)data[1]; |
Gaku0606 | 15:d8b263a8138e | 155 | mag[1] = (float)sign * mag_coef - mag_offset[1]; |
Gaku0606 | 0:d36bfb8300a2 | 156 | sign = ((short)data[4] << 8) | (short)data[3]; |
Gaku0606 | 15:d8b263a8138e | 157 | mag[0] = (float)sign * mag_coef - mag_offset[0]; |
Gaku0606 | 0:d36bfb8300a2 | 158 | sign = ((short)data[6] << 8) | (short)data[5]; |
Gaku0606 | 15:d8b263a8138e | 159 | mag[2] = (float)sign * -mag_coef - mag_offset[2]; |
Gaku0606 | 0:d36bfb8300a2 | 160 | } |
Gaku0606 | 15:d8b263a8138e | 161 | |
Gaku0606 | 1:6a4c2f84180b | 162 | template<typename T>void mpu9250::getMag(T *mx, T *my, T *mz){ |
Gaku0606 | 15:d8b263a8138e | 163 | float mag[3]; |
Gaku0606 | 0:d36bfb8300a2 | 164 | getMag(mag); |
Gaku0606 | 0:d36bfb8300a2 | 165 | *mx = mag[0]; |
Gaku0606 | 0:d36bfb8300a2 | 166 | *my = mag[1]; |
Gaku0606 | 0:d36bfb8300a2 | 167 | *mz = mag[2]; |
Gaku0606 | 0:d36bfb8300a2 | 168 | } |
Gaku0606 | 15:d8b263a8138e | 169 | |
Gaku0606 | 1:6a4c2f84180b | 170 | template<typename T>void mpu9250::getGyroAcc(T *imu){ |
Gaku0606 | 0:d36bfb8300a2 | 171 | char data[14]; |
Gaku0606 | 0:d36bfb8300a2 | 172 | short sign; |
Gaku0606 | 0:d36bfb8300a2 | 173 | readReg(_addr, ACCEL_XOUT_H, data, 14); |
Gaku0606 | 0:d36bfb8300a2 | 174 | sign = ((short)data[0] << 8) | (short)data[1]; |
Gaku0606 | 15:d8b263a8138e | 175 | imu[3] = (float)sign * acc_coef - acc_offset[0]; |
Gaku0606 | 0:d36bfb8300a2 | 176 | sign = ((short)data[2] << 8) | (short)data[3]; |
Gaku0606 | 15:d8b263a8138e | 177 | imu[4] = (float)sign * acc_coef - acc_offset[1]; |
Gaku0606 | 0:d36bfb8300a2 | 178 | sign = ((short)data[4] << 8) | (short)data[5]; |
Gaku0606 | 15:d8b263a8138e | 179 | imu[5] = (float)sign * acc_coef - acc_offset[2]; |
Gaku0606 | 15:d8b263a8138e | 180 | |
Gaku0606 | 15:d8b263a8138e | 181 | sign = ((short)data[8] << 8) | (short)data[9]; |
Gaku0606 | 15:d8b263a8138e | 182 | imu[0] = (float)sign * gyro_coef - gyro_offset[0]; |
Gaku0606 | 15:d8b263a8138e | 183 | sign = ((short)data[10] << 8) | (short)data[11]; |
Gaku0606 | 15:d8b263a8138e | 184 | imu[1] = (float)sign * gyro_coef - gyro_offset[1]; |
Gaku0606 | 15:d8b263a8138e | 185 | sign = ((short)data[12] << 8) | (short)data[13]; |
Gaku0606 | 15:d8b263a8138e | 186 | imu[2] = (float)sign * gyro_coef - gyro_offset[2]; |
Gaku0606 | 15:d8b263a8138e | 187 | } |
Gaku0606 | 15:d8b263a8138e | 188 | |
Gaku0606 | 15:d8b263a8138e | 189 | template<typename T>void mpu9250::getAll(T *imu, T *mag){ |
Gaku0606 | 15:d8b263a8138e | 190 | |
Gaku0606 | 15:d8b263a8138e | 191 | char data[14]; |
Gaku0606 | 15:d8b263a8138e | 192 | short sign; |
Gaku0606 | 15:d8b263a8138e | 193 | readReg(_addr, ACCEL_XOUT_H, data, 14); |
Gaku0606 | 15:d8b263a8138e | 194 | sign = ((short)data[0] << 8) | (short)data[1]; |
Gaku0606 | 15:d8b263a8138e | 195 | imu[3] = (float)sign * acc_coef - acc_offset[0]; |
Gaku0606 | 15:d8b263a8138e | 196 | sign = ((short)data[2] << 8) | (short)data[3]; |
Gaku0606 | 15:d8b263a8138e | 197 | imu[4] = (float)sign * acc_coef - acc_offset[1]; |
Gaku0606 | 15:d8b263a8138e | 198 | sign = ((short)data[4] << 8) | (short)data[5]; |
Gaku0606 | 15:d8b263a8138e | 199 | imu[5] = (float)sign * acc_coef - acc_offset[2]; |
Gaku0606 | 0:d36bfb8300a2 | 200 | |
Gaku0606 | 0:d36bfb8300a2 | 201 | sign = ((short)data[8] << 8) | (short)data[9]; |
Gaku0606 | 15:d8b263a8138e | 202 | imu[0] = (float)sign * gyro_coef - gyro_offset[0]; |
Gaku0606 | 0:d36bfb8300a2 | 203 | sign = ((short)data[10] << 8) | (short)data[11]; |
Gaku0606 | 15:d8b263a8138e | 204 | imu[1] = (float)sign * gyro_coef - gyro_offset[1]; |
Gaku0606 | 0:d36bfb8300a2 | 205 | sign = ((short)data[12] << 8) | (short)data[13]; |
Gaku0606 | 15:d8b263a8138e | 206 | imu[2] = (float)sign * gyro_coef - gyro_offset[2]; |
Gaku0606 | 15:d8b263a8138e | 207 | |
Gaku0606 | 15:d8b263a8138e | 208 | readReg(MPU9250_MAG_ADDR, MPU9250_ST1, data, 8); |
Gaku0606 | 15:d8b263a8138e | 209 | sign = ((short)data[2] << 8) | (short)data[1]; |
Gaku0606 | 15:d8b263a8138e | 210 | mag[1] = (float)sign * mag_coef - mag_offset[1]; |
Gaku0606 | 15:d8b263a8138e | 211 | sign = ((short)data[4] << 8) | (short)data[3]; |
Gaku0606 | 15:d8b263a8138e | 212 | mag[0] = (float)sign * mag_coef - mag_offset[0]; |
Gaku0606 | 15:d8b263a8138e | 213 | sign = ((short)data[6] << 8) | (short)data[5]; |
Gaku0606 | 15:d8b263a8138e | 214 | mag[2] = (float)sign * -mag_coef - mag_offset[2]; |
Gaku0606 | 15:d8b263a8138e | 215 | |
Gaku0606 | 0:d36bfb8300a2 | 216 | } |
Gaku0606 | 15:d8b263a8138e | 217 | |
Gaku0606 | 15:d8b263a8138e | 218 | template<typename T>void mpu9250::getAll(T *all){ |
Gaku0606 | 15:d8b263a8138e | 219 | |
Gaku0606 | 15:d8b263a8138e | 220 | char data[14]; |
Gaku0606 | 15:d8b263a8138e | 221 | short sign; |
Gaku0606 | 15:d8b263a8138e | 222 | readReg(_addr, ACCEL_XOUT_H, data, 14); |
Gaku0606 | 15:d8b263a8138e | 223 | sign = ((short)data[0] << 8) | (short)data[1]; |
Gaku0606 | 15:d8b263a8138e | 224 | all[3] = (float)sign * acc_coef - acc_offset[0]; |
Gaku0606 | 15:d8b263a8138e | 225 | sign = ((short)data[2] << 8) | (short)data[3]; |
Gaku0606 | 15:d8b263a8138e | 226 | all[4] = (float)sign * acc_coef - acc_offset[1]; |
Gaku0606 | 15:d8b263a8138e | 227 | sign = ((short)data[4] << 8) | (short)data[5]; |
Gaku0606 | 15:d8b263a8138e | 228 | all[5] = (float)sign * acc_coef - acc_offset[2]; |
Gaku0606 | 15:d8b263a8138e | 229 | |
Gaku0606 | 15:d8b263a8138e | 230 | sign = ((short)data[8] << 8) | (short)data[9]; |
Gaku0606 | 15:d8b263a8138e | 231 | all[0] = (float)sign * gyro_coef - gyro_offset[0]; |
Gaku0606 | 15:d8b263a8138e | 232 | sign = ((short)data[10] << 8) | (short)data[11]; |
Gaku0606 | 15:d8b263a8138e | 233 | all[1] = (float)sign * gyro_coef - gyro_offset[1]; |
Gaku0606 | 15:d8b263a8138e | 234 | sign = ((short)data[12] << 8) | (short)data[13]; |
Gaku0606 | 15:d8b263a8138e | 235 | all[2] = (float)sign * gyro_coef - gyro_offset[2]; |
Gaku0606 | 15:d8b263a8138e | 236 | |
Gaku0606 | 15:d8b263a8138e | 237 | readReg(MPU9250_MAG_ADDR, MPU9250_ST1, data, 8); |
Gaku0606 | 15:d8b263a8138e | 238 | sign = ((short)data[2] << 8) | (short)data[1]; |
Gaku0606 | 15:d8b263a8138e | 239 | all[7] = (float)sign * mag_coef - mag_offset[1]; |
Gaku0606 | 15:d8b263a8138e | 240 | sign = ((short)data[4] << 8) | (short)data[3]; |
Gaku0606 | 15:d8b263a8138e | 241 | all[6] = (float)sign * mag_coef - mag_offset[0]; |
Gaku0606 | 15:d8b263a8138e | 242 | sign = ((short)data[6] << 8) | (short)data[5]; |
Gaku0606 | 15:d8b263a8138e | 243 | all[8] = (float)sign * -mag_coef - mag_offset[2]; |
Gaku0606 | 15:d8b263a8138e | 244 | return; |
Gaku0606 | 15:d8b263a8138e | 245 | } |
Gaku0606 | 15:d8b263a8138e | 246 | |
Gaku0606 | 0:d36bfb8300a2 | 247 | |
Gaku0606 | 0:d36bfb8300a2 | 248 | void mpu9250::setAccLPF(A_BAND_WIDTH band){ |
Gaku0606 | 0:d36bfb8300a2 | 249 | writeReg(_addr, ACCEL_CONFIG2, band); |
Gaku0606 | 0:d36bfb8300a2 | 250 | wait_us(70); |
Gaku0606 | 1:6a4c2f84180b | 251 | } |
Gaku0606 | 1:6a4c2f84180b | 252 | |
Gaku0606 | 1:6a4c2f84180b | 253 | template void mpu9250::getAcc<double>(double *ax, double *ay, double *az); |
Gaku0606 | 1:6a4c2f84180b | 254 | template void mpu9250::getAcc<float>(float *ax, float *ay, float *az); |
Gaku0606 | 1:6a4c2f84180b | 255 | template void mpu9250::getAcc<double>(double *acc); |
Gaku0606 | 1:6a4c2f84180b | 256 | template void mpu9250::getAcc<float>(float *acc); |
Gaku0606 | 1:6a4c2f84180b | 257 | |
Gaku0606 | 1:6a4c2f84180b | 258 | template void mpu9250::getGyro<double>(double *gx, double *gy, double *gz); |
Gaku0606 | 1:6a4c2f84180b | 259 | template void mpu9250::getGyro<float>(float *gx, float *gy, float *gz); |
Gaku0606 | 1:6a4c2f84180b | 260 | template void mpu9250::getGyro<double>(double *gyro); |
Gaku0606 | 1:6a4c2f84180b | 261 | template void mpu9250::getGyro<float>(float *gyro); |
Gaku0606 | 1:6a4c2f84180b | 262 | |
Gaku0606 | 1:6a4c2f84180b | 263 | template void mpu9250::getMag<double>(double *mx, double *my, double *mz); |
Gaku0606 | 1:6a4c2f84180b | 264 | template void mpu9250::getMag<float>(float *mx, float *my, float *mz); |
Gaku0606 | 1:6a4c2f84180b | 265 | template void mpu9250::getMag<double>(double *mag); |
Gaku0606 | 1:6a4c2f84180b | 266 | template void mpu9250::getMag<float>(float *mag); |
Gaku0606 | 1:6a4c2f84180b | 267 | |
Gaku0606 | 1:6a4c2f84180b | 268 | template void mpu9250::getGyroAcc<double>(double *imu);//gx,gy,gz,ax,ay,az |
Gaku0606 | 1:6a4c2f84180b | 269 | template void mpu9250::getGyroAcc<float>(float *imu);//gx,gy,gz,ax,ay,az |
Gaku0606 | 15:d8b263a8138e | 270 | |
Gaku0606 | 15:d8b263a8138e | 271 | template void mpu9250::getAll<double>(double *all); |
Gaku0606 | 15:d8b263a8138e | 272 | template void mpu9250::getAll<float>(float *all); |
Gaku0606 | 15:d8b263a8138e | 273 | |
Gaku0606 | 15:d8b263a8138e | 274 | template void mpu9250::getAll<float>(float *imu, float *mag); |
Gaku0606 | 15:d8b263a8138e | 275 | template void mpu9250::getAll<double>(double *imu, double *mag); |