AHRS
Dependencies: Eigen
BMI088.cpp@25:fe14dbcef82d, 2020-01-06 (annotated)
- Committer:
- altb2
- Date:
- Mon Jan 06 12:49:38 2020 +0000
- Revision:
- 25:fe14dbcef82d
- Child:
- 27:973e495f4711
implement BMX055
Who changed what in which revision?
User | Revision | Line number | New 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); |
altb2 | 25:fe14dbcef82d | 13 | setAccOutputDataRate(ODR_100); |
altb2 | 25:fe14dbcef82d | 14 | setAccPowerMode(ACC_ACTIVE); |
altb2 | 25:fe14dbcef82d | 15 | |
altb2 | 25:fe14dbcef82d | 16 | setGyroScaleRange(RANGE_500); |
altb2 | 25:fe14dbcef82d | 17 | setGyroOutputDataRate(ODR_200_BW_23); |
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 |