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@0:4196bd8bb88f, 2018-11-23 (annotated)

Committer:
kenjiArai
Date:
Fri Nov 23 00:51:14 2018 +0000
Revision:
0:4196bd8bb88f
Child:
1:0d05e7c9ff4c
1st release

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 0:4196bd8bb88f 6 * Copyright (c) 2018 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 0:4196bd8bb88f 10 * Revised: October 26th, 2018
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 0:4196bd8bb88f 198 //printf("ACC+GYR+MAG are ready!\r\n");
kenjiArai 0:4196bd8bb88f 199 }
kenjiArai 0:4196bd8bb88f 200 // Set initial data
kenjiArai 0:4196bd8bb88f 201 set_parameters_to_regs();
kenjiArai 0:4196bd8bb88f 202 }
kenjiArai 0:4196bd8bb88f 203
kenjiArai 0:4196bd8bb88f 204 ////// Set initialize data to related registers ///////////
kenjiArai 0:4196bd8bb88f 205 void BMX055::set_parameters_to_regs(void)
kenjiArai 0:4196bd8bb88f 206 {
kenjiArai 0:4196bd8bb88f 207 // ACC
kenjiArai 0:4196bd8bb88f 208 chip_addr = inf_addr.acc_addr;
kenjiArai 0:4196bd8bb88f 209 dt[0] = 0x0f; // Select PMU_Range register
kenjiArai 0:4196bd8bb88f 210 dt[1] = bmx055_parameters.acc_fs;
kenjiArai 0:4196bd8bb88f 211 _i2c.write(chip_addr, dt, 2, false);
kenjiArai 0:4196bd8bb88f 212 wait_ms(1);
kenjiArai 0:4196bd8bb88f 213 dt[0] = 0x10; // Select PMU_BW register
kenjiArai 0:4196bd8bb88f 214 dt[1] = bmx055_parameters.acc_bw;
kenjiArai 0:4196bd8bb88f 215 _i2c.write(chip_addr, dt, 2, false);
kenjiArai 0:4196bd8bb88f 216 wait_ms(1);
kenjiArai 0:4196bd8bb88f 217 dt[0] = 0x11; // Select PMU_LPW register
kenjiArai 0:4196bd8bb88f 218 dt[1] = 0x00; // Normal mode, Sleep duration = 0.5ms
kenjiArai 0:4196bd8bb88f 219 _i2c.write(chip_addr, dt, 2, false);
kenjiArai 0:4196bd8bb88f 220 // GYR
kenjiArai 0:4196bd8bb88f 221 chip_addr = inf_addr.gyr_addr;
kenjiArai 0:4196bd8bb88f 222 dt[0] = 0x0f; // Select Range register
kenjiArai 0:4196bd8bb88f 223 dt[1] = bmx055_parameters.gyr_fs;
kenjiArai 0:4196bd8bb88f 224 _i2c.write(chip_addr, dt, 2, false);
kenjiArai 0:4196bd8bb88f 225 wait_ms(1);
kenjiArai 0:4196bd8bb88f 226 dt[0] = 0x10; // Select Bandwidth register
kenjiArai 0:4196bd8bb88f 227 dt[1] = bmx055_parameters.gyr_bw;
kenjiArai 0:4196bd8bb88f 228 _i2c.write(chip_addr, dt, 2, false);
kenjiArai 0:4196bd8bb88f 229 wait_ms(1);
kenjiArai 0:4196bd8bb88f 230 dt[0] = 0x11; // Select LPM1 register
kenjiArai 0:4196bd8bb88f 231 dt[1] = 0x00; // Normal mode, Sleep duration = 2ms
kenjiArai 0:4196bd8bb88f 232 _i2c.write(chip_addr, dt, 2, false);
kenjiArai 0:4196bd8bb88f 233 // MAG
kenjiArai 0:4196bd8bb88f 234 chip_addr = inf_addr.mag_addr;
kenjiArai 0:4196bd8bb88f 235 dt[0] = 0x4b; // Select Mag register
kenjiArai 0:4196bd8bb88f 236 dt[1] = 0x83; // Soft reset
kenjiArai 0:4196bd8bb88f 237 _i2c.write(chip_addr, dt, 2, false);
kenjiArai 0:4196bd8bb88f 238 wait_ms(10);
kenjiArai 0:4196bd8bb88f 239 dt[0] = 0x4b; // Select Mag register
kenjiArai 0:4196bd8bb88f 240 dt[1] = 0x01; // Soft reset
kenjiArai 0:4196bd8bb88f 241 _i2c.write(chip_addr, dt, 2, false);
kenjiArai 0:4196bd8bb88f 242 wait_ms(10);
kenjiArai 0:4196bd8bb88f 243 dt[0] = 0x4c; // Select Mag register
kenjiArai 0:4196bd8bb88f 244 dt[1] = bmx055_parameters.mag_odr;
kenjiArai 0:4196bd8bb88f 245 _i2c.write(chip_addr, dt, 2, false);
kenjiArai 0:4196bd8bb88f 246 wait_ms(1);
kenjiArai 0:4196bd8bb88f 247 dt[0] = 0x4e; // Select Mag register
kenjiArai 0:4196bd8bb88f 248 dt[1] = 0x84; // X, Y, Z-Axis enabled
kenjiArai 0:4196bd8bb88f 249 _i2c.write(chip_addr, dt, 2, false);
kenjiArai 0:4196bd8bb88f 250 wait_ms(1);
kenjiArai 0:4196bd8bb88f 251 dt[0] = 0x51; // Select Mag register
kenjiArai 0:4196bd8bb88f 252 dt[1] = 0x04; // No. of Repetitions for X-Y Axis = 9
kenjiArai 0:4196bd8bb88f 253 _i2c.write(chip_addr, dt, 2, false);
kenjiArai 0:4196bd8bb88f 254 wait_ms(1);
kenjiArai 0:4196bd8bb88f 255 dt[0] = 0x52; // Select Mag register
kenjiArai 0:4196bd8bb88f 256 dt[1] = 0x16; // No. of Repetitions for Z-Axis = 15
kenjiArai 0:4196bd8bb88f 257 _i2c.write(chip_addr, dt, 2, false);
kenjiArai 0:4196bd8bb88f 258 #if 0
kenjiArai 0:4196bd8bb88f 259 // ACC
kenjiArai 0:4196bd8bb88f 260 chip_addr = inf_addr.acc_addr;
kenjiArai 0:4196bd8bb88f 261 dt[0] = 0x0f; // Select PMU_Range register
kenjiArai 0:4196bd8bb88f 262 dt[1] = 0x03; // Range = +/- 2g
kenjiArai 0:4196bd8bb88f 263 _i2c.write(chip_addr, dt, 2, false);
kenjiArai 0:4196bd8bb88f 264 wait_ms(1);
kenjiArai 0:4196bd8bb88f 265 dt[0] = 0x10; // Select PMU_BW register
kenjiArai 0:4196bd8bb88f 266 dt[1] = 0x08; // Bandwidth = 7.81 Hz
kenjiArai 0:4196bd8bb88f 267 _i2c.write(chip_addr, dt, 2, false);
kenjiArai 0:4196bd8bb88f 268 wait_ms(1);
kenjiArai 0:4196bd8bb88f 269 dt[0] = 0x11; // Select PMU_LPW register
kenjiArai 0:4196bd8bb88f 270 dt[1] = 0x00; // Normal mode, Sleep duration = 0.5ms
kenjiArai 0:4196bd8bb88f 271 _i2c.write(chip_addr, dt, 2, false);
kenjiArai 0:4196bd8bb88f 272 wait_ms(1);
kenjiArai 0:4196bd8bb88f 273 // GYR
kenjiArai 0:4196bd8bb88f 274 chip_addr = inf_addr.gyr_addr;
kenjiArai 0:4196bd8bb88f 275 dt[0] = 0x0f; // Select Range register
kenjiArai 0:4196bd8bb88f 276 dt[1] = 0x04; // Full scale = +/- 125 degree/s
kenjiArai 0:4196bd8bb88f 277 _i2c.write(chip_addr, dt, 2, false);
kenjiArai 0:4196bd8bb88f 278 wait_ms(1);
kenjiArai 0:4196bd8bb88f 279 dt[0] = 0x10; // Select Bandwidth register
kenjiArai 0:4196bd8bb88f 280 dt[1] = 0x07; // ODR = 100 Hz
kenjiArai 0:4196bd8bb88f 281 _i2c.write(chip_addr, dt, 2, false);
kenjiArai 0:4196bd8bb88f 282 wait_ms(1);
kenjiArai 0:4196bd8bb88f 283 dt[0] = 0x11; // Select LPM1 register
kenjiArai 0:4196bd8bb88f 284 dt[1] = 0x00; // Normal mode, Sleep duration = 2ms
kenjiArai 0:4196bd8bb88f 285 _i2c.write(chip_addr, dt, 2, false);
kenjiArai 0:4196bd8bb88f 286 // MAG
kenjiArai 0:4196bd8bb88f 287 chip_addr = inf_addr.mag_addr;
kenjiArai 0:4196bd8bb88f 288 dt[0] = 0x4b; // Select Mag register
kenjiArai 0:4196bd8bb88f 289 dt[1] = 0x83; // Soft reset
kenjiArai 0:4196bd8bb88f 290 _i2c.write(chip_addr, dt, 2, false);
kenjiArai 0:4196bd8bb88f 291 wait_ms(10);
kenjiArai 0:4196bd8bb88f 292 dt[0] = 0x4b; // Select Mag register
kenjiArai 0:4196bd8bb88f 293 dt[1] = 0x01; // Soft reset
kenjiArai 0:4196bd8bb88f 294 _i2c.write(chip_addr, dt, 2, false);
kenjiArai 0:4196bd8bb88f 295 wait_ms(10);
kenjiArai 0:4196bd8bb88f 296 dt[0] = 0x4c; // Select Mag register
kenjiArai 0:4196bd8bb88f 297 dt[1] = 0x00; // Normal Mode, ODR = 10 Hz
kenjiArai 0:4196bd8bb88f 298 _i2c.write(chip_addr, dt, 2, false);
kenjiArai 0:4196bd8bb88f 299 wait_ms(1);
kenjiArai 0:4196bd8bb88f 300 dt[0] = 0x4e; // Select Mag register
kenjiArai 0:4196bd8bb88f 301 dt[1] = 0x84; // X, Y, Z-Axis enabled
kenjiArai 0:4196bd8bb88f 302 _i2c.write(chip_addr, dt, 2, false);
kenjiArai 0:4196bd8bb88f 303 wait_ms(1);
kenjiArai 0:4196bd8bb88f 304 dt[0] = 0x51; // Select Mag register
kenjiArai 0:4196bd8bb88f 305 dt[1] = 0x04; // No. of Repetitions for X-Y Axis = 9
kenjiArai 0:4196bd8bb88f 306 _i2c.write(chip_addr, dt, 2, false);
kenjiArai 0:4196bd8bb88f 307 wait_ms(1);
kenjiArai 0:4196bd8bb88f 308 dt[0] = 0x52; // Select Mag register
kenjiArai 0:4196bd8bb88f 309 dt[1] = 0x16; // No. of Repetitions for Z-Axis = 15
kenjiArai 0:4196bd8bb88f 310 _i2c.write(chip_addr, dt, 2, false);
kenjiArai 0:4196bd8bb88f 311 wait_ms(1);
kenjiArai 0:4196bd8bb88f 312 #endif
kenjiArai 0:4196bd8bb88f 313 }
kenjiArai 0:4196bd8bb88f 314
kenjiArai 0:4196bd8bb88f 315 /////////////// Check Who am I? ///////////////////////////
kenjiArai 0:4196bd8bb88f 316 void BMX055::check_id(void)
kenjiArai 0:4196bd8bb88f 317 {
kenjiArai 0:4196bd8bb88f 318 ready_flag = 0;
kenjiArai 0:4196bd8bb88f 319 // ID ACC
kenjiArai 0:4196bd8bb88f 320 inf_addr.acc_addr = BMX055_ACC_CHIP_ADDR;
kenjiArai 0:4196bd8bb88f 321 chip_addr = inf_addr.acc_addr;
kenjiArai 0:4196bd8bb88f 322 dt[0] = 0x00; // chip ID reg addr
kenjiArai 0:4196bd8bb88f 323 _i2c.write(chip_addr, dt, 1, true);
kenjiArai 0:4196bd8bb88f 324 _i2c.read(chip_addr, dt, 1, false);
kenjiArai 0:4196bd8bb88f 325 inf_id.acc_id = dt[0];
kenjiArai 0:4196bd8bb88f 326 if (inf_id.acc_id == I_AM_BMX055_ACC) {
kenjiArai 0:4196bd8bb88f 327 ready_flag |= 0x01;
kenjiArai 0:4196bd8bb88f 328 } else {
kenjiArai 0:4196bd8bb88f 329 inf_addr.acc_addr = (0x18 << 1);
kenjiArai 0:4196bd8bb88f 330 chip_addr = inf_addr.acc_addr;
kenjiArai 0:4196bd8bb88f 331 dt[0] = 0x00; // chip ID reg addr
kenjiArai 0:4196bd8bb88f 332 _i2c.write(chip_addr, dt, 1, true);
kenjiArai 0:4196bd8bb88f 333 _i2c.read(chip_addr, dt, 1, false);
kenjiArai 0:4196bd8bb88f 334 inf_id.acc_id = dt[0];
kenjiArai 0:4196bd8bb88f 335 if (inf_id.acc_id == I_AM_BMX055_ACC) {
kenjiArai 0:4196bd8bb88f 336 ready_flag |= 0x01;
kenjiArai 0:4196bd8bb88f 337 }
kenjiArai 0:4196bd8bb88f 338 }
kenjiArai 0:4196bd8bb88f 339 // ID GYRO
kenjiArai 0:4196bd8bb88f 340 inf_addr.gyr_addr = BMX055_GYR_CHIP_ADDR;
kenjiArai 0:4196bd8bb88f 341 chip_addr = inf_addr.gyr_addr;
kenjiArai 0:4196bd8bb88f 342 dt[0] = 0x00; // chip ID reg addr
kenjiArai 0:4196bd8bb88f 343 _i2c.write(chip_addr, dt, 1, true);
kenjiArai 0:4196bd8bb88f 344 _i2c.read(chip_addr, dt, 1, false);
kenjiArai 0:4196bd8bb88f 345 inf_id.gyr_id = dt[0];
kenjiArai 0:4196bd8bb88f 346 if (inf_id.gyr_id == I_AM_BMX055_GYR) {
kenjiArai 0:4196bd8bb88f 347 ready_flag |= 0x02;
kenjiArai 0:4196bd8bb88f 348 } else {
kenjiArai 0:4196bd8bb88f 349 inf_addr.gyr_addr = (0x68 << 1);
kenjiArai 0:4196bd8bb88f 350 chip_addr = inf_addr.gyr_addr;
kenjiArai 0:4196bd8bb88f 351 dt[0] = 0x00; // chip ID reg addr
kenjiArai 0:4196bd8bb88f 352 _i2c.write(chip_addr, dt, 1, true);
kenjiArai 0:4196bd8bb88f 353 _i2c.read(chip_addr, dt, 1, false);
kenjiArai 0:4196bd8bb88f 354 inf_id.gyr_id = dt[0];
kenjiArai 0:4196bd8bb88f 355 if (inf_id.gyr_id == I_AM_BMX055_GYR) {
kenjiArai 0:4196bd8bb88f 356 ready_flag |= 0x02;
kenjiArai 0:4196bd8bb88f 357 }
kenjiArai 0:4196bd8bb88f 358 }
kenjiArai 0:4196bd8bb88f 359 // ID Mag
kenjiArai 0:4196bd8bb88f 360 inf_addr.mag_addr = BMX055_MAG_CHIP_ADDR;
kenjiArai 0:4196bd8bb88f 361 chip_addr = inf_addr.mag_addr;
kenjiArai 0:4196bd8bb88f 362 dt[0] = 0x4b; // reg addr
kenjiArai 0:4196bd8bb88f 363 dt[1] = 0x01; // control power bit set 1
kenjiArai 0:4196bd8bb88f 364 _i2c.write(chip_addr, dt, 2, false);
kenjiArai 0:4196bd8bb88f 365 dt[0] = 0x40; // chip ID reg addr
kenjiArai 0:4196bd8bb88f 366 _i2c.write(chip_addr, dt, 1, true);
kenjiArai 0:4196bd8bb88f 367 _i2c.read(chip_addr, dt, 1, false);
kenjiArai 0:4196bd8bb88f 368 inf_id.mag_id = dt[0];
kenjiArai 0:4196bd8bb88f 369 if (inf_id.mag_id == I_AM_BMX055_MAG) {
kenjiArai 0:4196bd8bb88f 370 ready_flag |= 0x04;
kenjiArai 0:4196bd8bb88f 371 } else {
kenjiArai 0:4196bd8bb88f 372 inf_addr.mag_addr = (0x12 << 1);
kenjiArai 0:4196bd8bb88f 373 chip_addr = inf_addr.mag_addr;
kenjiArai 0:4196bd8bb88f 374 // control power bit set 1
kenjiArai 0:4196bd8bb88f 375 dt[0] = 0x4b;
kenjiArai 0:4196bd8bb88f 376 dt[1] = 0x01;
kenjiArai 0:4196bd8bb88f 377 _i2c.write(chip_addr, dt, 2, false);
kenjiArai 0:4196bd8bb88f 378 dt[0] = 0x40;
kenjiArai 0:4196bd8bb88f 379 _i2c.write(chip_addr, dt, 1, true);
kenjiArai 0:4196bd8bb88f 380 _i2c.read(chip_addr, dt, 1, false);
kenjiArai 0:4196bd8bb88f 381 inf_id.mag_id = dt[0];
kenjiArai 0:4196bd8bb88f 382 if (inf_id.mag_id == I_AM_BMX055_MAG) {
kenjiArai 0:4196bd8bb88f 383 ready_flag |= 0x04;
kenjiArai 0:4196bd8bb88f 384 } else {
kenjiArai 0:4196bd8bb88f 385 inf_addr.mag_addr = (0x11 << 1);
kenjiArai 0:4196bd8bb88f 386 chip_addr = inf_addr.mag_addr;
kenjiArai 0:4196bd8bb88f 387 // control power bit set 1
kenjiArai 0:4196bd8bb88f 388 dt[0] = 0x4b;
kenjiArai 0:4196bd8bb88f 389 dt[1] = 0x01;
kenjiArai 0:4196bd8bb88f 390 _i2c.write(chip_addr, dt, 2, false);
kenjiArai 0:4196bd8bb88f 391 dt[0] = 0x40;
kenjiArai 0:4196bd8bb88f 392 _i2c.write(chip_addr, dt, 1, true);
kenjiArai 0:4196bd8bb88f 393 _i2c.read(chip_addr, dt, 1, false);
kenjiArai 0:4196bd8bb88f 394 inf_id.mag_id = dt[0];
kenjiArai 0:4196bd8bb88f 395 if (inf_id.mag_id == I_AM_BMX055_MAG) {
kenjiArai 0:4196bd8bb88f 396 ready_flag |= 0x04;
kenjiArai 0:4196bd8bb88f 397 } else {
kenjiArai 0:4196bd8bb88f 398 inf_addr.mag_addr = (0x10 << 1);
kenjiArai 0:4196bd8bb88f 399 chip_addr = inf_addr.mag_addr;
kenjiArai 0:4196bd8bb88f 400 // control power bit set 1
kenjiArai 0:4196bd8bb88f 401 dt[0] = 0x4b;
kenjiArai 0:4196bd8bb88f 402 dt[1] = 0x01;
kenjiArai 0:4196bd8bb88f 403 _i2c.write(chip_addr, dt, 2, false);
kenjiArai 0:4196bd8bb88f 404 dt[0] = 0x40;
kenjiArai 0:4196bd8bb88f 405 _i2c.write(chip_addr, dt, 1, true);
kenjiArai 0:4196bd8bb88f 406 _i2c.read(chip_addr, dt, 1, false);
kenjiArai 0:4196bd8bb88f 407 inf_id.mag_id = dt[0];
kenjiArai 0:4196bd8bb88f 408 if (inf_id.mag_id == I_AM_BMX055_MAG) {
kenjiArai 0:4196bd8bb88f 409 ready_flag |= 0x04;
kenjiArai 0:4196bd8bb88f 410 }
kenjiArai 0:4196bd8bb88f 411 }
kenjiArai 0:4196bd8bb88f 412 }
kenjiArai 0:4196bd8bb88f 413 }
kenjiArai 0:4196bd8bb88f 414 //printf("ACC addr=0x%02x, id=0x%02x\r\n", chip_addr, inf_id.acc_id);
kenjiArai 0:4196bd8bb88f 415 //printf("GYR addr=0x%02x, id=0x%02x\r\n", chip_addr, inf_id.gyr_id);
kenjiArai 0:4196bd8bb88f 416 //printf("MAG addr=0x%02x, id=0x%02x\r\n", chip_addr, inf_id.mag_id);
kenjiArai 0:4196bd8bb88f 417 //printf("ready_flag = 0x%x\r\n", ready_flag);
kenjiArai 0:4196bd8bb88f 418 }
kenjiArai 0:4196bd8bb88f 419
kenjiArai 0:4196bd8bb88f 420 void BMX055::read_id_inf(BMX055_ID_INF_TypeDef *id)
kenjiArai 0:4196bd8bb88f 421 {
kenjiArai 0:4196bd8bb88f 422 id->acc_id = acc_id;
kenjiArai 0:4196bd8bb88f 423 id->mag_id = mag_id;
kenjiArai 0:4196bd8bb88f 424 id->gyr_id = gyr_id;
kenjiArai 0:4196bd8bb88f 425 }
kenjiArai 0:4196bd8bb88f 426
kenjiArai 0:4196bd8bb88f 427 /////////////// Check chip ready or not //////////////////
kenjiArai 0:4196bd8bb88f 428 bool BMX055::chip_ready(void)
kenjiArai 0:4196bd8bb88f 429 {
kenjiArai 0:4196bd8bb88f 430 if (ready_flag == 0x07) {
kenjiArai 0:4196bd8bb88f 431 return true;
kenjiArai 0:4196bd8bb88f 432 }
kenjiArai 0:4196bd8bb88f 433 return false;
kenjiArai 0:4196bd8bb88f 434 }
kenjiArai 0:4196bd8bb88f 435
kenjiArai 0:4196bd8bb88f 436 /////////////// I2C Freq. /////////////////////////////////
kenjiArai 0:4196bd8bb88f 437 void BMX055::frequency(int hz)
kenjiArai 0:4196bd8bb88f 438 {
kenjiArai 0:4196bd8bb88f 439 _i2c.frequency(hz);
kenjiArai 0:4196bd8bb88f 440 }
kenjiArai 0:4196bd8bb88f 441
kenjiArai 0:4196bd8bb88f 442 /////////////// Read/Write specific register //////////////
kenjiArai 0:4196bd8bb88f 443 uint8_t BMX055::read_reg(uint8_t addr)
kenjiArai 0:4196bd8bb88f 444 {
kenjiArai 0:4196bd8bb88f 445 dt[0] = addr;
kenjiArai 0:4196bd8bb88f 446 _i2c.write(chip_addr, dt, 1, true);
kenjiArai 0:4196bd8bb88f 447 _i2c.read(chip_addr, dt, 1, false);
kenjiArai 0:4196bd8bb88f 448 return (uint8_t)dt[0];
kenjiArai 0:4196bd8bb88f 449 }
kenjiArai 0:4196bd8bb88f 450
kenjiArai 0:4196bd8bb88f 451 uint8_t BMX055::write_reg(uint8_t addr, uint8_t data)
kenjiArai 0:4196bd8bb88f 452 {
kenjiArai 0:4196bd8bb88f 453 uint8_t d;
kenjiArai 0:4196bd8bb88f 454
kenjiArai 0:4196bd8bb88f 455 dt[0] = addr;
kenjiArai 0:4196bd8bb88f 456 dt[1] = data;
kenjiArai 0:4196bd8bb88f 457 _i2c.write(chip_addr, dt, 2, false);
kenjiArai 0:4196bd8bb88f 458 d = dt[0];
kenjiArai 0:4196bd8bb88f 459 return d;
kenjiArai 0:4196bd8bb88f 460 }