Kenji Arai / BMC050

eCompass (6-axes electronic compass) / Electronic Compass with Three-axis Magnetic Field Sensor and Three-axis Accelerometer by Bosch Sensortech

Dependents:   BLE_EddystoneBeacon_w_ACC_TY51822

BMC050.cpp@3:24aa4d5fa7de, 2017-08-23 (annotated)

Committer:
kenjiArai
Date:
Wed Aug 23 09:22:29 2017 +0000
Revision:
3:24aa4d5fa7de
Parent:
2:93141eb80862 
countermeasure for  NonCopyable

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kenjiArai 0:8de5e2fd5c48 1 /*
kenjiArai 0:8de5e2fd5c48 2 * mbed library program
kenjiArai 0:8de5e2fd5c48 3 * BMC050 COMPASS 6 AXIS, made by Bosch Sensortec
kenjiArai 0:8de5e2fd5c48 4 * http://jp.bosch-sensortec.com/content/language1/html/5033.htm
kenjiArai 0:8de5e2fd5c48 5 *
kenjiArai 3:24aa4d5fa7de 6 * Copyright (c) 2014,'16,'17 Kenji Arai / JH1PJL
kenjiArai 0:8de5e2fd5c48 7 * http://www.page.sannet.ne.jp/kenjia/index.html
kenjiArai 0:8de5e2fd5c48 8 * http://mbed.org/users/kenjiArai/
kenjiArai 3:24aa4d5fa7de 9 * Created: July 19th, 2014
kenjiArai 3:24aa4d5fa7de 10 * Revised: August 23rd, 2017
kenjiArai 0:8de5e2fd5c48 11 */
kenjiArai 0:8de5e2fd5c48 12
kenjiArai 0:8de5e2fd5c48 13 #include "BMC050.h"
kenjiArai 0:8de5e2fd5c48 14
kenjiArai 2:93141eb80862 15 BMC050::BMC050 (
kenjiArai 2:93141eb80862 16 PinName p_sda, PinName p_scl,
kenjiArai 2:93141eb80862 17 const BMC050ACC_TypeDef *acc_parameter,
kenjiArai 3:24aa4d5fa7de 18 const BMC050MAG_TypeDef *mag_parameter)
kenjiArai 3:24aa4d5fa7de 19 : _i2c_p(new I2C(p_sda, p_scl)), _i2c(*_i2c_p)
kenjiArai 2:93141eb80862 20 {
kenjiArai 0:8de5e2fd5c48 21 initialize (acc_parameter, mag_parameter);
kenjiArai 0:8de5e2fd5c48 22 }
kenjiArai 0:8de5e2fd5c48 23
kenjiArai 2:93141eb80862 24 BMC050::BMC050 (
kenjiArai 2:93141eb80862 25 I2C& p_i2c,
kenjiArai 2:93141eb80862 26 const BMC050ACC_TypeDef *acc_parameter,
kenjiArai 3:24aa4d5fa7de 27 const BMC050MAG_TypeDef *mag_parameter)
kenjiArai 3:24aa4d5fa7de 28 : _i2c(p_i2c)
kenjiArai 2:93141eb80862 29 {
kenjiArai 0:8de5e2fd5c48 30 initialize (acc_parameter, mag_parameter);
kenjiArai 0:8de5e2fd5c48 31 }
kenjiArai 0:8de5e2fd5c48 32
kenjiArai 2:93141eb80862 33 void BMC050::initialize (
kenjiArai 2:93141eb80862 34 const BMC050ACC_TypeDef *acc_parameter,
kenjiArai 2:93141eb80862 35 const BMC050MAG_TypeDef *mag_parameter)
kenjiArai 2:93141eb80862 36 {
kenjiArai 0:8de5e2fd5c48 37 /////////////// Magnetometer Configuration /////////////////
kenjiArai 0:8de5e2fd5c48 38 // after power-on, mag chip is keeping "Suspend mode"!!
kenjiArai 0:8de5e2fd5c48 39 // -> Need to go "Normal mode" via "Sleep mode"
kenjiArai 0:8de5e2fd5c48 40 // step 1/start
kenjiArai 0:8de5e2fd5c48 41 mag_addr = mag_parameter->addr;
kenjiArai 1:b022f8d7884d 42 if (mag_addr != BMC050_MAG_NOT_USED_ADDR) {
kenjiArai 1:b022f8d7884d 43 dbf[0] = BMC050_M_POWER_MODE;
kenjiArai 1:b022f8d7884d 44 dbf[1] = 0x01; // Power control bit on
kenjiArai 3:24aa4d5fa7de 45 _i2c.write(mag_addr, dbf, 2);
kenjiArai 1:b022f8d7884d 46 dbf[0] = BMC050_M_OPERATION;
kenjiArai 1:b022f8d7884d 47 dbf[1] = 0;
kenjiArai 3:24aa4d5fa7de 48 _i2c.write(mag_addr, dbf, 2);
kenjiArai 1:b022f8d7884d 49 }
kenjiArai 0:8de5e2fd5c48 50 /////////////// Accelerometer configuration ////////////////
kenjiArai 0:8de5e2fd5c48 51 // Check acc chip is available of not
kenjiArai 0:8de5e2fd5c48 52 // step 1/start
kenjiArai 0:8de5e2fd5c48 53 acc_addr = acc_parameter->addr;
kenjiArai 0:8de5e2fd5c48 54 dbf[0] = BMC050_A_WHO_AM_I;
kenjiArai 3:24aa4d5fa7de 55 _i2c.write(acc_addr, dbf, 1);
kenjiArai 3:24aa4d5fa7de 56 _i2c.read(acc_addr, dbf, 1);
kenjiArai 0:8de5e2fd5c48 57 if (dbf[0] == I_AM_BMC050_ACC){ acc_ready = 1;
kenjiArai 0:8de5e2fd5c48 58 } else { acc_ready = 0; }
kenjiArai 0:8de5e2fd5c48 59 /////////////// Magnetometer Configuration /////////////////
kenjiArai 0:8de5e2fd5c48 60 // after power-on, mag chip is keeping "Suspend mode"!!
kenjiArai 0:8de5e2fd5c48 61 // -> Need to go "Normal mode" via "Sleep mode"
kenjiArai 0:8de5e2fd5c48 62 // step 2
kenjiArai 1:b022f8d7884d 63 if (mag_addr != BMC050_MAG_NOT_USED_ADDR) {
kenjiArai 3:24aa4d5fa7de 64 _i2c.write(mag_addr, dbf, 2);
kenjiArai 1:b022f8d7884d 65 dbf[0] = BMC050_M_OPERATION;
kenjiArai 1:b022f8d7884d 66 dbf[1] = 0;
kenjiArai 3:24aa4d5fa7de 67 _i2c.write(mag_addr, dbf, 2);
kenjiArai 1:b022f8d7884d 68 }
kenjiArai 0:8de5e2fd5c48 69 /////////////// Accelerometer configuration ////////////////
kenjiArai 0:8de5e2fd5c48 70 // step 2/last
kenjiArai 0:8de5e2fd5c48 71 if ( acc_ready == 1){
kenjiArai 0:8de5e2fd5c48 72 // set g-range
kenjiArai 0:8de5e2fd5c48 73 dbf[0] = BMC050_A_G_RANGE;
kenjiArai 0:8de5e2fd5c48 74 dbf[1] = acc_parameter->g_range;
kenjiArai 0:8de5e2fd5c48 75 switch (dbf[1]){
kenjiArai 0:8de5e2fd5c48 76 case BMC050_FS_2G:
kenjiArai 0:8de5e2fd5c48 77 fs_factor_acc = 256;
kenjiArai 0:8de5e2fd5c48 78 break;
kenjiArai 0:8de5e2fd5c48 79 case BMC050_FS_4G:
kenjiArai 0:8de5e2fd5c48 80 fs_factor_acc = 128;
kenjiArai 0:8de5e2fd5c48 81 break;
kenjiArai 0:8de5e2fd5c48 82 case BMC050_FS_8G:
kenjiArai 0:8de5e2fd5c48 83 fs_factor_acc = 64;
kenjiArai 0:8de5e2fd5c48 84 break;
kenjiArai 0:8de5e2fd5c48 85 case BMC050_FS_16G:
kenjiArai 0:8de5e2fd5c48 86 fs_factor_acc = 32;
kenjiArai 0:8de5e2fd5c48 87 break;
kenjiArai 0:8de5e2fd5c48 88 default:
kenjiArai 0:8de5e2fd5c48 89 fs_factor_acc = 256;
kenjiArai 0:8de5e2fd5c48 90 dbf[1] = BMC050_FS_2G;
kenjiArai 0:8de5e2fd5c48 91 }
kenjiArai 3:24aa4d5fa7de 92 _i2c.write(acc_addr, dbf, 2);
kenjiArai 0:8de5e2fd5c48 93 // set bandwidth
kenjiArai 0:8de5e2fd5c48 94 dbf[1] = acc_parameter->bandwith;
kenjiArai 0:8de5e2fd5c48 95 if (dbf[1] == BMC050_NOT_FILTERED){
kenjiArai 0:8de5e2fd5c48 96 dbf[0] = BMC050_A_FILTER;
kenjiArai 0:8de5e2fd5c48 97 dbf[1] = 0x80;
kenjiArai 0:8de5e2fd5c48 98 } else {
kenjiArai 0:8de5e2fd5c48 99 dbf[0] = BMC050_A_BANDWIDTH;
kenjiArai 0:8de5e2fd5c48 100 }
kenjiArai 3:24aa4d5fa7de 101 _i2c.write(acc_addr, dbf, 2);
kenjiArai 0:8de5e2fd5c48 102 }
kenjiArai 0:8de5e2fd5c48 103 /////////////// Magnetometer Configuration /////////////////
kenjiArai 0:8de5e2fd5c48 104 // Check mag chip is available of not
kenjiArai 0:8de5e2fd5c48 105 // step 3/last
kenjiArai 1:b022f8d7884d 106 if (mag_addr != BMC050_MAG_NOT_USED_ADDR) {
kenjiArai 1:b022f8d7884d 107 dbf[0] = BMC050_M_WHO_AM_I;
kenjiArai 3:24aa4d5fa7de 108 _i2c.write(mag_addr, dbf, 1);
kenjiArai 3:24aa4d5fa7de 109 _i2c.read(mag_addr, dbf, 1);
kenjiArai 1:b022f8d7884d 110 if (dbf[0] == I_AM_BMC050_MAG){ mag_ready = 1;
kenjiArai 1:b022f8d7884d 111 } else { mag_ready = 0; }
kenjiArai 1:b022f8d7884d 112 if ( mag_ready == 1){
kenjiArai 1:b022f8d7884d 113 // set output data rate
kenjiArai 1:b022f8d7884d 114 dbf[0] = BMC050_M_OPERATION;
kenjiArai 1:b022f8d7884d 115 dbf[1] = mag_parameter->data_rate;
kenjiArai 3:24aa4d5fa7de 116 _i2c.write(mag_addr, dbf, 2);
kenjiArai 1:b022f8d7884d 117 }
kenjiArai 0:8de5e2fd5c48 118 }
kenjiArai 0:8de5e2fd5c48 119 }
kenjiArai 0:8de5e2fd5c48 120
kenjiArai 1:b022f8d7884d 121 /////////////// Accelerometer ///////////////////
kenjiArai 0:8de5e2fd5c48 122 void BMC050::read_data_acc(float *dt) {
kenjiArai 0:8de5e2fd5c48 123 char data[6];
kenjiArai 0:8de5e2fd5c48 124
kenjiArai 0:8de5e2fd5c48 125 if (acc_ready == 0){
kenjiArai 0:8de5e2fd5c48 126 dt[0] = dt[1] = dt[2] = 0;
kenjiArai 0:8de5e2fd5c48 127 return;
kenjiArai 0:8de5e2fd5c48 128 }
kenjiArai 1:b022f8d7884d 129 // X,Y &Z
kenjiArai 0:8de5e2fd5c48 130 dbf[0] = BMC050_A_OUT_X_L;
kenjiArai 3:24aa4d5fa7de 131 _i2c.write(acc_addr, dbf, 1, true);
kenjiArai 3:24aa4d5fa7de 132 _i2c.read(acc_addr, data, 6, false);
kenjiArai 0:8de5e2fd5c48 133 // change data type
kenjiArai 0:8de5e2fd5c48 134 dt[0] = float((short(data[1] << 8 | data[0] & 0xc0))>> 6) * GRAVITY / fs_factor_acc;
kenjiArai 0:8de5e2fd5c48 135 dt[1] = float((short(data[3] << 8 | data[2] & 0xc0))>> 6) * GRAVITY / fs_factor_acc;
kenjiArai 0:8de5e2fd5c48 136 dt[2] = float((short(data[5] << 8 | data[4] & 0xc0))>> 6) * GRAVITY / fs_factor_acc;
kenjiArai 0:8de5e2fd5c48 137 }
kenjiArai 0:8de5e2fd5c48 138
kenjiArai 1:b022f8d7884d 139 /////////////// Accelerometer ///////////////////
kenjiArai 1:b022f8d7884d 140 void BMC050::read_mg_acc(float *dt) {
kenjiArai 1:b022f8d7884d 141 char data[6];
kenjiArai 1:b022f8d7884d 142
kenjiArai 1:b022f8d7884d 143 if (acc_ready == 0){
kenjiArai 1:b022f8d7884d 144 dt[0] = 0;
kenjiArai 1:b022f8d7884d 145 dt[1] = 0;
kenjiArai 1:b022f8d7884d 146 dt[2] = 0;
kenjiArai 1:b022f8d7884d 147 return;
kenjiArai 1:b022f8d7884d 148 }
kenjiArai 1:b022f8d7884d 149 // X,Y &Z
kenjiArai 1:b022f8d7884d 150 dbf[0] = BMC050_A_OUT_X_L;
kenjiArai 3:24aa4d5fa7de 151 _i2c.write(acc_addr, dbf, 1, true);
kenjiArai 3:24aa4d5fa7de 152 _i2c.read(acc_addr, data, 6, false);
kenjiArai 1:b022f8d7884d 153 // change data type
kenjiArai 1:b022f8d7884d 154 dt[0] = float((short(data[1] << 8 | data[0] & 0xc0))>> 6) / fs_factor_acc;
kenjiArai 1:b022f8d7884d 155 dt[1] = float((short(data[3] << 8 | data[2] & 0xc0))>> 6) / fs_factor_acc;
kenjiArai 1:b022f8d7884d 156 dt[2] = float((short(data[5] << 8 | data[4] & 0xc0))>> 6) / fs_factor_acc;
kenjiArai 1:b022f8d7884d 157 }
kenjiArai 1:b022f8d7884d 158
kenjiArai 1:b022f8d7884d 159 /////////////// Magnetometer ////////////////////
kenjiArai 0:8de5e2fd5c48 160 void BMC050::read_data_mag(float *dt) {
kenjiArai 0:8de5e2fd5c48 161 char data[6];
kenjiArai 0:8de5e2fd5c48 162
kenjiArai 0:8de5e2fd5c48 163 if (mag_ready == 0){
kenjiArai 0:8de5e2fd5c48 164 dt[0] = dt[1] = dt[2] = 0;
kenjiArai 0:8de5e2fd5c48 165 return;
kenjiArai 0:8de5e2fd5c48 166 }
kenjiArai 1:b022f8d7884d 167 // X,Y &Z
kenjiArai 0:8de5e2fd5c48 168 dbf[0] = BMC050_M_OUT_X_L;
kenjiArai 3:24aa4d5fa7de 169 _i2c.write(mag_addr, dbf, 1, true);
kenjiArai 3:24aa4d5fa7de 170 _i2c.read(mag_addr, data, 6, false);
kenjiArai 0:8de5e2fd5c48 171 // change data type
kenjiArai 0:8de5e2fd5c48 172 dt[0] = float((short(data[1] << 8 | data[0] & 0xf8))>> 3);
kenjiArai 0:8de5e2fd5c48 173 dt[1] = float((short(data[3] << 8 | data[2] & 0xf8))>> 3);
kenjiArai 0:8de5e2fd5c48 174 dt[2] = float((short(data[5] << 8 | data[4] & 0xfe))>> 1);
kenjiArai 0:8de5e2fd5c48 175 }
kenjiArai 0:8de5e2fd5c48 176
kenjiArai 1:b022f8d7884d 177 /////////////// Accelerometer ///////////////////
kenjiArai 0:8de5e2fd5c48 178 float BMC050::read_temp() {
kenjiArai 0:8de5e2fd5c48 179 dbf[0] = BMC050_A_OUT_TEMP;
kenjiArai 3:24aa4d5fa7de 180 _i2c.write(acc_addr, dbf, 1);
kenjiArai 3:24aa4d5fa7de 181 _i2c.read(acc_addr, dbf, 1);
kenjiArai 3:24aa4d5fa7de 182 return ((float)dbf[0] * 0.5f + 24.0f);
kenjiArai 0:8de5e2fd5c48 183 }
kenjiArai 0:8de5e2fd5c48 184
kenjiArai 1:b022f8d7884d 185 /////////////// Accelerometer ///////////////////
kenjiArai 0:8de5e2fd5c48 186 uint8_t BMC050::read_id_acc() {
kenjiArai 0:8de5e2fd5c48 187 dbf[0] = BMC050_A_WHO_AM_I;
kenjiArai 3:24aa4d5fa7de 188 _i2c.write(acc_addr, dbf, 1);
kenjiArai 3:24aa4d5fa7de 189 _i2c.read(acc_addr, dbf, 1);
kenjiArai 0:8de5e2fd5c48 190 return (uint8_t)dbf[0];
kenjiArai 0:8de5e2fd5c48 191 }
kenjiArai 0:8de5e2fd5c48 192
kenjiArai 1:b022f8d7884d 193 /////////////// Magnetometer ////////////////////
kenjiArai 0:8de5e2fd5c48 194 uint8_t BMC050::read_id_mag() {
kenjiArai 0:8de5e2fd5c48 195 dbf[0] = BMC050_M_WHO_AM_I;
kenjiArai 3:24aa4d5fa7de 196 _i2c.write(mag_addr, dbf, 1);
kenjiArai 3:24aa4d5fa7de 197 _i2c.read(mag_addr, dbf, 1);
kenjiArai 0:8de5e2fd5c48 198 return (uint8_t)dbf[0];
kenjiArai 0:8de5e2fd5c48 199 }
kenjiArai 0:8de5e2fd5c48 200
kenjiArai 1:b022f8d7884d 201 /////////////// Accelerometer ///////////////////
kenjiArai 0:8de5e2fd5c48 202 uint8_t BMC050::data_ready_acc() {
kenjiArai 0:8de5e2fd5c48 203 if (acc_ready == 1){
kenjiArai 0:8de5e2fd5c48 204 dbf[0] = BMC050_A_OUT_X_L;
kenjiArai 3:24aa4d5fa7de 205 _i2c.write(acc_addr, dbf, 1);
kenjiArai 3:24aa4d5fa7de 206 _i2c.read(acc_addr, dbf, 1);
kenjiArai 0:8de5e2fd5c48 207 if (!(dbf[0] & 0x01)){
kenjiArai 0:8de5e2fd5c48 208 return 0;
kenjiArai 0:8de5e2fd5c48 209 }
kenjiArai 0:8de5e2fd5c48 210 }
kenjiArai 0:8de5e2fd5c48 211 return 1;
kenjiArai 0:8de5e2fd5c48 212 }
kenjiArai 0:8de5e2fd5c48 213
kenjiArai 1:b022f8d7884d 214 /////////////// Magnetometer ////////////////////
kenjiArai 0:8de5e2fd5c48 215 uint8_t BMC050::data_ready_mag() {
kenjiArai 0:8de5e2fd5c48 216 if (mag_ready == 1){
kenjiArai 0:8de5e2fd5c48 217 dbf[0] = BMC050_M_HALL_L;
kenjiArai 3:24aa4d5fa7de 218 _i2c.write(mag_addr, dbf, 1);
kenjiArai 3:24aa4d5fa7de 219 _i2c.read(mag_addr, dbf, 1);
kenjiArai 0:8de5e2fd5c48 220 if (!(dbf[0] & 0x01)){
kenjiArai 0:8de5e2fd5c48 221 return 0;
kenjiArai 0:8de5e2fd5c48 222 }
kenjiArai 0:8de5e2fd5c48 223 }
kenjiArai 0:8de5e2fd5c48 224 return 1;
kenjiArai 0:8de5e2fd5c48 225 }
kenjiArai 0:8de5e2fd5c48 226
kenjiArai 1:b022f8d7884d 227 /////////////// Accelerometer ///////////////////
kenjiArai 0:8de5e2fd5c48 228 uint8_t BMC050::read_reg_acc(uint8_t addr) {
kenjiArai 0:8de5e2fd5c48 229 if (acc_ready == 1){
kenjiArai 0:8de5e2fd5c48 230 dbf[0] = addr;
kenjiArai 3:24aa4d5fa7de 231 _i2c.write(acc_addr, dbf, 1);
kenjiArai 3:24aa4d5fa7de 232 _i2c.read(acc_addr, dbf, 1);
kenjiArai 0:8de5e2fd5c48 233 } else {
kenjiArai 0:8de5e2fd5c48 234 dbf[0] = 0xff;
kenjiArai 0:8de5e2fd5c48 235 }
kenjiArai 0:8de5e2fd5c48 236 return (uint8_t)dbf[0];
kenjiArai 0:8de5e2fd5c48 237 }
kenjiArai 0:8de5e2fd5c48 238
kenjiArai 1:b022f8d7884d 239 /////////////// Magnetometer ////////////////////
kenjiArai 1:b022f8d7884d 240 uint8_t BMC050::read_reg_mag(uint8_t addr) {
kenjiArai 1:b022f8d7884d 241 if (mag_ready == 1){
kenjiArai 1:b022f8d7884d 242 dbf[0] = addr;
kenjiArai 3:24aa4d5fa7de 243 _i2c.write(mag_addr, dbf, 1);
kenjiArai 3:24aa4d5fa7de 244 _i2c.read(mag_addr, dbf, 1);
kenjiArai 1:b022f8d7884d 245 } else {
kenjiArai 1:b022f8d7884d 246 dbf[0] = 0xff;
kenjiArai 1:b022f8d7884d 247 }
kenjiArai 1:b022f8d7884d 248 return (uint8_t)dbf[0];
kenjiArai 1:b022f8d7884d 249 }
kenjiArai 1:b022f8d7884d 250
kenjiArai 1:b022f8d7884d 251 /////////////// Accelerometer ///////////////////
kenjiArai 0:8de5e2fd5c48 252 void BMC050::write_reg_acc(uint8_t addr, uint8_t data) {
kenjiArai 0:8de5e2fd5c48 253 if (acc_ready == 1){
kenjiArai 0:8de5e2fd5c48 254 dbf[0] = addr;
kenjiArai 0:8de5e2fd5c48 255 dbf[1] = data;
kenjiArai 3:24aa4d5fa7de 256 _i2c.write(acc_addr, dbf, 2);
kenjiArai 0:8de5e2fd5c48 257 }
kenjiArai 0:8de5e2fd5c48 258 }
kenjiArai 0:8de5e2fd5c48 259
kenjiArai 1:b022f8d7884d 260 /////////////// Magnetometer ////////////////////
kenjiArai 0:8de5e2fd5c48 261 void BMC050::write_reg_mag(uint8_t addr, uint8_t data) {
kenjiArai 0:8de5e2fd5c48 262 if (mag_ready == 1){
kenjiArai 0:8de5e2fd5c48 263 dbf[0] = addr;
kenjiArai 0:8de5e2fd5c48 264 dbf[1] = data;
kenjiArai 3:24aa4d5fa7de 265 _i2c.write(mag_addr, dbf, 2);
kenjiArai 0:8de5e2fd5c48 266 }
kenjiArai 0:8de5e2fd5c48 267 }
kenjiArai 0:8de5e2fd5c48 268
kenjiArai 1:b022f8d7884d 269 /////////////// Common //////////////////////////
kenjiArai 1:b022f8d7884d 270 void BMC050::frequency(int hz) {
kenjiArai 3:24aa4d5fa7de 271 _i2c.frequency(hz);
kenjiArai 0:8de5e2fd5c48 272 }
kenjiArai 3:24aa4d5fa7de 273