AHRS
Dependencies: Eigen
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?
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 | 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 |