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