Small, versatile 9-axis sensor module by Bosch Sensortec 3D Accelerometer + 3D Gyroscope + 3D Magnetometer
Dependents: Pruebas_Flex_IMU_copy BMX055_Madgwick
Diff: BMX055.cpp
- Revision:
- 0:4196bd8bb88f
- Child:
- 1:0d05e7c9ff4c
diff -r 000000000000 -r 4196bd8bb88f BMX055.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BMX055.cpp Fri Nov 23 00:51:14 2018 +0000 @@ -0,0 +1,460 @@ +/* + * mbed library program + * BMX055 Small, versatile 9-axis sensor module + * by Bosch Sensortec + * + * Copyright (c) 2018 Kenji Arai / JH1PJL + * http://www.page.sannet.ne.jp/kenjia/index.html + * http://mbed.org/users/kenjiArai/ + * Started: October 24th, 2018 + * Revised: October 26th, 2018 + */ + +#include "mbed.h" +#include "BMX055.h" + + +#if MBED_MAJOR_VERSION == 2 +#define WAIT_MS(x) wait_ms(x) +#elif MBED_MAJOR_VERSION == 5 +#define WAIT_MS(x) Thread::wait(x) +#else +#error "Running on Unknown OS" +#endif + +BMX055::BMX055 (PinName p_sda, PinName p_scl): + _i2c_p(new I2C(p_sda, p_scl)), _i2c(*_i2c_p) +{ + bmx055_parameters = bmx055_std_paramtr; + initialize (); +} + +BMX055::BMX055 (I2C& p_i2c) : + _i2c(p_i2c) +{ + initialize (); +} + +/////////////// Set parameter ///////////////////////////// +void BMX055::set_parameter(const BMX055_TypeDef *bmx055_parameter) +{ + bmx055_parameters = *bmx055_parameter; + set_parameters_to_regs(); + +} + +/////////////// Read data & normalize ///////////////////// +void BMX055::get_accel(BMX055_ACCEL_TypeDef *acc) +{ + int16_t x,y,z; + float factor = 2.0f; + + chip_addr = inf_addr.acc_addr; + dt[0] = 0x02; + _i2c.write(chip_addr, dt, 1, true); + _i2c.read(chip_addr, dt, 6, false); +#if 0 + printf("Read ACC data-> "); + for (uint32_t i = 0; i < 6; i++){ + printf("i=%d,dt=0x%02x, ", i, dt[i]); + } + printf(", all\r\n"); +#endif + x = dt[1] << 8 | (dt[0] & 0xf0); + y = dt[3] << 8 | (dt[2] & 0xf0); + z = dt[5] << 8 | (dt[4] & 0xf0); + switch(bmx055_parameters.acc_fs){ + case ACC_2G: + factor = 2.0f; + break; + case ACC_4G: + factor = 4.0f; + break; + case ACC_8G: + factor = 8.0f; + break; + case ACC_16G: + factor = 16.0f; + break; + default: + factor = 0; + break; + } + acc->x = (double)x * factor / 2048.0f / 16.0f; + acc->y = (double)y * factor / 2048.0f / 16.0f; + acc->z = (double)z * factor / 2048.0f / 16.0f; +} + +void BMX055::get_gyro(BMX055_GYRO_TypeDef *gyr) +{ + int16_t x,y,z; + float factor = 2.0f; + + chip_addr = inf_addr.gyr_addr; + dt[0] = 0x02; + _i2c.write(chip_addr, dt, 1, true); + _i2c.read(chip_addr, dt, 6, false); +#if 0 + printf("Read MAG data-> "); + for (uint32_t i = 0; i < 6; i++){ + printf("i=%d,dt=0x%02x, ", i, dt[i]); + } + printf(", all\r\n"); +#endif + x = dt[1] << 8 | dt[0]; + y = dt[3] << 8 | dt[2]; + z = dt[5] << 8 | dt[4]; +#if 0 + switch(bmx055_parameters.gyr_fs){ + case GYR_2000DPS: + factor = 1998.0f; + break; + case GYR_1000DPS: + factor = 999.0f; + break; + case GYR_500DPS: + factor = 499.5f; + break; + case GYR_250DPS: + factor = 249.75f; + break; + case GYR_125DPS: + factor = 124.87f; + break; + default: + factor = 0; + break; + } + gyr->x = (double)x * factor / 32768.0f / 16.0f; + gyr->y = (double)y * factor / 32768.0f / 16.0f; + gyr->z = (double)z * factor / 32768.0f / 16.0f; +#else + switch(bmx055_parameters.gyr_fs){ + case GYR_2000DPS: + factor = 61.0f; + break; + case GYR_1000DPS: + factor = 30.5f; + break; + case GYR_500DPS: + factor = 15.3; + break; + case GYR_250DPS: + factor = 7.6f; + break; + case GYR_125DPS: + factor = 3.8f; + break; + default: + factor = 0; + break; + } + gyr->x = (double)x * factor / 1000.0f; + gyr->y = (double)y * factor / 1000.0f; + gyr->z = (double)z * factor / 1000.0f; +#endif +} + +void BMX055::get_magnet(BMX055_MAGNET_TypeDef *mag) +{ + int16_t x,y,z; + + chip_addr = inf_addr.gyr_addr; + dt[0] = 0x02; + _i2c.write(chip_addr, dt, 1, true); + _i2c.read(chip_addr, dt, 6, false); +#if 0 + printf("Read GYR data-> "); + for (uint32_t i = 0; i < 6; i++){ + printf("i=%d,dt=0x%02x, ", i, dt[i]); + } + printf(", all\r\n"); +#endif + x = dt[1] << 8 | (dt[0] & 0xf8); // 13bit + y = dt[3] << 8 | (dt[2] & 0xf8); // 13bit + z = dt[5] << 8 | (dt[4] & 0xfe); // 15bit + mag->x = (double)x / 8.0f; + mag->y = (double)y / 8.0f; + mag->z = (double)z / 2.0f; +} + +float BMX055::get_chip_temperature() +{ + chip_addr = inf_addr.acc_addr; + dt[0] = 0x08; // chip tempareture reg addr + _i2c.write(chip_addr, dt, 1, true); + _i2c.read(chip_addr, dt, 1, false); + //printf("Temp reg = 0x%02x\r\n", dt[0]); + return (float)((int8_t)dt[0]) * 0.5f + 23.0f; +} + +/////////////// Initialize //////////////////////////////// +void BMX055::initialize (void) +{ + _i2c.frequency(100000); + // Check Acc & Mag & Gyro are available of not + check_id(); + if (ready_flag == 0x07){ + //printf("ACC+GYR+MAG are ready!\r\n"); + } + // Set initial data + set_parameters_to_regs(); +} + +////// Set initialize data to related registers /////////// +void BMX055::set_parameters_to_regs(void) +{ + // ACC + chip_addr = inf_addr.acc_addr; + dt[0] = 0x0f; // Select PMU_Range register + dt[1] = bmx055_parameters.acc_fs; + _i2c.write(chip_addr, dt, 2, false); + wait_ms(1); + dt[0] = 0x10; // Select PMU_BW register + dt[1] = bmx055_parameters.acc_bw; + _i2c.write(chip_addr, dt, 2, false); + wait_ms(1); + dt[0] = 0x11; // Select PMU_LPW register + dt[1] = 0x00; // Normal mode, Sleep duration = 0.5ms + _i2c.write(chip_addr, dt, 2, false); + // GYR + chip_addr = inf_addr.gyr_addr; + dt[0] = 0x0f; // Select Range register + dt[1] = bmx055_parameters.gyr_fs; + _i2c.write(chip_addr, dt, 2, false); + wait_ms(1); + dt[0] = 0x10; // Select Bandwidth register + dt[1] = bmx055_parameters.gyr_bw; + _i2c.write(chip_addr, dt, 2, false); + wait_ms(1); + dt[0] = 0x11; // Select LPM1 register + dt[1] = 0x00; // Normal mode, Sleep duration = 2ms + _i2c.write(chip_addr, dt, 2, false); + // MAG + chip_addr = inf_addr.mag_addr; + dt[0] = 0x4b; // Select Mag register + dt[1] = 0x83; // Soft reset + _i2c.write(chip_addr, dt, 2, false); + wait_ms(10); + dt[0] = 0x4b; // Select Mag register + dt[1] = 0x01; // Soft reset + _i2c.write(chip_addr, dt, 2, false); + wait_ms(10); + dt[0] = 0x4c; // Select Mag register + dt[1] = bmx055_parameters.mag_odr; + _i2c.write(chip_addr, dt, 2, false); + wait_ms(1); + dt[0] = 0x4e; // Select Mag register + dt[1] = 0x84; // X, Y, Z-Axis enabled + _i2c.write(chip_addr, dt, 2, false); + wait_ms(1); + dt[0] = 0x51; // Select Mag register + dt[1] = 0x04; // No. of Repetitions for X-Y Axis = 9 + _i2c.write(chip_addr, dt, 2, false); + wait_ms(1); + dt[0] = 0x52; // Select Mag register + dt[1] = 0x16; // No. of Repetitions for Z-Axis = 15 + _i2c.write(chip_addr, dt, 2, false); +#if 0 + // ACC + chip_addr = inf_addr.acc_addr; + dt[0] = 0x0f; // Select PMU_Range register + dt[1] = 0x03; // Range = +/- 2g + _i2c.write(chip_addr, dt, 2, false); + wait_ms(1); + dt[0] = 0x10; // Select PMU_BW register + dt[1] = 0x08; // Bandwidth = 7.81 Hz + _i2c.write(chip_addr, dt, 2, false); + wait_ms(1); + dt[0] = 0x11; // Select PMU_LPW register + dt[1] = 0x00; // Normal mode, Sleep duration = 0.5ms + _i2c.write(chip_addr, dt, 2, false); + wait_ms(1); + // GYR + chip_addr = inf_addr.gyr_addr; + dt[0] = 0x0f; // Select Range register + dt[1] = 0x04; // Full scale = +/- 125 degree/s + _i2c.write(chip_addr, dt, 2, false); + wait_ms(1); + dt[0] = 0x10; // Select Bandwidth register + dt[1] = 0x07; // ODR = 100 Hz + _i2c.write(chip_addr, dt, 2, false); + wait_ms(1); + dt[0] = 0x11; // Select LPM1 register + dt[1] = 0x00; // Normal mode, Sleep duration = 2ms + _i2c.write(chip_addr, dt, 2, false); + // MAG + chip_addr = inf_addr.mag_addr; + dt[0] = 0x4b; // Select Mag register + dt[1] = 0x83; // Soft reset + _i2c.write(chip_addr, dt, 2, false); + wait_ms(10); + dt[0] = 0x4b; // Select Mag register + dt[1] = 0x01; // Soft reset + _i2c.write(chip_addr, dt, 2, false); + wait_ms(10); + dt[0] = 0x4c; // Select Mag register + dt[1] = 0x00; // Normal Mode, ODR = 10 Hz + _i2c.write(chip_addr, dt, 2, false); + wait_ms(1); + dt[0] = 0x4e; // Select Mag register + dt[1] = 0x84; // X, Y, Z-Axis enabled + _i2c.write(chip_addr, dt, 2, false); + wait_ms(1); + dt[0] = 0x51; // Select Mag register + dt[1] = 0x04; // No. of Repetitions for X-Y Axis = 9 + _i2c.write(chip_addr, dt, 2, false); + wait_ms(1); + dt[0] = 0x52; // Select Mag register + dt[1] = 0x16; // No. of Repetitions for Z-Axis = 15 + _i2c.write(chip_addr, dt, 2, false); + wait_ms(1); +#endif +} + +/////////////// Check Who am I? /////////////////////////// +void BMX055::check_id(void) +{ + ready_flag = 0; + // ID ACC + inf_addr.acc_addr = BMX055_ACC_CHIP_ADDR; + chip_addr = inf_addr.acc_addr; + dt[0] = 0x00; // chip ID reg addr + _i2c.write(chip_addr, dt, 1, true); + _i2c.read(chip_addr, dt, 1, false); + inf_id.acc_id = dt[0]; + if (inf_id.acc_id == I_AM_BMX055_ACC) { + ready_flag |= 0x01; + } else { + inf_addr.acc_addr = (0x18 << 1); + chip_addr = inf_addr.acc_addr; + dt[0] = 0x00; // chip ID reg addr + _i2c.write(chip_addr, dt, 1, true); + _i2c.read(chip_addr, dt, 1, false); + inf_id.acc_id = dt[0]; + if (inf_id.acc_id == I_AM_BMX055_ACC) { + ready_flag |= 0x01; + } + } + // ID GYRO + inf_addr.gyr_addr = BMX055_GYR_CHIP_ADDR; + chip_addr = inf_addr.gyr_addr; + dt[0] = 0x00; // chip ID reg addr + _i2c.write(chip_addr, dt, 1, true); + _i2c.read(chip_addr, dt, 1, false); + inf_id.gyr_id = dt[0]; + if (inf_id.gyr_id == I_AM_BMX055_GYR) { + ready_flag |= 0x02; + } else { + inf_addr.gyr_addr = (0x68 << 1); + chip_addr = inf_addr.gyr_addr; + dt[0] = 0x00; // chip ID reg addr + _i2c.write(chip_addr, dt, 1, true); + _i2c.read(chip_addr, dt, 1, false); + inf_id.gyr_id = dt[0]; + if (inf_id.gyr_id == I_AM_BMX055_GYR) { + ready_flag |= 0x02; + } + } + // ID Mag + inf_addr.mag_addr = BMX055_MAG_CHIP_ADDR; + chip_addr = inf_addr.mag_addr; + dt[0] = 0x4b; // reg addr + dt[1] = 0x01; // control power bit set 1 + _i2c.write(chip_addr, dt, 2, false); + dt[0] = 0x40; // chip ID reg addr + _i2c.write(chip_addr, dt, 1, true); + _i2c.read(chip_addr, dt, 1, false); + inf_id.mag_id = dt[0]; + if (inf_id.mag_id == I_AM_BMX055_MAG) { + ready_flag |= 0x04; + } else { + inf_addr.mag_addr = (0x12 << 1); + chip_addr = inf_addr.mag_addr; + // control power bit set 1 + dt[0] = 0x4b; + dt[1] = 0x01; + _i2c.write(chip_addr, dt, 2, false); + dt[0] = 0x40; + _i2c.write(chip_addr, dt, 1, true); + _i2c.read(chip_addr, dt, 1, false); + inf_id.mag_id = dt[0]; + if (inf_id.mag_id == I_AM_BMX055_MAG) { + ready_flag |= 0x04; + } else { + inf_addr.mag_addr = (0x11 << 1); + chip_addr = inf_addr.mag_addr; + // control power bit set 1 + dt[0] = 0x4b; + dt[1] = 0x01; + _i2c.write(chip_addr, dt, 2, false); + dt[0] = 0x40; + _i2c.write(chip_addr, dt, 1, true); + _i2c.read(chip_addr, dt, 1, false); + inf_id.mag_id = dt[0]; + if (inf_id.mag_id == I_AM_BMX055_MAG) { + ready_flag |= 0x04; + } else { + inf_addr.mag_addr = (0x10 << 1); + chip_addr = inf_addr.mag_addr; + // control power bit set 1 + dt[0] = 0x4b; + dt[1] = 0x01; + _i2c.write(chip_addr, dt, 2, false); + dt[0] = 0x40; + _i2c.write(chip_addr, dt, 1, true); + _i2c.read(chip_addr, dt, 1, false); + inf_id.mag_id = dt[0]; + if (inf_id.mag_id == I_AM_BMX055_MAG) { + ready_flag |= 0x04; + } + } + } + } + //printf("ACC addr=0x%02x, id=0x%02x\r\n", chip_addr, inf_id.acc_id); + //printf("GYR addr=0x%02x, id=0x%02x\r\n", chip_addr, inf_id.gyr_id); + //printf("MAG addr=0x%02x, id=0x%02x\r\n", chip_addr, inf_id.mag_id); + //printf("ready_flag = 0x%x\r\n", ready_flag); +} + +void BMX055::read_id_inf(BMX055_ID_INF_TypeDef *id) +{ + id->acc_id = acc_id; + id->mag_id = mag_id; + id->gyr_id = gyr_id; +} + +/////////////// Check chip ready or not ////////////////// +bool BMX055::chip_ready(void) +{ + if (ready_flag == 0x07) { + return true; + } + return false; +} + +/////////////// I2C Freq. ///////////////////////////////// +void BMX055::frequency(int hz) +{ + _i2c.frequency(hz); +} + +/////////////// Read/Write specific register ////////////// +uint8_t BMX055::read_reg(uint8_t addr) +{ + dt[0] = addr; + _i2c.write(chip_addr, dt, 1, true); + _i2c.read(chip_addr, dt, 1, false); + return (uint8_t)dt[0]; +} + +uint8_t BMX055::write_reg(uint8_t addr, uint8_t data) +{ + uint8_t d; + + dt[0] = addr; + dt[1] = data; + _i2c.write(chip_addr, dt, 2, false); + d = dt[0]; + return d; +}