Kenji Arai / BMX055

Small, versatile 9-axis sensor module by Bosch Sensortec 3D Accelerometer + 3D Gyroscope + 3D Magnetometer

Dependents:   Pruebas_Flex_IMU_copy BMX055_Madgwick

BMX055.cpp@2:1bea0ef16d84, 2019-03-04 (annotated)

Committer:
kenjiArai
Date:
Mon Mar 04 04:10:59 2019 +0000
Revision:
2:1bea0ef16d84
Parent:
1:0d05e7c9ff4c 
magnetic part -> Bug fix

Who changed what in which revision?

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