altb_pmic / AHRS

AHRS

Dependencies:   Eigen

Dependents:   IndNav_QK3_T265

BMI088.cpp@30:9b0cd3caf0ec, 2020-01-27 (annotated)

Committer:
pmic
Date:
Mon Jan 27 10:54:13 2020 +0000
Revision:
30:9b0cd3caf0ec
Parent:
29:cd963a6d31c5 
Correct magnetometer.

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