altb_pmic / AHRS

AHRS

Dependencies:   Eigen

Dependents:   IndNav_QK3_T265

BMI088.cpp@28:21dfb161c67c, 2020-01-20 (annotated)

Committer:
altb2
Date:
Mon Jan 20 12:41:13 2020 +0000
Revision:
28:21dfb161c67c
Parent:
27:973e495f4711
Child:
29:cd963a6d31c5
read also BMI088, only for datalogging

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