AHRS
Dependencies: Eigen
BMI088.cpp@29:cd963a6d31c5, 2020-01-27 (annotated)
- Committer:
- pmic
- Date:
- Mon Jan 27 09:23:58 2020 +0000
- Revision:
- 29:cd963a6d31c5
- Parent:
- 28:21dfb161c67c
Correct magnetometer frame transformation. Adjust BMI088 acc filter parametrization. Correct air drag coefficient for QK4.
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 | 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 |