altb_pmic / AHRS

AHRS

Dependencies:   Eigen

Dependents:   IndNav_QK3_T265

BMX055.cpp@29:cd963a6d31c5, 2020-01-27 (annotated)

Committer:
pmic
Date:
Mon Jan 27 09:23:58 2020 +0000
Revision:
29:cd963a6d31c5
Parent:
28:21dfb161c67c
Child:
30:9b0cd3caf0ec
Correct magnetometer frame transformation. Adjust BMI088 acc filter parametrization. Correct air drag coefficient for QK4.

Who changed what in which revision?

UserRevisionLine numberNew 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);
pmic 29:cd963a6d31c5 55 x = dt[1] << 8 | (dt[0] & 0xf0); // values have to be divided by 16
altb2 25:fe14dbcef82d 56 y = dt[3] << 8 | (dt[2] & 0xf0);
altb2 25:fe14dbcef82d 57 z = dt[5] << 8 | (dt[4] & 0xf0);
altb2 28:21dfb161c67c 58 accX = (float)x * acc_factor;
altb2 28:21dfb161c67c 59 accY = (float)y * acc_factor;
altb2 28:21dfb161c67c 60 accZ = (float)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 28:21dfb161c67c 75 gyroX = (float)x * gyr_factor;
altb2 28:21dfb161c67c 76 gyroY = (float)y * gyr_factor;
altb2 28:21dfb161c67c 77 gyroZ = (float)z * gyr_factor;
altb2 25:fe14dbcef82d 78 }
altb2 25:fe14dbcef82d 79
altb2 25:fe14dbcef82d 80 void BMX055::readMag(void)
altb2 25:fe14dbcef82d 81 {
pmic 29:cd963a6d31c5 82 int16_t x,y,z;
pmic 29:cd963a6d31c5 83
altb2 25:fe14dbcef82d 84 chip_addr = inf_addr.mag_addr;
altb2 25:fe14dbcef82d 85 dt[0] = 0x42;
altb2 25:fe14dbcef82d 86 i2c.write(chip_addr, dt, 1, true);
altb2 25:fe14dbcef82d 87 i2c.read(chip_addr, dt, 6, false);
altb2 25:fe14dbcef82d 88
pmic 29:cd963a6d31c5 89 // corrected, pmic 21.01.2020
pmic 29:cd963a6d31c5 90 x = dt[1] << 8 | (dt[0] & 0xf8); // value has to be divided by 8
pmic 29:cd963a6d31c5 91 y = dt[3] << 8 | (dt[2] & 0xf8); // value has to be divided by 8
pmic 29:cd963a6d31c5 92 z = dt[5] << 8 | (dt[4] & 0xfe); // value has to be divided by 2
pmic 29:cd963a6d31c5 93
pmic 29:cd963a6d31c5 94 // scaling factor 1/301.5815f if experimentally validated in comparision to pes board -> approx. same scaling of mag values, pmic 21.01.2020
pmic 29:cd963a6d31c5 95 magX = -(float)y / 8.0f / 301.5815f; // MAGNETOMETER x-y are switched, see BMX055 datasheet page 161, also measurements show change in sign x!!!
pmic 29:cd963a6d31c5 96 magY = (float)x / 8.0f / 301.5815f; // x-y is switched
pmic 29:cd963a6d31c5 97 magZ = -(float)z / 2.0f / 301.5815f; // z is -z
altb2 25:fe14dbcef82d 98 }
altb2 25:fe14dbcef82d 99
altb2 25:fe14dbcef82d 100 float BMX055::get_chip_temperature()
altb2 25:fe14dbcef82d 101 {
altb2 25:fe14dbcef82d 102 chip_addr = inf_addr.acc_addr;
altb2 25:fe14dbcef82d 103 dt[0] = 0x08; // chip tempareture reg addr
altb2 25:fe14dbcef82d 104 i2c.write(chip_addr, dt, 1, true);
altb2 25:fe14dbcef82d 105 i2c.read(chip_addr, dt, 1, false);
altb2 25:fe14dbcef82d 106 //printf("Temp reg = 0x%02x\r\n", dt[0]);
altb2 25:fe14dbcef82d 107 return (float)((int8_t)dt[0]) * 0.5f + 23.0f;
altb2 25:fe14dbcef82d 108 }
altb2 25:fe14dbcef82d 109
altb2 25:fe14dbcef82d 110 /////////////// Initialize ////////////////////////////////
altb2 25:fe14dbcef82d 111 void BMX055::initialize (void)
altb2 25:fe14dbcef82d 112 {
altb2 25:fe14dbcef82d 113 i2c.frequency(400000);
altb2 25:fe14dbcef82d 114 // Check Acc & Mag & Gyro are available of not
altb2 25:fe14dbcef82d 115 check_id();
altb2 25:fe14dbcef82d 116 if (ready_flag == 0x07){
altb2 25:fe14dbcef82d 117 }
altb2 25:fe14dbcef82d 118 // Set initial data
altb2 25:fe14dbcef82d 119 set_parameters_to_regs();
altb2 25:fe14dbcef82d 120 switch(bmx055_parameters.gyr_fs){
altb2 25:fe14dbcef82d 121 case GYR_2000DPS:
altb2 25:fe14dbcef82d 122 gyr_factor = 2000.0f*0.01745329f / 32768.0f;
altb2 25:fe14dbcef82d 123 break;
altb2 25:fe14dbcef82d 124 case GYR_1000DPS:
altb2 25:fe14dbcef82d 125 gyr_factor = 1000.0f*0.01745329f / 32768.0f;
altb2 25:fe14dbcef82d 126 break;
altb2 25:fe14dbcef82d 127 case GYR_500DPS:
altb2 25:fe14dbcef82d 128 gyr_factor = 500.0f*0.01745329f / 32768.0f;
altb2 25:fe14dbcef82d 129 break;
altb2 25:fe14dbcef82d 130 case GYR_250DPS:
altb2 25:fe14dbcef82d 131 gyr_factor = 250.0f*0.01745329f / 32768.0f;
altb2 25:fe14dbcef82d 132 break;
altb2 25:fe14dbcef82d 133 case GYR_125DPS:
altb2 25:fe14dbcef82d 134 gyr_factor = 125.0f*0.01745329f / 32768.0f;
altb2 25:fe14dbcef82d 135 break;
altb2 25:fe14dbcef82d 136 default:
altb2 25:fe14dbcef82d 137 gyr_factor = 0;
altb2 25:fe14dbcef82d 138 break;
altb2 25:fe14dbcef82d 139 }
altb2 25:fe14dbcef82d 140 switch(bmx055_parameters.acc_fs){
altb2 25:fe14dbcef82d 141 case ACC_2G:
altb2 25:fe14dbcef82d 142 acc_factor = 2.0f * 9.81f / 2048.0f / 16.0f;
altb2 25:fe14dbcef82d 143 break;
altb2 25:fe14dbcef82d 144 case ACC_4G:
altb2 25:fe14dbcef82d 145 acc_factor = 4.0f * 9.81f / 2048.0f / 16.0f;
altb2 25:fe14dbcef82d 146 break;
altb2 25:fe14dbcef82d 147 case ACC_8G:
altb2 25:fe14dbcef82d 148 acc_factor = 8.0f * 9.81f / 2048.0f / 16.0f;
altb2 25:fe14dbcef82d 149 break;
altb2 25:fe14dbcef82d 150 case ACC_16G:
altb2 25:fe14dbcef82d 151 acc_factor = 16.0f * 9.81f / 2048.0f / 16.0f;
altb2 25:fe14dbcef82d 152 break;
altb2 25:fe14dbcef82d 153 default:
altb2 25:fe14dbcef82d 154 acc_factor = 0;
altb2 25:fe14dbcef82d 155 break;
altb2 25:fe14dbcef82d 156 }
altb2 25:fe14dbcef82d 157
altb2 25:fe14dbcef82d 158
altb2 25:fe14dbcef82d 159 }
altb2 25:fe14dbcef82d 160
altb2 25:fe14dbcef82d 161 ////// Set initialize data to related registers ///////////
altb2 25:fe14dbcef82d 162 void BMX055::set_parameters_to_regs(void)
altb2 25:fe14dbcef82d 163 {
altb2 25:fe14dbcef82d 164 // ACC
altb2 25:fe14dbcef82d 165 chip_addr = inf_addr.acc_addr;
altb2 25:fe14dbcef82d 166 dt[0] = 0x0f; // Select PMU_Range register
altb2 25:fe14dbcef82d 167 dt[1] = bmx055_parameters.acc_fs;
altb2 25:fe14dbcef82d 168 i2c.write(chip_addr, dt, 2, false);
altb2 25:fe14dbcef82d 169 wait_ms(1);
altb2 25:fe14dbcef82d 170 dt[0] = 0x10; // Select PMU_BW register
altb2 25:fe14dbcef82d 171 dt[1] = bmx055_parameters.acc_bw;
altb2 25:fe14dbcef82d 172 i2c.write(chip_addr, dt, 2, false);
altb2 25:fe14dbcef82d 173 wait_ms(1);
altb2 25:fe14dbcef82d 174 dt[0] = 0x11; // Select PMU_LPW register
altb2 25:fe14dbcef82d 175 dt[1] = 0x00; // Normal mode, Sleep duration = 0.5ms
altb2 25:fe14dbcef82d 176 i2c.write(chip_addr, dt, 2, false);
altb2 25:fe14dbcef82d 177 // GYR
altb2 25:fe14dbcef82d 178 chip_addr = inf_addr.gyr_addr;
altb2 25:fe14dbcef82d 179 dt[0] = 0x0f; // Select Range register
altb2 25:fe14dbcef82d 180 dt[1] = bmx055_parameters.gyr_fs;
altb2 25:fe14dbcef82d 181 i2c.write(chip_addr, dt, 2, false);
altb2 25:fe14dbcef82d 182 wait_ms(1);
altb2 25:fe14dbcef82d 183 dt[0] = 0x10; // Select Bandwidth register
altb2 25:fe14dbcef82d 184 dt[1] = bmx055_parameters.gyr_bw;
altb2 25:fe14dbcef82d 185 i2c.write(chip_addr, dt, 2, false);
altb2 25:fe14dbcef82d 186 wait_ms(1);
altb2 25:fe14dbcef82d 187 dt[0] = 0x11; // Select LPM1 register
altb2 25:fe14dbcef82d 188 dt[1] = 0x00; // Normal mode, Sleep duration = 2ms
altb2 25:fe14dbcef82d 189 i2c.write(chip_addr, dt, 2, false);
altb2 25:fe14dbcef82d 190 // MAG
altb2 25:fe14dbcef82d 191 chip_addr = inf_addr.mag_addr;
altb2 25:fe14dbcef82d 192 dt[0] = 0x4b; // Select Mag register
pmic 29:cd963a6d31c5 193 dt[1] = 0x83; // Soft reset 10000011
altb2 25:fe14dbcef82d 194 i2c.write(chip_addr, dt, 2, false);
altb2 25:fe14dbcef82d 195 wait_ms(10);
altb2 25:fe14dbcef82d 196 dt[0] = 0x4b; // Select Mag register
pmic 29:cd963a6d31c5 197 dt[1] = 0x01; // Soft reset 00000001
altb2 25:fe14dbcef82d 198 i2c.write(chip_addr, dt, 2, false);
altb2 25:fe14dbcef82d 199 wait_ms(10);
altb2 25:fe14dbcef82d 200 dt[0] = 0x4c; // Select Mag register
pmic 29:cd963a6d31c5 201 dt[1] = bmx055_parameters.mag_odr << 3; // corrected, pmic 20.01.2020
altb2 25:fe14dbcef82d 202 i2c.write(chip_addr, dt, 2, false);
altb2 25:fe14dbcef82d 203 wait_ms(1);
altb2 25:fe14dbcef82d 204 dt[0] = 0x4e; // Select Mag register
altb2 25:fe14dbcef82d 205 dt[1] = 0x84; // X, Y, Z-Axis enabled
altb2 25:fe14dbcef82d 206 i2c.write(chip_addr, dt, 2, false);
altb2 25:fe14dbcef82d 207 wait_ms(1);
altb2 25:fe14dbcef82d 208 dt[0] = 0x51; // Select Mag register
pmic 29:cd963a6d31c5 209 dt[1] = 0x51; // No. of Repetitions for X-Y Axis = 163, fmaxODR = 33.2060 Hz, pmic 21.01.2020
altb2 25:fe14dbcef82d 210 i2c.write(chip_addr, dt, 2, false);
altb2 25:fe14dbcef82d 211 wait_ms(1);
altb2 25:fe14dbcef82d 212 dt[0] = 0x52; // Select Mag register
pmic 29:cd963a6d31c5 213 dt[1] = 0x0a; // No. of Repetitions for Z-Axis = 11, fmaxODR = 33.2060 Hz, pmic 21.01.2020
altb2 25:fe14dbcef82d 214 i2c.write(chip_addr, dt, 2, false);
altb2 25:fe14dbcef82d 215 #if 0
altb2 25:fe14dbcef82d 216 // ACC
altb2 25:fe14dbcef82d 217 chip_addr = inf_addr.acc_addr;
altb2 25:fe14dbcef82d 218 dt[0] = 0x0f; // Select PMU_Range register
altb2 25:fe14dbcef82d 219 dt[1] = 0x03; // Range = +/- 2g
altb2 25:fe14dbcef82d 220 i2c.write(chip_addr, dt, 2, false);
altb2 25:fe14dbcef82d 221 wait_ms(1);
altb2 25:fe14dbcef82d 222 dt[0] = 0x10; // Select PMU_BW register
altb2 25:fe14dbcef82d 223 dt[1] = 0x08; // Bandwidth = 7.81 Hz
altb2 25:fe14dbcef82d 224 i2c.write(chip_addr, dt, 2, false);
altb2 25:fe14dbcef82d 225 wait_ms(1);
altb2 25:fe14dbcef82d 226 dt[0] = 0x11; // Select PMU_LPW register
altb2 25:fe14dbcef82d 227 dt[1] = 0x00; // Normal mode, Sleep duration = 0.5ms
altb2 25:fe14dbcef82d 228 i2c.write(chip_addr, dt, 2, false);
altb2 25:fe14dbcef82d 229 wait_ms(1);
altb2 25:fe14dbcef82d 230 // GYR
altb2 25:fe14dbcef82d 231 chip_addr = inf_addr.gyr_addr;
altb2 25:fe14dbcef82d 232 dt[0] = 0x0f; // Select Range register
altb2 25:fe14dbcef82d 233 dt[1] = 0x04; // Full scale = +/- 125 degree/s
altb2 25:fe14dbcef82d 234 i2c.write(chip_addr, dt, 2, false);
altb2 25:fe14dbcef82d 235 wait_ms(1);
altb2 25:fe14dbcef82d 236 dt[0] = 0x10; // Select Bandwidth register
altb2 25:fe14dbcef82d 237 dt[1] = 0x07; // ODR = 100 Hz
altb2 25:fe14dbcef82d 238 i2c.write(chip_addr, dt, 2, false);
altb2 25:fe14dbcef82d 239 wait_ms(1);
altb2 25:fe14dbcef82d 240 dt[0] = 0x11; // Select LPM1 register
altb2 25:fe14dbcef82d 241 dt[1] = 0x00; // Normal mode, Sleep duration = 2ms
altb2 25:fe14dbcef82d 242 i2c.write(chip_addr, dt, 2, false);
altb2 25:fe14dbcef82d 243 // MAG
altb2 25:fe14dbcef82d 244 chip_addr = inf_addr.mag_addr;
altb2 25:fe14dbcef82d 245 dt[0] = 0x4b; // Select Mag register
altb2 25:fe14dbcef82d 246 dt[1] = 0x83; // Soft reset
altb2 25:fe14dbcef82d 247 i2c.write(chip_addr, dt, 2, false);
altb2 25:fe14dbcef82d 248 wait_ms(10);
altb2 25:fe14dbcef82d 249 dt[0] = 0x4b; // Select Mag register
altb2 25:fe14dbcef82d 250 dt[1] = 0x01; // Soft reset
altb2 25:fe14dbcef82d 251 i2c.write(chip_addr, dt, 2, false);
altb2 25:fe14dbcef82d 252 wait_ms(10);
altb2 25:fe14dbcef82d 253 dt[0] = 0x4c; // Select Mag register
altb2 25:fe14dbcef82d 254 dt[1] = 0x00; // Normal Mode, ODR = 10 Hz
altb2 25:fe14dbcef82d 255 i2c.write(chip_addr, dt, 2, false);
altb2 25:fe14dbcef82d 256 wait_ms(1);
altb2 25:fe14dbcef82d 257 dt[0] = 0x4e; // Select Mag register
altb2 25:fe14dbcef82d 258 dt[1] = 0x84; // X, Y, Z-Axis enabled
altb2 25:fe14dbcef82d 259 i2c.write(chip_addr, dt, 2, false);
altb2 25:fe14dbcef82d 260 wait_ms(1);
altb2 25:fe14dbcef82d 261 dt[0] = 0x51; // Select Mag register
altb2 25:fe14dbcef82d 262 dt[1] = 0x04; // No. of Repetitions for X-Y Axis = 9
altb2 25:fe14dbcef82d 263 i2c.write(chip_addr, dt, 2, false);
altb2 25:fe14dbcef82d 264 wait_ms(1);
altb2 25:fe14dbcef82d 265 dt[0] = 0x52; // Select Mag register
altb2 25:fe14dbcef82d 266 dt[1] = 0x16; // No. of Repetitions for Z-Axis = 15
altb2 25:fe14dbcef82d 267 i2c.write(chip_addr, dt, 2, false);
altb2 25:fe14dbcef82d 268 wait_ms(1);
altb2 25:fe14dbcef82d 269 #endif
altb2 25:fe14dbcef82d 270 }
altb2 25:fe14dbcef82d 271
altb2 25:fe14dbcef82d 272 /////////////// Check Who am I? ///////////////////////////
altb2 25:fe14dbcef82d 273 void BMX055::check_id(void)
altb2 25:fe14dbcef82d 274 {
altb2 25:fe14dbcef82d 275 ready_flag = 0;
altb2 25:fe14dbcef82d 276 // ID ACC
altb2 25:fe14dbcef82d 277 inf_addr.acc_addr = BMX055_ACC_CHIP_ADDR;
altb2 25:fe14dbcef82d 278 chip_addr = inf_addr.acc_addr;
altb2 25:fe14dbcef82d 279 dt[0] = 0x00; // chip ID reg addr
altb2 25:fe14dbcef82d 280 i2c.write(chip_addr, dt, 1, true);
altb2 25:fe14dbcef82d 281 i2c.read(chip_addr, dt, 1, false);
altb2 25:fe14dbcef82d 282 inf_id.acc_id = dt[0];
altb2 25:fe14dbcef82d 283 if (inf_id.acc_id == I_AM_BMX055_ACC) {
altb2 25:fe14dbcef82d 284 ready_flag |= 0x01;
altb2 25:fe14dbcef82d 285 } else {
altb2 25:fe14dbcef82d 286 inf_addr.acc_addr = (0x18 << 1);
altb2 25:fe14dbcef82d 287 chip_addr = inf_addr.acc_addr;
altb2 25:fe14dbcef82d 288 dt[0] = 0x00; // chip ID reg addr
altb2 25:fe14dbcef82d 289 i2c.write(chip_addr, dt, 1, true);
altb2 25:fe14dbcef82d 290 i2c.read(chip_addr, dt, 1, false);
altb2 25:fe14dbcef82d 291 inf_id.acc_id = dt[0];
altb2 25:fe14dbcef82d 292 if (inf_id.acc_id == I_AM_BMX055_ACC) {
altb2 25:fe14dbcef82d 293 ready_flag |= 0x01;
altb2 25:fe14dbcef82d 294 }
altb2 25:fe14dbcef82d 295 }
altb2 25:fe14dbcef82d 296 // ID GYRO
altb2 25:fe14dbcef82d 297 inf_addr.gyr_addr = BMX055_GYR_CHIP_ADDR;
altb2 25:fe14dbcef82d 298 chip_addr = inf_addr.gyr_addr;
altb2 25:fe14dbcef82d 299 dt[0] = 0x00; // chip ID reg addr
altb2 25:fe14dbcef82d 300 i2c.write(chip_addr, dt, 1, true);
altb2 25:fe14dbcef82d 301 i2c.read(chip_addr, dt, 1, false);
altb2 25:fe14dbcef82d 302 inf_id.gyr_id = dt[0];
altb2 25:fe14dbcef82d 303 if (inf_id.gyr_id == I_AM_BMX055_GYR) {
altb2 25:fe14dbcef82d 304 ready_flag |= 0x02;
altb2 25:fe14dbcef82d 305 } else {
altb2 25:fe14dbcef82d 306 inf_addr.gyr_addr = (0x68 << 1);
altb2 25:fe14dbcef82d 307 chip_addr = inf_addr.gyr_addr;
altb2 25:fe14dbcef82d 308 dt[0] = 0x00; // chip ID reg addr
altb2 25:fe14dbcef82d 309 i2c.write(chip_addr, dt, 1, true);
altb2 25:fe14dbcef82d 310 i2c.read(chip_addr, dt, 1, false);
altb2 25:fe14dbcef82d 311 inf_id.gyr_id = dt[0];
altb2 25:fe14dbcef82d 312 if (inf_id.gyr_id == I_AM_BMX055_GYR) {
altb2 25:fe14dbcef82d 313 ready_flag |= 0x02;
altb2 25:fe14dbcef82d 314 }
altb2 25:fe14dbcef82d 315 }
altb2 25:fe14dbcef82d 316 // ID Mag
altb2 25:fe14dbcef82d 317 inf_addr.mag_addr = BMX055_MAG_CHIP_ADDR;
altb2 25:fe14dbcef82d 318 chip_addr = inf_addr.mag_addr;
altb2 25:fe14dbcef82d 319 dt[0] = 0x4b; // reg addr
altb2 25:fe14dbcef82d 320 dt[1] = 0x01; // control power bit set 1
altb2 25:fe14dbcef82d 321 i2c.write(chip_addr, dt, 2, false);
altb2 25:fe14dbcef82d 322 dt[0] = 0x40; // chip ID reg addr
altb2 25:fe14dbcef82d 323 i2c.write(chip_addr, dt, 1, true);
altb2 25:fe14dbcef82d 324 i2c.read(chip_addr, dt, 1, false);
altb2 25:fe14dbcef82d 325 inf_id.mag_id = dt[0];
altb2 25:fe14dbcef82d 326 if (inf_id.mag_id == I_AM_BMX055_MAG) {
altb2 25:fe14dbcef82d 327 ready_flag |= 0x04;
altb2 25:fe14dbcef82d 328 } else {
altb2 25:fe14dbcef82d 329 inf_addr.mag_addr = (0x12 << 1);
altb2 25:fe14dbcef82d 330 chip_addr = inf_addr.mag_addr;
altb2 25:fe14dbcef82d 331 // control power bit set 1
altb2 25:fe14dbcef82d 332 dt[0] = 0x4b;
altb2 25:fe14dbcef82d 333 dt[1] = 0x01;
altb2 25:fe14dbcef82d 334 i2c.write(chip_addr, dt, 2, false);
altb2 25:fe14dbcef82d 335 dt[0] = 0x40;
altb2 25:fe14dbcef82d 336 i2c.write(chip_addr, dt, 1, true);
altb2 25:fe14dbcef82d 337 i2c.read(chip_addr, dt, 1, false);
altb2 25:fe14dbcef82d 338 inf_id.mag_id = dt[0];
altb2 25:fe14dbcef82d 339 if (inf_id.mag_id == I_AM_BMX055_MAG) {
altb2 25:fe14dbcef82d 340 ready_flag |= 0x04;
altb2 25:fe14dbcef82d 341 } else {
altb2 25:fe14dbcef82d 342 inf_addr.mag_addr = (0x11 << 1);
altb2 25:fe14dbcef82d 343 chip_addr = inf_addr.mag_addr;
altb2 25:fe14dbcef82d 344 // control power bit set 1
altb2 25:fe14dbcef82d 345 dt[0] = 0x4b;
altb2 25:fe14dbcef82d 346 dt[1] = 0x01;
altb2 25:fe14dbcef82d 347 i2c.write(chip_addr, dt, 2, false);
altb2 25:fe14dbcef82d 348 dt[0] = 0x40;
altb2 25:fe14dbcef82d 349 i2c.write(chip_addr, dt, 1, true);
altb2 25:fe14dbcef82d 350 i2c.read(chip_addr, dt, 1, false);
altb2 25:fe14dbcef82d 351 inf_id.mag_id = dt[0];
altb2 25:fe14dbcef82d 352 if (inf_id.mag_id == I_AM_BMX055_MAG) {
altb2 25:fe14dbcef82d 353 ready_flag |= 0x04;
altb2 25:fe14dbcef82d 354 } else {
altb2 25:fe14dbcef82d 355 inf_addr.mag_addr = (0x10 << 1);
altb2 25:fe14dbcef82d 356 chip_addr = inf_addr.mag_addr;
altb2 25:fe14dbcef82d 357 // control power bit set 1
altb2 25:fe14dbcef82d 358 dt[0] = 0x4b;
altb2 25:fe14dbcef82d 359 dt[1] = 0x01;
altb2 25:fe14dbcef82d 360 i2c.write(chip_addr, dt, 2, false);
altb2 25:fe14dbcef82d 361 dt[0] = 0x40;
altb2 25:fe14dbcef82d 362 i2c.write(chip_addr, dt, 1, true);
altb2 25:fe14dbcef82d 363 i2c.read(chip_addr, dt, 1, false);
altb2 25:fe14dbcef82d 364 inf_id.mag_id = dt[0];
altb2 25:fe14dbcef82d 365 if (inf_id.mag_id == I_AM_BMX055_MAG) {
altb2 25:fe14dbcef82d 366 ready_flag |= 0x04;
altb2 25:fe14dbcef82d 367 }
altb2 25:fe14dbcef82d 368 }
altb2 25:fe14dbcef82d 369 }
altb2 25:fe14dbcef82d 370 }
altb2 25:fe14dbcef82d 371 #if DEBUG
altb2 25:fe14dbcef82d 372 printf("ACC addr=0x%02x, id=0x%02x\r\n", chip_addr, inf_id.acc_id);
altb2 25:fe14dbcef82d 373 printf("GYR addr=0x%02x, id=0x%02x\r\n", chip_addr, inf_id.gyr_id);
altb2 25:fe14dbcef82d 374 printf("MAG addr=0x%02x, id=0x%02x\r\n", chip_addr, inf_id.mag_id);
altb2 25:fe14dbcef82d 375 printf("ready_flag = 0x%x\r\n", ready_flag);
altb2 25:fe14dbcef82d 376 #endif
altb2 25:fe14dbcef82d 377 }
altb2 25:fe14dbcef82d 378
altb2 25:fe14dbcef82d 379 void BMX055::read_id_inf(BMX055_ID_INF_TypeDef *id)
altb2 25:fe14dbcef82d 380 {
altb2 25:fe14dbcef82d 381 id->acc_id = acc_id;
altb2 25:fe14dbcef82d 382 id->mag_id = mag_id;
altb2 25:fe14dbcef82d 383 id->gyr_id = gyr_id;
altb2 25:fe14dbcef82d 384 }
altb2 25:fe14dbcef82d 385
altb2 25:fe14dbcef82d 386 /////////////// Check chip ready or not //////////////////
altb2 25:fe14dbcef82d 387 bool BMX055::chip_ready(void)
altb2 25:fe14dbcef82d 388 {
altb2 25:fe14dbcef82d 389 if (ready_flag == 0x07) {
altb2 25:fe14dbcef82d 390 return true;
altb2 25:fe14dbcef82d 391 }
altb2 25:fe14dbcef82d 392 return false;
altb2 25:fe14dbcef82d 393 }
altb2 25:fe14dbcef82d 394
altb2 25:fe14dbcef82d 395 /////////////// I2C Freq. /////////////////////////////////
altb2 25:fe14dbcef82d 396 void BMX055::frequency(int hz)
altb2 25:fe14dbcef82d 397 {
altb2 25:fe14dbcef82d 398 i2c.frequency(hz);
altb2 25:fe14dbcef82d 399 }
altb2 25:fe14dbcef82d 400
altb2 25:fe14dbcef82d 401 /////////////// Read/Write specific register //////////////
altb2 25:fe14dbcef82d 402 uint8_t BMX055::read_reg(uint8_t addr)
altb2 25:fe14dbcef82d 403 {
altb2 25:fe14dbcef82d 404 dt[0] = addr;
altb2 25:fe14dbcef82d 405 i2c.write(chip_addr, dt, 1, true);
altb2 25:fe14dbcef82d 406 i2c.read(chip_addr, dt, 1, false);
altb2 25:fe14dbcef82d 407 return (uint8_t)dt[0];
altb2 25:fe14dbcef82d 408 }
altb2 25:fe14dbcef82d 409
altb2 25:fe14dbcef82d 410 uint8_t BMX055::write_reg(uint8_t addr, uint8_t data)
altb2 25:fe14dbcef82d 411 {
altb2 25:fe14dbcef82d 412 uint8_t d;
altb2 25:fe14dbcef82d 413
altb2 25:fe14dbcef82d 414 dt[0] = addr;
altb2 25:fe14dbcef82d 415 dt[1] = data;
altb2 25:fe14dbcef82d 416 i2c.write(chip_addr, dt, 2, false);
altb2 25:fe14dbcef82d 417 d = dt[0];
altb2 25:fe14dbcef82d 418 return d;
altb2 25:fe14dbcef82d 419 }