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@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?

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 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 }