Small, versatile 9-axis sensor module by Bosch Sensortec 3D Accelerometer + 3D Gyroscope + 3D Magnetometer
Dependents: Pruebas_Flex_IMU_copy BMX055_Madgwick
BMX055.cpp@1:0d05e7c9ff4c, 2019-01-16 (annotated)
- Committer:
- kenjiArai
- Date:
- Wed Jan 16 01:36:49 2019 +0000
- Revision:
- 1:0d05e7c9ff4c
- Parent:
- 0:4196bd8bb88f
- Child:
- 2:1bea0ef16d84
small modification
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kenjiArai | 0:4196bd8bb88f | 1 | /* |
kenjiArai | 0:4196bd8bb88f | 2 | * mbed library program |
kenjiArai | 0:4196bd8bb88f | 3 | * BMX055 Small, versatile 9-axis sensor module |
kenjiArai | 0:4196bd8bb88f | 4 | * by Bosch Sensortec |
kenjiArai | 0:4196bd8bb88f | 5 | * |
kenjiArai | 1:0d05e7c9ff4c | 6 | * Copyright (c) 2018,'19 Kenji Arai / JH1PJL |
kenjiArai | 0:4196bd8bb88f | 7 | * http://www.page.sannet.ne.jp/kenjia/index.html |
kenjiArai | 0:4196bd8bb88f | 8 | * http://mbed.org/users/kenjiArai/ |
kenjiArai | 0:4196bd8bb88f | 9 | * Started: October 24th, 2018 |
kenjiArai | 1:0d05e7c9ff4c | 10 | * Revised: January 8th, 2019 |
kenjiArai | 0:4196bd8bb88f | 11 | */ |
kenjiArai | 0:4196bd8bb88f | 12 | |
kenjiArai | 0:4196bd8bb88f | 13 | #include "mbed.h" |
kenjiArai | 0:4196bd8bb88f | 14 | #include "BMX055.h" |
kenjiArai | 0:4196bd8bb88f | 15 | |
kenjiArai | 0:4196bd8bb88f | 16 | |
kenjiArai | 0:4196bd8bb88f | 17 | #if MBED_MAJOR_VERSION == 2 |
kenjiArai | 0:4196bd8bb88f | 18 | #define WAIT_MS(x) wait_ms(x) |
kenjiArai | 0:4196bd8bb88f | 19 | #elif MBED_MAJOR_VERSION == 5 |
kenjiArai | 0:4196bd8bb88f | 20 | #define WAIT_MS(x) Thread::wait(x) |
kenjiArai | 0:4196bd8bb88f | 21 | #else |
kenjiArai | 0:4196bd8bb88f | 22 | #error "Running on Unknown OS" |
kenjiArai | 0:4196bd8bb88f | 23 | #endif |
kenjiArai | 0:4196bd8bb88f | 24 | |
kenjiArai | 0:4196bd8bb88f | 25 | BMX055::BMX055 (PinName p_sda, PinName p_scl): |
kenjiArai | 0:4196bd8bb88f | 26 | _i2c_p(new I2C(p_sda, p_scl)), _i2c(*_i2c_p) |
kenjiArai | 0:4196bd8bb88f | 27 | { |
kenjiArai | 0:4196bd8bb88f | 28 | bmx055_parameters = bmx055_std_paramtr; |
kenjiArai | 0:4196bd8bb88f | 29 | initialize (); |
kenjiArai | 0:4196bd8bb88f | 30 | } |
kenjiArai | 0:4196bd8bb88f | 31 | |
kenjiArai | 0:4196bd8bb88f | 32 | BMX055::BMX055 (I2C& p_i2c) : |
kenjiArai | 0:4196bd8bb88f | 33 | _i2c(p_i2c) |
kenjiArai | 0:4196bd8bb88f | 34 | { |
kenjiArai | 0:4196bd8bb88f | 35 | initialize (); |
kenjiArai | 0:4196bd8bb88f | 36 | } |
kenjiArai | 0:4196bd8bb88f | 37 | |
kenjiArai | 0:4196bd8bb88f | 38 | /////////////// Set parameter ///////////////////////////// |
kenjiArai | 0:4196bd8bb88f | 39 | void BMX055::set_parameter(const BMX055_TypeDef *bmx055_parameter) |
kenjiArai | 0:4196bd8bb88f | 40 | { |
kenjiArai | 0:4196bd8bb88f | 41 | bmx055_parameters = *bmx055_parameter; |
kenjiArai | 0:4196bd8bb88f | 42 | set_parameters_to_regs(); |
kenjiArai | 0:4196bd8bb88f | 43 | |
kenjiArai | 0:4196bd8bb88f | 44 | } |
kenjiArai | 0:4196bd8bb88f | 45 | |
kenjiArai | 0:4196bd8bb88f | 46 | /////////////// Read data & normalize ///////////////////// |
kenjiArai | 0:4196bd8bb88f | 47 | void BMX055::get_accel(BMX055_ACCEL_TypeDef *acc) |
kenjiArai | 0:4196bd8bb88f | 48 | { |
kenjiArai | 0:4196bd8bb88f | 49 | int16_t x,y,z; |
kenjiArai | 0:4196bd8bb88f | 50 | float factor = 2.0f; |
kenjiArai | 0:4196bd8bb88f | 51 | |
kenjiArai | 0:4196bd8bb88f | 52 | chip_addr = inf_addr.acc_addr; |
kenjiArai | 0:4196bd8bb88f | 53 | dt[0] = 0x02; |
kenjiArai | 0:4196bd8bb88f | 54 | _i2c.write(chip_addr, dt, 1, true); |
kenjiArai | 0:4196bd8bb88f | 55 | _i2c.read(chip_addr, dt, 6, false); |
kenjiArai | 0:4196bd8bb88f | 56 | #if 0 |
kenjiArai | 0:4196bd8bb88f | 57 | printf("Read ACC data-> "); |
kenjiArai | 0:4196bd8bb88f | 58 | for (uint32_t i = 0; i < 6; i++){ |
kenjiArai | 0:4196bd8bb88f | 59 | printf("i=%d,dt=0x%02x, ", i, dt[i]); |
kenjiArai | 0:4196bd8bb88f | 60 | } |
kenjiArai | 0:4196bd8bb88f | 61 | printf(", all\r\n"); |
kenjiArai | 0:4196bd8bb88f | 62 | #endif |
kenjiArai | 0:4196bd8bb88f | 63 | x = dt[1] << 8 | (dt[0] & 0xf0); |
kenjiArai | 0:4196bd8bb88f | 64 | y = dt[3] << 8 | (dt[2] & 0xf0); |
kenjiArai | 0:4196bd8bb88f | 65 | z = dt[5] << 8 | (dt[4] & 0xf0); |
kenjiArai | 0:4196bd8bb88f | 66 | switch(bmx055_parameters.acc_fs){ |
kenjiArai | 0:4196bd8bb88f | 67 | case ACC_2G: |
kenjiArai | 0:4196bd8bb88f | 68 | factor = 2.0f; |
kenjiArai | 0:4196bd8bb88f | 69 | break; |
kenjiArai | 0:4196bd8bb88f | 70 | case ACC_4G: |
kenjiArai | 0:4196bd8bb88f | 71 | factor = 4.0f; |
kenjiArai | 0:4196bd8bb88f | 72 | break; |
kenjiArai | 0:4196bd8bb88f | 73 | case ACC_8G: |
kenjiArai | 0:4196bd8bb88f | 74 | factor = 8.0f; |
kenjiArai | 0:4196bd8bb88f | 75 | break; |
kenjiArai | 0:4196bd8bb88f | 76 | case ACC_16G: |
kenjiArai | 0:4196bd8bb88f | 77 | factor = 16.0f; |
kenjiArai | 0:4196bd8bb88f | 78 | break; |
kenjiArai | 0:4196bd8bb88f | 79 | default: |
kenjiArai | 0:4196bd8bb88f | 80 | factor = 0; |
kenjiArai | 0:4196bd8bb88f | 81 | break; |
kenjiArai | 0:4196bd8bb88f | 82 | } |
kenjiArai | 0:4196bd8bb88f | 83 | acc->x = (double)x * factor / 2048.0f / 16.0f; |
kenjiArai | 0:4196bd8bb88f | 84 | acc->y = (double)y * factor / 2048.0f / 16.0f; |
kenjiArai | 0:4196bd8bb88f | 85 | acc->z = (double)z * factor / 2048.0f / 16.0f; |
kenjiArai | 0:4196bd8bb88f | 86 | } |
kenjiArai | 0:4196bd8bb88f | 87 | |
kenjiArai | 0:4196bd8bb88f | 88 | void BMX055::get_gyro(BMX055_GYRO_TypeDef *gyr) |
kenjiArai | 0:4196bd8bb88f | 89 | { |
kenjiArai | 0:4196bd8bb88f | 90 | int16_t x,y,z; |
kenjiArai | 0:4196bd8bb88f | 91 | float factor = 2.0f; |
kenjiArai | 0:4196bd8bb88f | 92 | |
kenjiArai | 0:4196bd8bb88f | 93 | chip_addr = inf_addr.gyr_addr; |
kenjiArai | 0:4196bd8bb88f | 94 | dt[0] = 0x02; |
kenjiArai | 0:4196bd8bb88f | 95 | _i2c.write(chip_addr, dt, 1, true); |
kenjiArai | 0:4196bd8bb88f | 96 | _i2c.read(chip_addr, dt, 6, false); |
kenjiArai | 0:4196bd8bb88f | 97 | #if 0 |
kenjiArai | 0:4196bd8bb88f | 98 | printf("Read MAG data-> "); |
kenjiArai | 0:4196bd8bb88f | 99 | for (uint32_t i = 0; i < 6; i++){ |
kenjiArai | 0:4196bd8bb88f | 100 | printf("i=%d,dt=0x%02x, ", i, dt[i]); |
kenjiArai | 0:4196bd8bb88f | 101 | } |
kenjiArai | 0:4196bd8bb88f | 102 | printf(", all\r\n"); |
kenjiArai | 0:4196bd8bb88f | 103 | #endif |
kenjiArai | 0:4196bd8bb88f | 104 | x = dt[1] << 8 | dt[0]; |
kenjiArai | 0:4196bd8bb88f | 105 | y = dt[3] << 8 | dt[2]; |
kenjiArai | 0:4196bd8bb88f | 106 | z = dt[5] << 8 | dt[4]; |
kenjiArai | 0:4196bd8bb88f | 107 | #if 0 |
kenjiArai | 0:4196bd8bb88f | 108 | switch(bmx055_parameters.gyr_fs){ |
kenjiArai | 0:4196bd8bb88f | 109 | case GYR_2000DPS: |
kenjiArai | 0:4196bd8bb88f | 110 | factor = 1998.0f; |
kenjiArai | 0:4196bd8bb88f | 111 | break; |
kenjiArai | 0:4196bd8bb88f | 112 | case GYR_1000DPS: |
kenjiArai | 0:4196bd8bb88f | 113 | factor = 999.0f; |
kenjiArai | 0:4196bd8bb88f | 114 | break; |
kenjiArai | 0:4196bd8bb88f | 115 | case GYR_500DPS: |
kenjiArai | 0:4196bd8bb88f | 116 | factor = 499.5f; |
kenjiArai | 0:4196bd8bb88f | 117 | break; |
kenjiArai | 0:4196bd8bb88f | 118 | case GYR_250DPS: |
kenjiArai | 0:4196bd8bb88f | 119 | factor = 249.75f; |
kenjiArai | 0:4196bd8bb88f | 120 | break; |
kenjiArai | 0:4196bd8bb88f | 121 | case GYR_125DPS: |
kenjiArai | 0:4196bd8bb88f | 122 | factor = 124.87f; |
kenjiArai | 0:4196bd8bb88f | 123 | break; |
kenjiArai | 0:4196bd8bb88f | 124 | default: |
kenjiArai | 0:4196bd8bb88f | 125 | factor = 0; |
kenjiArai | 0:4196bd8bb88f | 126 | break; |
kenjiArai | 0:4196bd8bb88f | 127 | } |
kenjiArai | 0:4196bd8bb88f | 128 | gyr->x = (double)x * factor / 32768.0f / 16.0f; |
kenjiArai | 0:4196bd8bb88f | 129 | gyr->y = (double)y * factor / 32768.0f / 16.0f; |
kenjiArai | 0:4196bd8bb88f | 130 | gyr->z = (double)z * factor / 32768.0f / 16.0f; |
kenjiArai | 0:4196bd8bb88f | 131 | #else |
kenjiArai | 0:4196bd8bb88f | 132 | switch(bmx055_parameters.gyr_fs){ |
kenjiArai | 0:4196bd8bb88f | 133 | case GYR_2000DPS: |
kenjiArai | 0:4196bd8bb88f | 134 | factor = 61.0f; |
kenjiArai | 0:4196bd8bb88f | 135 | break; |
kenjiArai | 0:4196bd8bb88f | 136 | case GYR_1000DPS: |
kenjiArai | 0:4196bd8bb88f | 137 | factor = 30.5f; |
kenjiArai | 0:4196bd8bb88f | 138 | break; |
kenjiArai | 0:4196bd8bb88f | 139 | case GYR_500DPS: |
kenjiArai | 0:4196bd8bb88f | 140 | factor = 15.3; |
kenjiArai | 0:4196bd8bb88f | 141 | break; |
kenjiArai | 0:4196bd8bb88f | 142 | case GYR_250DPS: |
kenjiArai | 0:4196bd8bb88f | 143 | factor = 7.6f; |
kenjiArai | 0:4196bd8bb88f | 144 | break; |
kenjiArai | 0:4196bd8bb88f | 145 | case GYR_125DPS: |
kenjiArai | 0:4196bd8bb88f | 146 | factor = 3.8f; |
kenjiArai | 0:4196bd8bb88f | 147 | break; |
kenjiArai | 0:4196bd8bb88f | 148 | default: |
kenjiArai | 0:4196bd8bb88f | 149 | factor = 0; |
kenjiArai | 0:4196bd8bb88f | 150 | break; |
kenjiArai | 0:4196bd8bb88f | 151 | } |
kenjiArai | 0:4196bd8bb88f | 152 | gyr->x = (double)x * factor / 1000.0f; |
kenjiArai | 0:4196bd8bb88f | 153 | gyr->y = (double)y * factor / 1000.0f; |
kenjiArai | 0:4196bd8bb88f | 154 | gyr->z = (double)z * factor / 1000.0f; |
kenjiArai | 0:4196bd8bb88f | 155 | #endif |
kenjiArai | 0:4196bd8bb88f | 156 | } |
kenjiArai | 0:4196bd8bb88f | 157 | |
kenjiArai | 0:4196bd8bb88f | 158 | void BMX055::get_magnet(BMX055_MAGNET_TypeDef *mag) |
kenjiArai | 0:4196bd8bb88f | 159 | { |
kenjiArai | 0:4196bd8bb88f | 160 | int16_t x,y,z; |
kenjiArai | 0:4196bd8bb88f | 161 | |
kenjiArai | 0:4196bd8bb88f | 162 | chip_addr = inf_addr.gyr_addr; |
kenjiArai | 0:4196bd8bb88f | 163 | dt[0] = 0x02; |
kenjiArai | 0:4196bd8bb88f | 164 | _i2c.write(chip_addr, dt, 1, true); |
kenjiArai | 0:4196bd8bb88f | 165 | _i2c.read(chip_addr, dt, 6, false); |
kenjiArai | 0:4196bd8bb88f | 166 | #if 0 |
kenjiArai | 0:4196bd8bb88f | 167 | printf("Read GYR data-> "); |
kenjiArai | 0:4196bd8bb88f | 168 | for (uint32_t i = 0; i < 6; i++){ |
kenjiArai | 0:4196bd8bb88f | 169 | printf("i=%d,dt=0x%02x, ", i, dt[i]); |
kenjiArai | 0:4196bd8bb88f | 170 | } |
kenjiArai | 0:4196bd8bb88f | 171 | printf(", all\r\n"); |
kenjiArai | 0:4196bd8bb88f | 172 | #endif |
kenjiArai | 0:4196bd8bb88f | 173 | x = dt[1] << 8 | (dt[0] & 0xf8); // 13bit |
kenjiArai | 0:4196bd8bb88f | 174 | y = dt[3] << 8 | (dt[2] & 0xf8); // 13bit |
kenjiArai | 0:4196bd8bb88f | 175 | z = dt[5] << 8 | (dt[4] & 0xfe); // 15bit |
kenjiArai | 0:4196bd8bb88f | 176 | mag->x = (double)x / 8.0f; |
kenjiArai | 0:4196bd8bb88f | 177 | mag->y = (double)y / 8.0f; |
kenjiArai | 0:4196bd8bb88f | 178 | mag->z = (double)z / 2.0f; |
kenjiArai | 0:4196bd8bb88f | 179 | } |
kenjiArai | 0:4196bd8bb88f | 180 | |
kenjiArai | 0:4196bd8bb88f | 181 | float BMX055::get_chip_temperature() |
kenjiArai | 0:4196bd8bb88f | 182 | { |
kenjiArai | 0:4196bd8bb88f | 183 | chip_addr = inf_addr.acc_addr; |
kenjiArai | 0:4196bd8bb88f | 184 | dt[0] = 0x08; // chip tempareture reg addr |
kenjiArai | 0:4196bd8bb88f | 185 | _i2c.write(chip_addr, dt, 1, true); |
kenjiArai | 0:4196bd8bb88f | 186 | _i2c.read(chip_addr, dt, 1, false); |
kenjiArai | 0:4196bd8bb88f | 187 | //printf("Temp reg = 0x%02x\r\n", dt[0]); |
kenjiArai | 0:4196bd8bb88f | 188 | return (float)((int8_t)dt[0]) * 0.5f + 23.0f; |
kenjiArai | 0:4196bd8bb88f | 189 | } |
kenjiArai | 0:4196bd8bb88f | 190 | |
kenjiArai | 0:4196bd8bb88f | 191 | /////////////// Initialize //////////////////////////////// |
kenjiArai | 0:4196bd8bb88f | 192 | void BMX055::initialize (void) |
kenjiArai | 0:4196bd8bb88f | 193 | { |
kenjiArai | 0:4196bd8bb88f | 194 | _i2c.frequency(100000); |
kenjiArai | 0:4196bd8bb88f | 195 | // Check Acc & Mag & Gyro are available of not |
kenjiArai | 0:4196bd8bb88f | 196 | check_id(); |
kenjiArai | 0:4196bd8bb88f | 197 | if (ready_flag == 0x07){ |
kenjiArai | 1:0d05e7c9ff4c | 198 | #if 0 |
kenjiArai | 1:0d05e7c9ff4c | 199 | printf("ACC+GYR+MAG are ready!\r\n"); |
kenjiArai | 1:0d05e7c9ff4c | 200 | #endif |
kenjiArai | 0:4196bd8bb88f | 201 | } |
kenjiArai | 0:4196bd8bb88f | 202 | // Set initial data |
kenjiArai | 0:4196bd8bb88f | 203 | set_parameters_to_regs(); |
kenjiArai | 0:4196bd8bb88f | 204 | } |
kenjiArai | 0:4196bd8bb88f | 205 | |
kenjiArai | 0:4196bd8bb88f | 206 | ////// Set initialize data to related registers /////////// |
kenjiArai | 0:4196bd8bb88f | 207 | void BMX055::set_parameters_to_regs(void) |
kenjiArai | 0:4196bd8bb88f | 208 | { |
kenjiArai | 0:4196bd8bb88f | 209 | // ACC |
kenjiArai | 0:4196bd8bb88f | 210 | chip_addr = inf_addr.acc_addr; |
kenjiArai | 0:4196bd8bb88f | 211 | dt[0] = 0x0f; // Select PMU_Range register |
kenjiArai | 0:4196bd8bb88f | 212 | dt[1] = bmx055_parameters.acc_fs; |
kenjiArai | 0:4196bd8bb88f | 213 | _i2c.write(chip_addr, dt, 2, false); |
kenjiArai | 0:4196bd8bb88f | 214 | wait_ms(1); |
kenjiArai | 0:4196bd8bb88f | 215 | dt[0] = 0x10; // Select PMU_BW register |
kenjiArai | 0:4196bd8bb88f | 216 | dt[1] = bmx055_parameters.acc_bw; |
kenjiArai | 0:4196bd8bb88f | 217 | _i2c.write(chip_addr, dt, 2, false); |
kenjiArai | 0:4196bd8bb88f | 218 | wait_ms(1); |
kenjiArai | 0:4196bd8bb88f | 219 | dt[0] = 0x11; // Select PMU_LPW register |
kenjiArai | 0:4196bd8bb88f | 220 | dt[1] = 0x00; // Normal mode, Sleep duration = 0.5ms |
kenjiArai | 0:4196bd8bb88f | 221 | _i2c.write(chip_addr, dt, 2, false); |
kenjiArai | 0:4196bd8bb88f | 222 | // GYR |
kenjiArai | 0:4196bd8bb88f | 223 | chip_addr = inf_addr.gyr_addr; |
kenjiArai | 0:4196bd8bb88f | 224 | dt[0] = 0x0f; // Select Range register |
kenjiArai | 0:4196bd8bb88f | 225 | dt[1] = bmx055_parameters.gyr_fs; |
kenjiArai | 0:4196bd8bb88f | 226 | _i2c.write(chip_addr, dt, 2, false); |
kenjiArai | 0:4196bd8bb88f | 227 | wait_ms(1); |
kenjiArai | 0:4196bd8bb88f | 228 | dt[0] = 0x10; // Select Bandwidth register |
kenjiArai | 0:4196bd8bb88f | 229 | dt[1] = bmx055_parameters.gyr_bw; |
kenjiArai | 0:4196bd8bb88f | 230 | _i2c.write(chip_addr, dt, 2, false); |
kenjiArai | 0:4196bd8bb88f | 231 | wait_ms(1); |
kenjiArai | 0:4196bd8bb88f | 232 | dt[0] = 0x11; // Select LPM1 register |
kenjiArai | 0:4196bd8bb88f | 233 | dt[1] = 0x00; // Normal mode, Sleep duration = 2ms |
kenjiArai | 0:4196bd8bb88f | 234 | _i2c.write(chip_addr, dt, 2, false); |
kenjiArai | 0:4196bd8bb88f | 235 | // MAG |
kenjiArai | 0:4196bd8bb88f | 236 | chip_addr = inf_addr.mag_addr; |
kenjiArai | 0:4196bd8bb88f | 237 | dt[0] = 0x4b; // Select Mag register |
kenjiArai | 0:4196bd8bb88f | 238 | dt[1] = 0x83; // Soft reset |
kenjiArai | 0:4196bd8bb88f | 239 | _i2c.write(chip_addr, dt, 2, false); |
kenjiArai | 0:4196bd8bb88f | 240 | wait_ms(10); |
kenjiArai | 0:4196bd8bb88f | 241 | dt[0] = 0x4b; // Select Mag register |
kenjiArai | 0:4196bd8bb88f | 242 | dt[1] = 0x01; // Soft reset |
kenjiArai | 0:4196bd8bb88f | 243 | _i2c.write(chip_addr, dt, 2, false); |
kenjiArai | 0:4196bd8bb88f | 244 | wait_ms(10); |
kenjiArai | 0:4196bd8bb88f | 245 | dt[0] = 0x4c; // Select Mag register |
kenjiArai | 0:4196bd8bb88f | 246 | dt[1] = bmx055_parameters.mag_odr; |
kenjiArai | 0:4196bd8bb88f | 247 | _i2c.write(chip_addr, dt, 2, false); |
kenjiArai | 0:4196bd8bb88f | 248 | wait_ms(1); |
kenjiArai | 0:4196bd8bb88f | 249 | dt[0] = 0x4e; // Select Mag register |
kenjiArai | 0:4196bd8bb88f | 250 | dt[1] = 0x84; // X, Y, Z-Axis enabled |
kenjiArai | 0:4196bd8bb88f | 251 | _i2c.write(chip_addr, dt, 2, false); |
kenjiArai | 0:4196bd8bb88f | 252 | wait_ms(1); |
kenjiArai | 0:4196bd8bb88f | 253 | dt[0] = 0x51; // Select Mag register |
kenjiArai | 0:4196bd8bb88f | 254 | dt[1] = 0x04; // No. of Repetitions for X-Y Axis = 9 |
kenjiArai | 0:4196bd8bb88f | 255 | _i2c.write(chip_addr, dt, 2, false); |
kenjiArai | 0:4196bd8bb88f | 256 | wait_ms(1); |
kenjiArai | 0:4196bd8bb88f | 257 | dt[0] = 0x52; // Select Mag register |
kenjiArai | 0:4196bd8bb88f | 258 | dt[1] = 0x16; // No. of Repetitions for Z-Axis = 15 |
kenjiArai | 0:4196bd8bb88f | 259 | _i2c.write(chip_addr, dt, 2, false); |
kenjiArai | 0:4196bd8bb88f | 260 | #if 0 |
kenjiArai | 0:4196bd8bb88f | 261 | // ACC |
kenjiArai | 0:4196bd8bb88f | 262 | chip_addr = inf_addr.acc_addr; |
kenjiArai | 0:4196bd8bb88f | 263 | dt[0] = 0x0f; // Select PMU_Range register |
kenjiArai | 0:4196bd8bb88f | 264 | dt[1] = 0x03; // Range = +/- 2g |
kenjiArai | 0:4196bd8bb88f | 265 | _i2c.write(chip_addr, dt, 2, false); |
kenjiArai | 0:4196bd8bb88f | 266 | wait_ms(1); |
kenjiArai | 0:4196bd8bb88f | 267 | dt[0] = 0x10; // Select PMU_BW register |
kenjiArai | 0:4196bd8bb88f | 268 | dt[1] = 0x08; // Bandwidth = 7.81 Hz |
kenjiArai | 0:4196bd8bb88f | 269 | _i2c.write(chip_addr, dt, 2, false); |
kenjiArai | 0:4196bd8bb88f | 270 | wait_ms(1); |
kenjiArai | 0:4196bd8bb88f | 271 | dt[0] = 0x11; // Select PMU_LPW register |
kenjiArai | 0:4196bd8bb88f | 272 | dt[1] = 0x00; // Normal mode, Sleep duration = 0.5ms |
kenjiArai | 0:4196bd8bb88f | 273 | _i2c.write(chip_addr, dt, 2, false); |
kenjiArai | 0:4196bd8bb88f | 274 | wait_ms(1); |
kenjiArai | 0:4196bd8bb88f | 275 | // GYR |
kenjiArai | 0:4196bd8bb88f | 276 | chip_addr = inf_addr.gyr_addr; |
kenjiArai | 0:4196bd8bb88f | 277 | dt[0] = 0x0f; // Select Range register |
kenjiArai | 0:4196bd8bb88f | 278 | dt[1] = 0x04; // Full scale = +/- 125 degree/s |
kenjiArai | 0:4196bd8bb88f | 279 | _i2c.write(chip_addr, dt, 2, false); |
kenjiArai | 0:4196bd8bb88f | 280 | wait_ms(1); |
kenjiArai | 0:4196bd8bb88f | 281 | dt[0] = 0x10; // Select Bandwidth register |
kenjiArai | 0:4196bd8bb88f | 282 | dt[1] = 0x07; // ODR = 100 Hz |
kenjiArai | 0:4196bd8bb88f | 283 | _i2c.write(chip_addr, dt, 2, false); |
kenjiArai | 0:4196bd8bb88f | 284 | wait_ms(1); |
kenjiArai | 0:4196bd8bb88f | 285 | dt[0] = 0x11; // Select LPM1 register |
kenjiArai | 0:4196bd8bb88f | 286 | dt[1] = 0x00; // Normal mode, Sleep duration = 2ms |
kenjiArai | 0:4196bd8bb88f | 287 | _i2c.write(chip_addr, dt, 2, false); |
kenjiArai | 0:4196bd8bb88f | 288 | // MAG |
kenjiArai | 0:4196bd8bb88f | 289 | chip_addr = inf_addr.mag_addr; |
kenjiArai | 0:4196bd8bb88f | 290 | dt[0] = 0x4b; // Select Mag register |
kenjiArai | 0:4196bd8bb88f | 291 | dt[1] = 0x83; // Soft reset |
kenjiArai | 0:4196bd8bb88f | 292 | _i2c.write(chip_addr, dt, 2, false); |
kenjiArai | 0:4196bd8bb88f | 293 | wait_ms(10); |
kenjiArai | 0:4196bd8bb88f | 294 | dt[0] = 0x4b; // Select Mag register |
kenjiArai | 0:4196bd8bb88f | 295 | dt[1] = 0x01; // Soft reset |
kenjiArai | 0:4196bd8bb88f | 296 | _i2c.write(chip_addr, dt, 2, false); |
kenjiArai | 0:4196bd8bb88f | 297 | wait_ms(10); |
kenjiArai | 0:4196bd8bb88f | 298 | dt[0] = 0x4c; // Select Mag register |
kenjiArai | 0:4196bd8bb88f | 299 | dt[1] = 0x00; // Normal Mode, ODR = 10 Hz |
kenjiArai | 0:4196bd8bb88f | 300 | _i2c.write(chip_addr, dt, 2, false); |
kenjiArai | 0:4196bd8bb88f | 301 | wait_ms(1); |
kenjiArai | 0:4196bd8bb88f | 302 | dt[0] = 0x4e; // Select Mag register |
kenjiArai | 0:4196bd8bb88f | 303 | dt[1] = 0x84; // X, Y, Z-Axis enabled |
kenjiArai | 0:4196bd8bb88f | 304 | _i2c.write(chip_addr, dt, 2, false); |
kenjiArai | 0:4196bd8bb88f | 305 | wait_ms(1); |
kenjiArai | 0:4196bd8bb88f | 306 | dt[0] = 0x51; // Select Mag register |
kenjiArai | 0:4196bd8bb88f | 307 | dt[1] = 0x04; // No. of Repetitions for X-Y Axis = 9 |
kenjiArai | 0:4196bd8bb88f | 308 | _i2c.write(chip_addr, dt, 2, false); |
kenjiArai | 0:4196bd8bb88f | 309 | wait_ms(1); |
kenjiArai | 0:4196bd8bb88f | 310 | dt[0] = 0x52; // Select Mag register |
kenjiArai | 0:4196bd8bb88f | 311 | dt[1] = 0x16; // No. of Repetitions for Z-Axis = 15 |
kenjiArai | 0:4196bd8bb88f | 312 | _i2c.write(chip_addr, dt, 2, false); |
kenjiArai | 0:4196bd8bb88f | 313 | wait_ms(1); |
kenjiArai | 0:4196bd8bb88f | 314 | #endif |
kenjiArai | 0:4196bd8bb88f | 315 | } |
kenjiArai | 0:4196bd8bb88f | 316 | |
kenjiArai | 0:4196bd8bb88f | 317 | /////////////// Check Who am I? /////////////////////////// |
kenjiArai | 0:4196bd8bb88f | 318 | void BMX055::check_id(void) |
kenjiArai | 0:4196bd8bb88f | 319 | { |
kenjiArai | 0:4196bd8bb88f | 320 | ready_flag = 0; |
kenjiArai | 0:4196bd8bb88f | 321 | // ID ACC |
kenjiArai | 0:4196bd8bb88f | 322 | inf_addr.acc_addr = BMX055_ACC_CHIP_ADDR; |
kenjiArai | 0:4196bd8bb88f | 323 | chip_addr = inf_addr.acc_addr; |
kenjiArai | 0:4196bd8bb88f | 324 | dt[0] = 0x00; // chip ID reg addr |
kenjiArai | 0:4196bd8bb88f | 325 | _i2c.write(chip_addr, dt, 1, true); |
kenjiArai | 0:4196bd8bb88f | 326 | _i2c.read(chip_addr, dt, 1, false); |
kenjiArai | 0:4196bd8bb88f | 327 | inf_id.acc_id = dt[0]; |
kenjiArai | 0:4196bd8bb88f | 328 | if (inf_id.acc_id == I_AM_BMX055_ACC) { |
kenjiArai | 0:4196bd8bb88f | 329 | ready_flag |= 0x01; |
kenjiArai | 0:4196bd8bb88f | 330 | } else { |
kenjiArai | 0:4196bd8bb88f | 331 | inf_addr.acc_addr = (0x18 << 1); |
kenjiArai | 0:4196bd8bb88f | 332 | chip_addr = inf_addr.acc_addr; |
kenjiArai | 0:4196bd8bb88f | 333 | dt[0] = 0x00; // chip ID reg addr |
kenjiArai | 0:4196bd8bb88f | 334 | _i2c.write(chip_addr, dt, 1, true); |
kenjiArai | 0:4196bd8bb88f | 335 | _i2c.read(chip_addr, dt, 1, false); |
kenjiArai | 0:4196bd8bb88f | 336 | inf_id.acc_id = dt[0]; |
kenjiArai | 0:4196bd8bb88f | 337 | if (inf_id.acc_id == I_AM_BMX055_ACC) { |
kenjiArai | 0:4196bd8bb88f | 338 | ready_flag |= 0x01; |
kenjiArai | 0:4196bd8bb88f | 339 | } |
kenjiArai | 0:4196bd8bb88f | 340 | } |
kenjiArai | 0:4196bd8bb88f | 341 | // ID GYRO |
kenjiArai | 0:4196bd8bb88f | 342 | inf_addr.gyr_addr = BMX055_GYR_CHIP_ADDR; |
kenjiArai | 0:4196bd8bb88f | 343 | chip_addr = inf_addr.gyr_addr; |
kenjiArai | 0:4196bd8bb88f | 344 | dt[0] = 0x00; // chip ID reg addr |
kenjiArai | 0:4196bd8bb88f | 345 | _i2c.write(chip_addr, dt, 1, true); |
kenjiArai | 0:4196bd8bb88f | 346 | _i2c.read(chip_addr, dt, 1, false); |
kenjiArai | 0:4196bd8bb88f | 347 | inf_id.gyr_id = dt[0]; |
kenjiArai | 0:4196bd8bb88f | 348 | if (inf_id.gyr_id == I_AM_BMX055_GYR) { |
kenjiArai | 0:4196bd8bb88f | 349 | ready_flag |= 0x02; |
kenjiArai | 0:4196bd8bb88f | 350 | } else { |
kenjiArai | 0:4196bd8bb88f | 351 | inf_addr.gyr_addr = (0x68 << 1); |
kenjiArai | 0:4196bd8bb88f | 352 | chip_addr = inf_addr.gyr_addr; |
kenjiArai | 0:4196bd8bb88f | 353 | dt[0] = 0x00; // chip ID reg addr |
kenjiArai | 0:4196bd8bb88f | 354 | _i2c.write(chip_addr, dt, 1, true); |
kenjiArai | 0:4196bd8bb88f | 355 | _i2c.read(chip_addr, dt, 1, false); |
kenjiArai | 0:4196bd8bb88f | 356 | inf_id.gyr_id = dt[0]; |
kenjiArai | 0:4196bd8bb88f | 357 | if (inf_id.gyr_id == I_AM_BMX055_GYR) { |
kenjiArai | 0:4196bd8bb88f | 358 | ready_flag |= 0x02; |
kenjiArai | 0:4196bd8bb88f | 359 | } |
kenjiArai | 0:4196bd8bb88f | 360 | } |
kenjiArai | 0:4196bd8bb88f | 361 | // ID Mag |
kenjiArai | 0:4196bd8bb88f | 362 | inf_addr.mag_addr = BMX055_MAG_CHIP_ADDR; |
kenjiArai | 0:4196bd8bb88f | 363 | chip_addr = inf_addr.mag_addr; |
kenjiArai | 0:4196bd8bb88f | 364 | dt[0] = 0x4b; // reg addr |
kenjiArai | 0:4196bd8bb88f | 365 | dt[1] = 0x01; // control power bit set 1 |
kenjiArai | 0:4196bd8bb88f | 366 | _i2c.write(chip_addr, dt, 2, false); |
kenjiArai | 0:4196bd8bb88f | 367 | dt[0] = 0x40; // chip ID reg addr |
kenjiArai | 0:4196bd8bb88f | 368 | _i2c.write(chip_addr, dt, 1, true); |
kenjiArai | 0:4196bd8bb88f | 369 | _i2c.read(chip_addr, dt, 1, false); |
kenjiArai | 0:4196bd8bb88f | 370 | inf_id.mag_id = dt[0]; |
kenjiArai | 0:4196bd8bb88f | 371 | if (inf_id.mag_id == I_AM_BMX055_MAG) { |
kenjiArai | 0:4196bd8bb88f | 372 | ready_flag |= 0x04; |
kenjiArai | 0:4196bd8bb88f | 373 | } else { |
kenjiArai | 0:4196bd8bb88f | 374 | inf_addr.mag_addr = (0x12 << 1); |
kenjiArai | 0:4196bd8bb88f | 375 | chip_addr = inf_addr.mag_addr; |
kenjiArai | 0:4196bd8bb88f | 376 | // control power bit set 1 |
kenjiArai | 0:4196bd8bb88f | 377 | dt[0] = 0x4b; |
kenjiArai | 0:4196bd8bb88f | 378 | dt[1] = 0x01; |
kenjiArai | 0:4196bd8bb88f | 379 | _i2c.write(chip_addr, dt, 2, false); |
kenjiArai | 0:4196bd8bb88f | 380 | dt[0] = 0x40; |
kenjiArai | 0:4196bd8bb88f | 381 | _i2c.write(chip_addr, dt, 1, true); |
kenjiArai | 0:4196bd8bb88f | 382 | _i2c.read(chip_addr, dt, 1, false); |
kenjiArai | 0:4196bd8bb88f | 383 | inf_id.mag_id = dt[0]; |
kenjiArai | 0:4196bd8bb88f | 384 | if (inf_id.mag_id == I_AM_BMX055_MAG) { |
kenjiArai | 0:4196bd8bb88f | 385 | ready_flag |= 0x04; |
kenjiArai | 0:4196bd8bb88f | 386 | } else { |
kenjiArai | 0:4196bd8bb88f | 387 | inf_addr.mag_addr = (0x11 << 1); |
kenjiArai | 0:4196bd8bb88f | 388 | chip_addr = inf_addr.mag_addr; |
kenjiArai | 0:4196bd8bb88f | 389 | // control power bit set 1 |
kenjiArai | 0:4196bd8bb88f | 390 | dt[0] = 0x4b; |
kenjiArai | 0:4196bd8bb88f | 391 | dt[1] = 0x01; |
kenjiArai | 0:4196bd8bb88f | 392 | _i2c.write(chip_addr, dt, 2, false); |
kenjiArai | 0:4196bd8bb88f | 393 | dt[0] = 0x40; |
kenjiArai | 0:4196bd8bb88f | 394 | _i2c.write(chip_addr, dt, 1, true); |
kenjiArai | 0:4196bd8bb88f | 395 | _i2c.read(chip_addr, dt, 1, false); |
kenjiArai | 0:4196bd8bb88f | 396 | inf_id.mag_id = dt[0]; |
kenjiArai | 0:4196bd8bb88f | 397 | if (inf_id.mag_id == I_AM_BMX055_MAG) { |
kenjiArai | 0:4196bd8bb88f | 398 | ready_flag |= 0x04; |
kenjiArai | 0:4196bd8bb88f | 399 | } else { |
kenjiArai | 0:4196bd8bb88f | 400 | inf_addr.mag_addr = (0x10 << 1); |
kenjiArai | 0:4196bd8bb88f | 401 | chip_addr = inf_addr.mag_addr; |
kenjiArai | 0:4196bd8bb88f | 402 | // control power bit set 1 |
kenjiArai | 0:4196bd8bb88f | 403 | dt[0] = 0x4b; |
kenjiArai | 0:4196bd8bb88f | 404 | dt[1] = 0x01; |
kenjiArai | 0:4196bd8bb88f | 405 | _i2c.write(chip_addr, dt, 2, false); |
kenjiArai | 0:4196bd8bb88f | 406 | dt[0] = 0x40; |
kenjiArai | 0:4196bd8bb88f | 407 | _i2c.write(chip_addr, dt, 1, true); |
kenjiArai | 0:4196bd8bb88f | 408 | _i2c.read(chip_addr, dt, 1, false); |
kenjiArai | 0:4196bd8bb88f | 409 | inf_id.mag_id = dt[0]; |
kenjiArai | 0:4196bd8bb88f | 410 | if (inf_id.mag_id == I_AM_BMX055_MAG) { |
kenjiArai | 0:4196bd8bb88f | 411 | ready_flag |= 0x04; |
kenjiArai | 0:4196bd8bb88f | 412 | } |
kenjiArai | 0:4196bd8bb88f | 413 | } |
kenjiArai | 0:4196bd8bb88f | 414 | } |
kenjiArai | 0:4196bd8bb88f | 415 | } |
kenjiArai | 1:0d05e7c9ff4c | 416 | #if 0 |
kenjiArai | 1:0d05e7c9ff4c | 417 | printf("ACC addr=0x%02x, id=0x%02x\r\n", chip_addr, inf_id.acc_id); |
kenjiArai | 1:0d05e7c9ff4c | 418 | printf("GYR addr=0x%02x, id=0x%02x\r\n", chip_addr, inf_id.gyr_id); |
kenjiArai | 1:0d05e7c9ff4c | 419 | printf("MAG addr=0x%02x, id=0x%02x\r\n", chip_addr, inf_id.mag_id); |
kenjiArai | 1:0d05e7c9ff4c | 420 | printf("ready_flag = 0x%x\r\n", ready_flag); |
kenjiArai | 1:0d05e7c9ff4c | 421 | #endif |
kenjiArai | 0:4196bd8bb88f | 422 | } |
kenjiArai | 0:4196bd8bb88f | 423 | |
kenjiArai | 0:4196bd8bb88f | 424 | void BMX055::read_id_inf(BMX055_ID_INF_TypeDef *id) |
kenjiArai | 0:4196bd8bb88f | 425 | { |
kenjiArai | 0:4196bd8bb88f | 426 | id->acc_id = acc_id; |
kenjiArai | 0:4196bd8bb88f | 427 | id->mag_id = mag_id; |
kenjiArai | 0:4196bd8bb88f | 428 | id->gyr_id = gyr_id; |
kenjiArai | 0:4196bd8bb88f | 429 | } |
kenjiArai | 0:4196bd8bb88f | 430 | |
kenjiArai | 0:4196bd8bb88f | 431 | /////////////// Check chip ready or not ////////////////// |
kenjiArai | 0:4196bd8bb88f | 432 | bool BMX055::chip_ready(void) |
kenjiArai | 0:4196bd8bb88f | 433 | { |
kenjiArai | 0:4196bd8bb88f | 434 | if (ready_flag == 0x07) { |
kenjiArai | 0:4196bd8bb88f | 435 | return true; |
kenjiArai | 0:4196bd8bb88f | 436 | } |
kenjiArai | 0:4196bd8bb88f | 437 | return false; |
kenjiArai | 0:4196bd8bb88f | 438 | } |
kenjiArai | 0:4196bd8bb88f | 439 | |
kenjiArai | 0:4196bd8bb88f | 440 | /////////////// I2C Freq. ///////////////////////////////// |
kenjiArai | 0:4196bd8bb88f | 441 | void BMX055::frequency(int hz) |
kenjiArai | 0:4196bd8bb88f | 442 | { |
kenjiArai | 0:4196bd8bb88f | 443 | _i2c.frequency(hz); |
kenjiArai | 0:4196bd8bb88f | 444 | } |
kenjiArai | 0:4196bd8bb88f | 445 | |
kenjiArai | 0:4196bd8bb88f | 446 | /////////////// Read/Write specific register ////////////// |
kenjiArai | 0:4196bd8bb88f | 447 | uint8_t BMX055::read_reg(uint8_t addr) |
kenjiArai | 0:4196bd8bb88f | 448 | { |
kenjiArai | 0:4196bd8bb88f | 449 | dt[0] = addr; |
kenjiArai | 0:4196bd8bb88f | 450 | _i2c.write(chip_addr, dt, 1, true); |
kenjiArai | 0:4196bd8bb88f | 451 | _i2c.read(chip_addr, dt, 1, false); |
kenjiArai | 0:4196bd8bb88f | 452 | return (uint8_t)dt[0]; |
kenjiArai | 0:4196bd8bb88f | 453 | } |
kenjiArai | 0:4196bd8bb88f | 454 | |
kenjiArai | 0:4196bd8bb88f | 455 | uint8_t BMX055::write_reg(uint8_t addr, uint8_t data) |
kenjiArai | 0:4196bd8bb88f | 456 | { |
kenjiArai | 0:4196bd8bb88f | 457 | uint8_t d; |
kenjiArai | 0:4196bd8bb88f | 458 | |
kenjiArai | 0:4196bd8bb88f | 459 | dt[0] = addr; |
kenjiArai | 0:4196bd8bb88f | 460 | dt[1] = data; |
kenjiArai | 0:4196bd8bb88f | 461 | _i2c.write(chip_addr, dt, 2, false); |
kenjiArai | 0:4196bd8bb88f | 462 | d = dt[0]; |
kenjiArai | 0:4196bd8bb88f | 463 | return d; |
kenjiArai | 0:4196bd8bb88f | 464 | } |