altb_pmic / AHRS

AHRS

Dependencies:   Eigen

Dependents:   IndNav_QK3_T265

BMX055.cpp@26:1da7c6204775, 2020-01-10 (annotated)

Committer:
altb2
Date:
Fri Jan 10 16:00:47 2020 +0000
Revision:
26:1da7c6204775
Parent:
25:fe14dbcef82d
Child:
28:21dfb161c67c
recalib MAgnetometer

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