altb_pmic / AHRS

AHRS

Dependencies:   Eigen

Dependents:   IndNav_QK3_T265

BMI088.cpp@27:973e495f4711, 2020-01-14 (annotated)

Committer:
pmic
Date:
Tue Jan 14 14:24:03 2020 +0000
Revision:
27:973e495f4711
Parent:
25:fe14dbcef82d
Child:
28:21dfb161c67c
Change settings for BMI55 & BMI88.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb2 25:fe14dbcef82d 1 #include "BMI088.h"
altb2 25:fe14dbcef82d 2
altb2 25:fe14dbcef82d 3 BMI088::BMI088(I2C& i2c)
altb2 25:fe14dbcef82d 4 {
altb2 25:fe14dbcef82d 5 this->i2c = &i2c;
altb2 25:fe14dbcef82d 6 devAddrAcc = BMI088_ACC_ADDRESS;
altb2 25:fe14dbcef82d 7 devAddrGyro = BMI088_GYRO_ADDRESS;
altb2 25:fe14dbcef82d 8 }
altb2 25:fe14dbcef82d 9
altb2 25:fe14dbcef82d 10 void BMI088::initialize(void)
altb2 25:fe14dbcef82d 11 {
altb2 25:fe14dbcef82d 12 setAccScaleRange(RANGE_3G);
pmic 27:973e495f4711 13 setAccOutputDataRate(ODR_100); // ODR_100 -> 40 Hz bandwith in normal mode
altb2 25:fe14dbcef82d 14 setAccPowerMode(ACC_ACTIVE);
altb2 25:fe14dbcef82d 15
altb2 25:fe14dbcef82d 16 setGyroScaleRange(RANGE_500);
pmic 27:973e495f4711 17 setGyroOutputDataRate(ODR_400_BW_47);
altb2 25:fe14dbcef82d 18 setGyroPowerMode(GYRO_NORMAL);
altb2 25:fe14dbcef82d 19 }
altb2 25:fe14dbcef82d 20
altb2 25:fe14dbcef82d 21 bool BMI088::isConnection(void)
altb2 25:fe14dbcef82d 22 {
altb2 25:fe14dbcef82d 23 return ((getAccID() == 0x1E) && (getGyroID() == 0x0F));
altb2 25:fe14dbcef82d 24 }
altb2 25:fe14dbcef82d 25
altb2 25:fe14dbcef82d 26 void BMI088::resetAcc(void)
altb2 25:fe14dbcef82d 27 {
altb2 25:fe14dbcef82d 28 write8(ACC, BMI088_ACC_SOFT_RESET, 0xB6);
altb2 25:fe14dbcef82d 29 }
altb2 25:fe14dbcef82d 30
altb2 25:fe14dbcef82d 31 void BMI088::resetGyro(void)
altb2 25:fe14dbcef82d 32 {
altb2 25:fe14dbcef82d 33 write8(GYRO, BMI088_GYRO_SOFT_RESET, 0xB6);
altb2 25:fe14dbcef82d 34 }
altb2 25:fe14dbcef82d 35
altb2 25:fe14dbcef82d 36 uint8_t BMI088::getAccID(void)
altb2 25:fe14dbcef82d 37 {
altb2 25:fe14dbcef82d 38 return read8(ACC, BMI088_ACC_CHIP_ID);
altb2 25:fe14dbcef82d 39 }
altb2 25:fe14dbcef82d 40
altb2 25:fe14dbcef82d 41 uint8_t BMI088::getGyroID(void)
altb2 25:fe14dbcef82d 42 {
altb2 25:fe14dbcef82d 43 return read8(GYRO, BMI088_GYRO_CHIP_ID);
altb2 25:fe14dbcef82d 44 }
altb2 25:fe14dbcef82d 45
altb2 25:fe14dbcef82d 46 void BMI088::setAccPowerMode(acc_power_type_t mode)
altb2 25:fe14dbcef82d 47 {
altb2 25:fe14dbcef82d 48 if(mode == ACC_ACTIVE) {
altb2 25:fe14dbcef82d 49 write8(ACC, BMI088_ACC_PWR_CTRl, 0x04);
altb2 25:fe14dbcef82d 50 write8(ACC, BMI088_ACC_PWR_CONF, 0x00);
altb2 25:fe14dbcef82d 51 } else if(mode == ACC_SUSPEND) {
altb2 25:fe14dbcef82d 52 write8(ACC, BMI088_ACC_PWR_CONF, 0x03);
altb2 25:fe14dbcef82d 53 write8(ACC, BMI088_ACC_PWR_CTRl, 0x00);
altb2 25:fe14dbcef82d 54 }
altb2 25:fe14dbcef82d 55 }
altb2 25:fe14dbcef82d 56
altb2 25:fe14dbcef82d 57 void BMI088::setGyroPowerMode(gyro_power_type_t mode)
altb2 25:fe14dbcef82d 58 {
altb2 25:fe14dbcef82d 59 if(mode == GYRO_NORMAL) {
altb2 25:fe14dbcef82d 60 write8(GYRO, BMI088_GYRO_LPM_1, (uint8_t)GYRO_NORMAL);
altb2 25:fe14dbcef82d 61 } else if(mode == GYRO_SUSPEND) {
altb2 25:fe14dbcef82d 62 write8(GYRO, BMI088_GYRO_LPM_1, (uint8_t)GYRO_SUSPEND);
altb2 25:fe14dbcef82d 63 } else if(mode == GYRO_DEEP_SUSPEND) {
altb2 25:fe14dbcef82d 64 write8(GYRO, BMI088_GYRO_LPM_1, (uint8_t)GYRO_DEEP_SUSPEND);
altb2 25:fe14dbcef82d 65 }
altb2 25:fe14dbcef82d 66 }
altb2 25:fe14dbcef82d 67
altb2 25:fe14dbcef82d 68 void BMI088::setAccScaleRange(acc_scale_type_t range)
altb2 25:fe14dbcef82d 69 {
altb2 25:fe14dbcef82d 70 if(range == RANGE_3G) accRange = 3 * 9.81 / 32768.0f;
altb2 25:fe14dbcef82d 71 else if(range == RANGE_6G) accRange = 6 * 9.81 / 32768.0f;
altb2 25:fe14dbcef82d 72 else if(range == RANGE_12G) accRange = 12 * 9.81 / 32768.0f;
altb2 25:fe14dbcef82d 73 else if(range == RANGE_24G) accRange = 24 * 9.81 / 32768.0f;
altb2 25:fe14dbcef82d 74
altb2 25:fe14dbcef82d 75 write8(ACC, BMI088_ACC_RANGE, (uint8_t)range);
altb2 25:fe14dbcef82d 76 }
altb2 25:fe14dbcef82d 77
altb2 25:fe14dbcef82d 78 void BMI088::setAccOutputDataRate(acc_odr_type_t odr)
altb2 25:fe14dbcef82d 79 {
altb2 25:fe14dbcef82d 80 uint8_t data = 0;
altb2 25:fe14dbcef82d 81
altb2 25:fe14dbcef82d 82 data = read8(ACC, BMI088_ACC_CONF);
altb2 25:fe14dbcef82d 83 data = data & 0xf0;
altb2 25:fe14dbcef82d 84 data = data | (uint8_t)odr;
altb2 25:fe14dbcef82d 85
altb2 25:fe14dbcef82d 86 write8(ACC, BMI088_ACC_CONF, data);
altb2 25:fe14dbcef82d 87 }
altb2 25:fe14dbcef82d 88
altb2 25:fe14dbcef82d 89 void BMI088::setGyroScaleRange(gyro_scale_type_t range)
altb2 25:fe14dbcef82d 90 {
altb2 25:fe14dbcef82d 91 if(range == RANGE_2000) gyroRange = 2000 * 0.01745329 / 32768.0f;
altb2 25:fe14dbcef82d 92 else if(range == RANGE_1000) gyroRange = 1000 * 0.01745329f / 32768.0f;
altb2 25:fe14dbcef82d 93 else if(range == RANGE_500) gyroRange = 500 * 0.01745329f / 32768.0f;
altb2 25:fe14dbcef82d 94 else if(range == RANGE_250) gyroRange = 250 * 0.01745329f / 32768.0f;
altb2 25:fe14dbcef82d 95 else if(range == RANGE_125) gyroRange = 125 * 0.01745329f / 32768.0f;
altb2 25:fe14dbcef82d 96
altb2 25:fe14dbcef82d 97 write8(GYRO, BMI088_GYRO_RANGE, (uint8_t)range);
altb2 25:fe14dbcef82d 98 }
altb2 25:fe14dbcef82d 99
altb2 25:fe14dbcef82d 100 void BMI088::setGyroOutputDataRate(gyro_odr_type_t odr)
altb2 25:fe14dbcef82d 101 {
altb2 25:fe14dbcef82d 102 write8(GYRO, BMI088_GYRO_BAND_WIDTH, (uint8_t)odr);
altb2 25:fe14dbcef82d 103 }
altb2 25:fe14dbcef82d 104
altb2 25:fe14dbcef82d 105 void BMI088::getAcceleration(float* x, float* y, float* z)
altb2 25:fe14dbcef82d 106 {
altb2 25:fe14dbcef82d 107 uint8_t buf[6] = {0};
altb2 25:fe14dbcef82d 108 uint16_t ax = 0, ay = 0, az = 0;
altb2 25:fe14dbcef82d 109 float value = 0;
altb2 25:fe14dbcef82d 110
altb2 25:fe14dbcef82d 111 read(ACC, BMI088_ACC_X_LSB, buf, 6);
altb2 25:fe14dbcef82d 112
altb2 25:fe14dbcef82d 113 ax = buf[0] | (buf[1] << 8);
altb2 25:fe14dbcef82d 114 ay = buf[2] | (buf[3] << 8);
altb2 25:fe14dbcef82d 115 az = buf[4] | (buf[5] << 8);
altb2 25:fe14dbcef82d 116
altb2 25:fe14dbcef82d 117 value = (int16_t)ax;
altb2 25:fe14dbcef82d 118 *x = accRange * value ;
altb2 25:fe14dbcef82d 119
altb2 25:fe14dbcef82d 120 value = (int16_t)ay;
altb2 25:fe14dbcef82d 121 *y = accRange * value;
altb2 25:fe14dbcef82d 122
altb2 25:fe14dbcef82d 123 value = (int16_t)az;
altb2 25:fe14dbcef82d 124 *z = accRange * value ;
altb2 25:fe14dbcef82d 125 }
altb2 25:fe14dbcef82d 126
altb2 25:fe14dbcef82d 127 float BMI088::getAccelerationX(void)
altb2 25:fe14dbcef82d 128 {
altb2 25:fe14dbcef82d 129 uint16_t ax = 0;
altb2 25:fe14dbcef82d 130 float value = 0;
altb2 25:fe14dbcef82d 131
altb2 25:fe14dbcef82d 132 ax = read16(ACC, BMI088_ACC_X_LSB);
altb2 25:fe14dbcef82d 133
altb2 25:fe14dbcef82d 134 value = (int16_t)ax;
altb2 25:fe14dbcef82d 135 value = accRange * value ;
altb2 25:fe14dbcef82d 136
altb2 25:fe14dbcef82d 137 return value;
altb2 25:fe14dbcef82d 138 }
altb2 25:fe14dbcef82d 139
altb2 25:fe14dbcef82d 140 float BMI088::getAccelerationY(void)
altb2 25:fe14dbcef82d 141 {
altb2 25:fe14dbcef82d 142 uint16_t ay = 0;
altb2 25:fe14dbcef82d 143 float value = 0;
altb2 25:fe14dbcef82d 144
altb2 25:fe14dbcef82d 145 ay = read16(ACC, BMI088_ACC_Y_LSB);
altb2 25:fe14dbcef82d 146
altb2 25:fe14dbcef82d 147 value = (int16_t)ay;
altb2 25:fe14dbcef82d 148 value = accRange * value ;
altb2 25:fe14dbcef82d 149
altb2 25:fe14dbcef82d 150 return value;
altb2 25:fe14dbcef82d 151 }
altb2 25:fe14dbcef82d 152
altb2 25:fe14dbcef82d 153 float BMI088::getAccelerationZ(void)
altb2 25:fe14dbcef82d 154 {
altb2 25:fe14dbcef82d 155 uint16_t az = 0;
altb2 25:fe14dbcef82d 156 float value = 0;
altb2 25:fe14dbcef82d 157
altb2 25:fe14dbcef82d 158 az = read16(ACC, BMI088_ACC_Z_LSB);
altb2 25:fe14dbcef82d 159
altb2 25:fe14dbcef82d 160 value = (int16_t)az;
altb2 25:fe14dbcef82d 161 value = accRange * value;
altb2 25:fe14dbcef82d 162
altb2 25:fe14dbcef82d 163 return value;
altb2 25:fe14dbcef82d 164 }
altb2 25:fe14dbcef82d 165 void BMI088::readAccel(void){
altb2 25:fe14dbcef82d 166 accX = getAccelerationX();
altb2 25:fe14dbcef82d 167 accY = getAccelerationY();
altb2 25:fe14dbcef82d 168 accZ = getAccelerationZ();
altb2 25:fe14dbcef82d 169 }
altb2 25:fe14dbcef82d 170
altb2 25:fe14dbcef82d 171
altb2 25:fe14dbcef82d 172 void BMI088::getGyroscope(float* x, float* y, float* z)
altb2 25:fe14dbcef82d 173 {
altb2 25:fe14dbcef82d 174 uint8_t buf[6] = {0};
altb2 25:fe14dbcef82d 175 uint16_t gx = 0, gy = 0, gz = 0;
altb2 25:fe14dbcef82d 176 float value = 0;
altb2 25:fe14dbcef82d 177
altb2 25:fe14dbcef82d 178 read(GYRO, BMI088_GYRO_RATE_X_LSB, buf, 6);
altb2 25:fe14dbcef82d 179
altb2 25:fe14dbcef82d 180 gx = buf[0] | (buf[1] << 8);
altb2 25:fe14dbcef82d 181 gy = buf[2] | (buf[3] << 8);
altb2 25:fe14dbcef82d 182 gz = buf[4] | (buf[5] << 8);
altb2 25:fe14dbcef82d 183
altb2 25:fe14dbcef82d 184 value = (int16_t)gx;
altb2 25:fe14dbcef82d 185 *x = gyroRange * value ;
altb2 25:fe14dbcef82d 186
altb2 25:fe14dbcef82d 187 value = (int16_t)gy;
altb2 25:fe14dbcef82d 188 *y = gyroRange * value ;
altb2 25:fe14dbcef82d 189
altb2 25:fe14dbcef82d 190 value = (int16_t)gz;
altb2 25:fe14dbcef82d 191 *z = gyroRange * value ;
altb2 25:fe14dbcef82d 192 }
altb2 25:fe14dbcef82d 193
altb2 25:fe14dbcef82d 194 float BMI088::getGyroscopeX(void)
altb2 25:fe14dbcef82d 195 {
altb2 25:fe14dbcef82d 196 uint16_t gx = 0;
altb2 25:fe14dbcef82d 197 float value = 0;
altb2 25:fe14dbcef82d 198
altb2 25:fe14dbcef82d 199 gx = read16(GYRO, BMI088_GYRO_RATE_X_LSB);
altb2 25:fe14dbcef82d 200
altb2 25:fe14dbcef82d 201 value = (int16_t)gx;
altb2 25:fe14dbcef82d 202 value = gyroRange * value ;
altb2 25:fe14dbcef82d 203
altb2 25:fe14dbcef82d 204 return value;
altb2 25:fe14dbcef82d 205 }
altb2 25:fe14dbcef82d 206
altb2 25:fe14dbcef82d 207 float BMI088::getGyroscopeY(void)
altb2 25:fe14dbcef82d 208 {
altb2 25:fe14dbcef82d 209 uint16_t gy = 0;
altb2 25:fe14dbcef82d 210 float value = 0;
altb2 25:fe14dbcef82d 211
altb2 25:fe14dbcef82d 212 gy = read16(GYRO, BMI088_GYRO_RATE_Y_LSB);
altb2 25:fe14dbcef82d 213
altb2 25:fe14dbcef82d 214 value = (int16_t)gy;
altb2 25:fe14dbcef82d 215 value = gyroRange * value ;
altb2 25:fe14dbcef82d 216
altb2 25:fe14dbcef82d 217 return value;
altb2 25:fe14dbcef82d 218 }
altb2 25:fe14dbcef82d 219
altb2 25:fe14dbcef82d 220 float BMI088::getGyroscopeZ(void)
altb2 25:fe14dbcef82d 221 {
altb2 25:fe14dbcef82d 222 uint16_t gz = 0;
altb2 25:fe14dbcef82d 223 float value = 0;
altb2 25:fe14dbcef82d 224
altb2 25:fe14dbcef82d 225 gz = read16(GYRO, BMI088_GYRO_RATE_Z_LSB);
altb2 25:fe14dbcef82d 226
altb2 25:fe14dbcef82d 227 value = (int16_t)gz;
altb2 25:fe14dbcef82d 228 value = gyroRange * value ;
altb2 25:fe14dbcef82d 229
altb2 25:fe14dbcef82d 230 return value;
altb2 25:fe14dbcef82d 231 }
altb2 25:fe14dbcef82d 232 void BMI088::readGyro(void){
altb2 25:fe14dbcef82d 233 gyroX = getGyroscopeX();
altb2 25:fe14dbcef82d 234 gyroY = getGyroscopeY();
altb2 25:fe14dbcef82d 235 gyroZ = getGyroscopeZ();
altb2 25:fe14dbcef82d 236 }
altb2 25:fe14dbcef82d 237 int16_t BMI088::getTemperature(void)
altb2 25:fe14dbcef82d 238 {
altb2 25:fe14dbcef82d 239 int16_t data = 0;
altb2 25:fe14dbcef82d 240
altb2 25:fe14dbcef82d 241 data = read16Be(ACC, BMI088_ACC_TEMP_MSB);
altb2 25:fe14dbcef82d 242 data = data >> 5;
altb2 25:fe14dbcef82d 243
altb2 25:fe14dbcef82d 244 if(data > 1023) data = data - 2048;
altb2 25:fe14dbcef82d 245
altb2 25:fe14dbcef82d 246 return (int16_t)(data / 8 + 23);
altb2 25:fe14dbcef82d 247 }
altb2 25:fe14dbcef82d 248
altb2 25:fe14dbcef82d 249 void BMI088::write8(device_type_t dev, uint8_t reg, uint8_t val)
altb2 25:fe14dbcef82d 250 {
altb2 25:fe14dbcef82d 251 uint8_t addr = 0;
altb2 25:fe14dbcef82d 252
altb2 25:fe14dbcef82d 253 if(dev) addr = devAddrGyro;
altb2 25:fe14dbcef82d 254 else addr = devAddrAcc;
altb2 25:fe14dbcef82d 255
altb2 25:fe14dbcef82d 256 i2c->start();
altb2 25:fe14dbcef82d 257 if(i2c->write(addr << 1 | 0) != 1) {
altb2 25:fe14dbcef82d 258 return;
altb2 25:fe14dbcef82d 259 }
altb2 25:fe14dbcef82d 260 if(i2c->write(reg) != 1 ) {
altb2 25:fe14dbcef82d 261 return;
altb2 25:fe14dbcef82d 262 }
altb2 25:fe14dbcef82d 263 if(i2c->write(val) != 1 ) {
altb2 25:fe14dbcef82d 264 return;
altb2 25:fe14dbcef82d 265 }
altb2 25:fe14dbcef82d 266 i2c->stop();
altb2 25:fe14dbcef82d 267
altb2 25:fe14dbcef82d 268
altb2 25:fe14dbcef82d 269 }
altb2 25:fe14dbcef82d 270
altb2 25:fe14dbcef82d 271 uint8_t BMI088::read8(device_type_t dev, uint8_t reg)
altb2 25:fe14dbcef82d 272 {
altb2 25:fe14dbcef82d 273 uint8_t addr = 0, data = 0;
altb2 25:fe14dbcef82d 274
altb2 25:fe14dbcef82d 275 if(dev) addr = devAddrGyro;
altb2 25:fe14dbcef82d 276 else addr = devAddrAcc;
altb2 25:fe14dbcef82d 277
altb2 25:fe14dbcef82d 278 i2c->start();
altb2 25:fe14dbcef82d 279 if(i2c->write(addr << 1 | 0) != 1) {
altb2 25:fe14dbcef82d 280 return 0xff;
altb2 25:fe14dbcef82d 281 }
altb2 25:fe14dbcef82d 282
altb2 25:fe14dbcef82d 283 if(i2c->write(reg)!=1) {
altb2 25:fe14dbcef82d 284 return 0xff;
altb2 25:fe14dbcef82d 285 }
altb2 25:fe14dbcef82d 286 //i2c->stop();
altb2 25:fe14dbcef82d 287
altb2 25:fe14dbcef82d 288
altb2 25:fe14dbcef82d 289 i2c->start();
altb2 25:fe14dbcef82d 290 if(i2c->write(addr<<1|1)!=1) {
altb2 25:fe14dbcef82d 291 return 0xff;
altb2 25:fe14dbcef82d 292 }
altb2 25:fe14dbcef82d 293
altb2 25:fe14dbcef82d 294 data = i2c->read(0);
altb2 25:fe14dbcef82d 295 i2c->stop();
altb2 25:fe14dbcef82d 296
altb2 25:fe14dbcef82d 297 return data;
altb2 25:fe14dbcef82d 298 }
altb2 25:fe14dbcef82d 299
altb2 25:fe14dbcef82d 300 uint16_t BMI088::read16(device_type_t dev, uint8_t reg)
altb2 25:fe14dbcef82d 301 {
altb2 25:fe14dbcef82d 302 uint8_t addr = 0;
altb2 25:fe14dbcef82d 303 uint16_t msb = 0, lsb = 0;
altb2 25:fe14dbcef82d 304
altb2 25:fe14dbcef82d 305 if(dev) addr = devAddrGyro;
altb2 25:fe14dbcef82d 306 else addr = devAddrAcc;
altb2 25:fe14dbcef82d 307
altb2 25:fe14dbcef82d 308 i2c->start();
altb2 25:fe14dbcef82d 309 if(i2c->write(addr << 1 | 0) != 1) {
altb2 25:fe14dbcef82d 310 return 0xffff;
altb2 25:fe14dbcef82d 311 }
altb2 25:fe14dbcef82d 312
altb2 25:fe14dbcef82d 313 if(i2c->write(reg) != 1) {
altb2 25:fe14dbcef82d 314 return 0xffff;
altb2 25:fe14dbcef82d 315 }
altb2 25:fe14dbcef82d 316 //i2c->stop();
altb2 25:fe14dbcef82d 317
altb2 25:fe14dbcef82d 318
altb2 25:fe14dbcef82d 319 i2c->start();
altb2 25:fe14dbcef82d 320 if(i2c->write(addr << 1 | 1) != 1) {
altb2 25:fe14dbcef82d 321 return 0xffff;
altb2 25:fe14dbcef82d 322 }
altb2 25:fe14dbcef82d 323
altb2 25:fe14dbcef82d 324 //i2c->start();
altb2 25:fe14dbcef82d 325 lsb = i2c->read(1);
altb2 25:fe14dbcef82d 326 msb = i2c->read(0);
altb2 25:fe14dbcef82d 327 i2c->stop();
altb2 25:fe14dbcef82d 328
altb2 25:fe14dbcef82d 329 return (lsb | (msb << 8));
altb2 25:fe14dbcef82d 330 }
altb2 25:fe14dbcef82d 331
altb2 25:fe14dbcef82d 332 // bei der Temp sind MSB und LSB vertauscht
altb2 25:fe14dbcef82d 333 uint16_t BMI088::read16Be(device_type_t dev, uint8_t reg)
altb2 25:fe14dbcef82d 334 {
altb2 25:fe14dbcef82d 335 uint8_t addr = 0;
altb2 25:fe14dbcef82d 336 uint16_t msb = 0, lsb = 0;
altb2 25:fe14dbcef82d 337
altb2 25:fe14dbcef82d 338 if(dev) addr = devAddrGyro;
altb2 25:fe14dbcef82d 339 else addr = devAddrAcc;
altb2 25:fe14dbcef82d 340
altb2 25:fe14dbcef82d 341 i2c->start();
altb2 25:fe14dbcef82d 342 if(i2c->write(addr << 1 | 0) != 1) {
altb2 25:fe14dbcef82d 343 return 0xffff;
altb2 25:fe14dbcef82d 344 }
altb2 25:fe14dbcef82d 345
altb2 25:fe14dbcef82d 346 if(i2c->write(reg) != 1) {
altb2 25:fe14dbcef82d 347 return 0xffff;
altb2 25:fe14dbcef82d 348 }
altb2 25:fe14dbcef82d 349 //i2c->stop();
altb2 25:fe14dbcef82d 350
altb2 25:fe14dbcef82d 351
altb2 25:fe14dbcef82d 352 i2c->start();
altb2 25:fe14dbcef82d 353 if(i2c->write(addr << 1 | 1) != 1) {
altb2 25:fe14dbcef82d 354 return 0xffff;
altb2 25:fe14dbcef82d 355 }
altb2 25:fe14dbcef82d 356
altb2 25:fe14dbcef82d 357 msb = i2c->read(1);
altb2 25:fe14dbcef82d 358 lsb = i2c->read(0);
altb2 25:fe14dbcef82d 359 i2c->stop();
altb2 25:fe14dbcef82d 360
altb2 25:fe14dbcef82d 361 return (lsb | (msb << 8));
altb2 25:fe14dbcef82d 362 }
altb2 25:fe14dbcef82d 363
altb2 25:fe14dbcef82d 364 uint32_t BMI088::read24(device_type_t dev, uint8_t reg)
altb2 25:fe14dbcef82d 365 {
altb2 25:fe14dbcef82d 366 uint8_t addr = 0;
altb2 25:fe14dbcef82d 367 uint32_t hsb = 0, msb = 0, lsb = 0;
altb2 25:fe14dbcef82d 368
altb2 25:fe14dbcef82d 369 i2c->start();
altb2 25:fe14dbcef82d 370 if(i2c->write(addr<<1|0)!=1) {
altb2 25:fe14dbcef82d 371 return 0xffffff;
altb2 25:fe14dbcef82d 372 }
altb2 25:fe14dbcef82d 373
altb2 25:fe14dbcef82d 374 if(i2c->write(reg)!=1) {
altb2 25:fe14dbcef82d 375 return 0xffffff;
altb2 25:fe14dbcef82d 376 }
altb2 25:fe14dbcef82d 377 i2c->stop();
altb2 25:fe14dbcef82d 378
altb2 25:fe14dbcef82d 379
altb2 25:fe14dbcef82d 380 i2c->start();
altb2 25:fe14dbcef82d 381 if(i2c->write(addr<<1|1)!=1) {
altb2 25:fe14dbcef82d 382 return 0xffffff;
altb2 25:fe14dbcef82d 383 }
altb2 25:fe14dbcef82d 384
altb2 25:fe14dbcef82d 385 lsb = i2c->read(1);
altb2 25:fe14dbcef82d 386 msb = i2c->read(1);
altb2 25:fe14dbcef82d 387 hsb = i2c->read(0);
altb2 25:fe14dbcef82d 388 i2c->stop();
altb2 25:fe14dbcef82d 389
altb2 25:fe14dbcef82d 390 if(dev) addr = devAddrGyro;
altb2 25:fe14dbcef82d 391 else addr = devAddrAcc;
altb2 25:fe14dbcef82d 392
altb2 25:fe14dbcef82d 393 return (lsb | (msb << 8) | (hsb << 16));
altb2 25:fe14dbcef82d 394 }
altb2 25:fe14dbcef82d 395
altb2 25:fe14dbcef82d 396 void BMI088::read(device_type_t dev, uint8_t reg, uint8_t *buf, uint16_t len)
altb2 25:fe14dbcef82d 397 {
altb2 25:fe14dbcef82d 398 uint8_t addr = 0;
altb2 25:fe14dbcef82d 399
altb2 25:fe14dbcef82d 400 if(dev) addr = devAddrGyro;
altb2 25:fe14dbcef82d 401 else addr = devAddrAcc;
altb2 25:fe14dbcef82d 402
altb2 25:fe14dbcef82d 403 i2c->start();
altb2 25:fe14dbcef82d 404 if(i2c->write(addr<<1|0)!=1) {
altb2 25:fe14dbcef82d 405 for(int in(0); in < len; in++) {
altb2 25:fe14dbcef82d 406 buf[in] = 0xff;
altb2 25:fe14dbcef82d 407 }
altb2 25:fe14dbcef82d 408 return;
altb2 25:fe14dbcef82d 409 }
altb2 25:fe14dbcef82d 410
altb2 25:fe14dbcef82d 411 if(i2c->write(reg)!=1) {/*
altb2 25:fe14dbcef82d 412 for(int in(0); in < len; in++) {
altb2 25:fe14dbcef82d 413 buf[in] = 0xff;
altb2 25:fe14dbcef82d 414 }
altb2 25:fe14dbcef82d 415 return;*/
altb2 25:fe14dbcef82d 416 }
altb2 25:fe14dbcef82d 417 i2c->stop();
altb2 25:fe14dbcef82d 418
altb2 25:fe14dbcef82d 419
altb2 25:fe14dbcef82d 420 i2c->start();
altb2 25:fe14dbcef82d 421 if(i2c->write(addr<<1|1)!=1) {
altb2 25:fe14dbcef82d 422 for(int in(0); in < len; in++) {
altb2 25:fe14dbcef82d 423 buf[in] = 0xff;
altb2 25:fe14dbcef82d 424 }
altb2 25:fe14dbcef82d 425 return;
altb2 25:fe14dbcef82d 426 }
altb2 25:fe14dbcef82d 427
altb2 25:fe14dbcef82d 428 for(int in = 0; in < len; in++) {
altb2 25:fe14dbcef82d 429 buf[in] = i2c->read(1);
altb2 25:fe14dbcef82d 430 }
altb2 25:fe14dbcef82d 431 i2c->stop();
altb2 25:fe14dbcef82d 432 return;
altb2 25:fe14dbcef82d 433
altb2 25:fe14dbcef82d 434 }
altb2 25:fe14dbcef82d 435
altb2 25:fe14dbcef82d 436 //BMI088