Akifumi Takahashi / BMX055

Library to use BMX055 via I2C

Dependents:   NineIMUAttitude_MadgwickFilter

BMX055.cpp@0:be73a6efa20b, 2020-08-21 (annotated)

Committer:
aktk
Date:
Fri Aug 21 03:11:36 2020 +0000
Revision:
0:be73a6efa20b
The first commit.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aktk 0:be73a6efa20b 1 #include "BMX055.hpp"
aktk 0:be73a6efa20b 2
aktk 0:be73a6efa20b 3 void BMX055::Init_I2C(PinName arg_SDA, PinName arg_SCL)
aktk 0:be73a6efa20b 4 {
aktk 0:be73a6efa20b 5 i2c = new I2C(arg_SDA, arg_SCL);
aktk 0:be73a6efa20b 6 i2c->frequency(400000);//BMX055 MAX I2C Speed is 400kHz
aktk 0:be73a6efa20b 7 char cmd[2];
aktk 0:be73a6efa20b 8
aktk 0:be73a6efa20b 9 // Accelerometer initialization
aktk 0:be73a6efa20b 10 cmd[0] = 0x0f; // Select PMU_Range register
aktk 0:be73a6efa20b 11 cmd[1] = 0x03; // Range = +/- 2g
aktk 0:be73a6efa20b 12 i2c->write(m_addr_acc, cmd, 2);
aktk 0:be73a6efa20b 13 wait_ms(100);
aktk 0:be73a6efa20b 14
aktk 0:be73a6efa20b 15 cmd[0] = 0x10; // Select PMU_BW register
aktk 0:be73a6efa20b 16 cmd[1] = 0xff; // Bandwidth = 1000 Hz
aktk 0:be73a6efa20b 17 i2c->write(m_addr_acc, cmd, 2);
aktk 0:be73a6efa20b 18 wait_ms(100);
aktk 0:be73a6efa20b 19
aktk 0:be73a6efa20b 20 cmd[0] = 0x11; // Select PMU_LPW register
aktk 0:be73a6efa20b 21 cmd[1] = 0x00; // Normal mode, Sleep duration = 0.5ms
aktk 0:be73a6efa20b 22 i2c->write(m_addr_acc, cmd, 2);
aktk 0:be73a6efa20b 23 wait_ms(100);
aktk 0:be73a6efa20b 24
aktk 0:be73a6efa20b 25 cmd[0] = 0x3e; // Select FIFO_CONFIG_1 register
aktk 0:be73a6efa20b 26 cmd[1] = 0x00; // BYPASS mode, The mode a frame has X-Y-Z data (6bytes)
aktk 0:be73a6efa20b 27 i2c->write(m_addr_acc, cmd, 2);
aktk 0:be73a6efa20b 28 wait_ms(100);
aktk 0:be73a6efa20b 29
aktk 0:be73a6efa20b 30
aktk 0:be73a6efa20b 31 // gyrometer initialization
aktk 0:be73a6efa20b 32 cmd[0] = 0x0f; // Select Range register
aktk 0:be73a6efa20b 33 //cmd[1] = 0x04; // Full scale = +/- 125 degree/s 3.8 m deg/s/LSB
aktk 0:be73a6efa20b 34 cmd[1] = 0x02; // Full scale = +/- 500 degree/s; 15.3 m deg/s/LSB
aktk 0:be73a6efa20b 35 i2c->write(m_addr_gyr, cmd, 2);
aktk 0:be73a6efa20b 36 wait_ms(100);
aktk 0:be73a6efa20b 37
aktk 0:be73a6efa20b 38 cmd[0] = 0x10; // Select Bandwidth register
aktk 0:be73a6efa20b 39 cmd[1] = 0x02; // ODR = 1000 Hz
aktk 0:be73a6efa20b 40 i2c->write(m_addr_gyr, cmd, 2);
aktk 0:be73a6efa20b 41 wait_ms(100);
aktk 0:be73a6efa20b 42
aktk 0:be73a6efa20b 43 cmd[0] = 0x11; // Select LPM1 register
aktk 0:be73a6efa20b 44 cmd[1] = 0x00; // Normal mode, Sleep duration = 2ms
aktk 0:be73a6efa20b 45 i2c->write(m_addr_gyr, cmd, 2);
aktk 0:be73a6efa20b 46 wait_ms(100);
aktk 0:be73a6efa20b 47
aktk 0:be73a6efa20b 48 cmd[0] = 0x3e; // Select FIFO_CONFIG_1 register
aktk 0:be73a6efa20b 49 cmd[1] = 0x00; // BYPASS mode, The mode a frame has XYZ data + stata (8bytes)
aktk 0:be73a6efa20b 50 i2c->write(m_addr_gyr, cmd, 2);
aktk 0:be73a6efa20b 51 wait_ms(100);
aktk 0:be73a6efa20b 52
aktk 0:be73a6efa20b 53
aktk 0:be73a6efa20b 54 // magnetic sensor initialization
aktk 0:be73a6efa20b 55 cmd[0] = 0x4b; // Select Mag register
aktk 0:be73a6efa20b 56 cmd[1] = 0x83; // Soft reset
aktk 0:be73a6efa20b 57 i2c->write(m_addr_mag, cmd, 2);
aktk 0:be73a6efa20b 58 wait_ms(100);
aktk 0:be73a6efa20b 59 cmd[0] = 0x4B; // Select Mag register
aktk 0:be73a6efa20b 60 cmd[1] = 0x01; // Soft reset
aktk 0:be73a6efa20b 61 i2c->write(m_addr_mag, cmd, 2);
aktk 0:be73a6efa20b 62 wait_ms(100);
aktk 0:be73a6efa20b 63 cmd[0] = 0x4c; // Select Mag register
aktk 0:be73a6efa20b 64 cmd[1] = 0x00; // Normal Mode, Output Data Rate = 10 Hz
aktk 0:be73a6efa20b 65 i2c->write(m_addr_mag, cmd, 2);
aktk 0:be73a6efa20b 66 wait_ms(100);
aktk 0:be73a6efa20b 67 cmd[0] = 0x4e; // Select Mag register
aktk 0:be73a6efa20b 68 cmd[1] = 0x84; // X, Y, Z-Axis enabled
aktk 0:be73a6efa20b 69 i2c->write(m_addr_mag, cmd, 2);
aktk 0:be73a6efa20b 70 wait_ms(100);
aktk 0:be73a6efa20b 71 cmd[0] = 0x51; // Select Mag register
aktk 0:be73a6efa20b 72 cmd[1] = 0x04; // No. of Repetitions for X-Y Axis = 9
aktk 0:be73a6efa20b 73 i2c->write(m_addr_mag, cmd, 2);
aktk 0:be73a6efa20b 74 wait_ms(100);
aktk 0:be73a6efa20b 75 cmd[0] = 0x52; // Select Mag register
aktk 0:be73a6efa20b 76 cmd[1] = 0x16; // No. of Repetitions for Z-Axis = 15
aktk 0:be73a6efa20b 77 i2c->write(m_addr_mag, cmd, 2);
aktk 0:be73a6efa20b 78 wait_ms(100);
aktk 0:be73a6efa20b 79 }
aktk 0:be73a6efa20b 80
aktk 0:be73a6efa20b 81
aktk 0:be73a6efa20b 82 void BMX055::UpdateIMU()
aktk 0:be73a6efa20b 83 {
aktk 0:be73a6efa20b 84 static char data_acc[6] = {0,0,0,0,0,0};
aktk 0:be73a6efa20b 85 static char data_gyr[8] = {0,0,0,0,0,0,0,0};
aktk 0:be73a6efa20b 86
aktk 0:be73a6efa20b 87 // Request Read out from FIFO buff
aktk 0:be73a6efa20b 88 char reg = 0x3f;
aktk 0:be73a6efa20b 89 i2c->write(m_addr_acc, &reg, 1);// Select data_acc register
aktk 0:be73a6efa20b 90 // - Read out 6 bytes of data_acc X-Y-Z frame
aktk 0:be73a6efa20b 91 // - FRAME:
aktk 0:be73a6efa20b 92 // [xAccl lsb | xAccl msb | yAccl lsb | yAccl msb | zAccl lsb | zAccl msb]
aktk 0:be73a6efa20b 93 i2c->read(m_addr_acc, data_acc, 6);
aktk 0:be73a6efa20b 94
aktk 0:be73a6efa20b 95 i2c->write(m_addr_gyr, &reg, 1);// Select data_acc register
aktk 0:be73a6efa20b 96 // - Read 6 bytes out of a frame which contain 8 bytes
aktk 0:be73a6efa20b 97 // - FRAME:
aktk 0:be73a6efa20b 98 // [xGyro lsb | xGyro msb | yGyro lsb | yGyro msb | zGyro lsb | zGyro msb |
aktk 0:be73a6efa20b 99 // status-0 | status-1 ]
aktk 0:be73a6efa20b 100 // - Discurding the frame it takes 1.5us
aktk 0:be73a6efa20b 101 // but it is less than reading 2 bytes via I2C
aktk 0:be73a6efa20b 102 i2c->read(m_addr_gyr, data_gyr, 8);
aktk 0:be73a6efa20b 103
aktk 0:be73a6efa20b 104 // Convert the data_acc to 12-bits
aktk 0:be73a6efa20b 105 // top 4 bits should be same (Two's complement)
aktk 0:be73a6efa20b 106 acc.x = (signed short)(((unsigned short)data_acc[1] << 8) | (unsigned short)(data_acc[0] & 0xf0)) >> 4;
aktk 0:be73a6efa20b 107 acc.y = (signed short)(((unsigned short)data_acc[3] << 8) | (unsigned short)(data_acc[2] & 0xf0)) >> 4;
aktk 0:be73a6efa20b 108 acc.z = (signed short)(((unsigned short)data_acc[5] << 8) | (unsigned short)(data_acc[4] & 0xf0)) >> 4;
aktk 0:be73a6efa20b 109 // Convert the data_gyr to 16-bits
aktk 0:be73a6efa20b 110 gyr.x = (signed short)(((unsigned short)data_gyr[1] << 8) | (unsigned short)data_gyr[0]);
aktk 0:be73a6efa20b 111 gyr.y = (signed short)(((unsigned short)data_gyr[3] << 8) | (unsigned short)data_gyr[2]);
aktk 0:be73a6efa20b 112 gyr.z = (signed short)(((unsigned short)data_gyr[5] << 8) | (unsigned short)data_gyr[4]);
aktk 0:be73a6efa20b 113 }
aktk 0:be73a6efa20b 114
aktk 0:be73a6efa20b 115 void BMX055::Update()
aktk 0:be73a6efa20b 116 {
aktk 0:be73a6efa20b 117 UpdateIMU();
aktk 0:be73a6efa20b 118
aktk 0:be73a6efa20b 119 char buf;
aktk 0:be73a6efa20b 120 unsigned short data_mag[8];
aktk 0:be73a6efa20b 121 for (int i = 0; i < 8; i++) {
aktk 0:be73a6efa20b 122 char reg = 0x42 + i;
aktk 0:be73a6efa20b 123 i2c->write(m_addr_mag, &reg, 1);// Select data register
aktk 0:be73a6efa20b 124 // Read 8 bytes of data
aktk 0:be73a6efa20b 125 // xMag lsb, xMag msb, yMag lsb, yMag msb, zMag lsb, zMag msb
aktk 0:be73a6efa20b 126 // + 2 bytes for control bits
aktk 0:be73a6efa20b 127 i2c->read(m_addr_mag, &buf, 1);// Request 1 byte of data
aktk 0:be73a6efa20b 128 data_mag[i] = (unsigned char)buf;
aktk 0:be73a6efa20b 129 }
aktk 0:be73a6efa20b 130
aktk 0:be73a6efa20b 131 // Convert the data
aktk 0:be73a6efa20b 132 mag.x = ((signed short)((data_mag[1] << 8) | data_mag[0])) >> 3;
aktk 0:be73a6efa20b 133 mag.y = ((signed short)((data_mag[3] << 8) | data_mag[2])) >> 3;
aktk 0:be73a6efa20b 134 mag.z = ((signed short)((data_mag[5] << 8) | data_mag[4])) >> 1;
aktk 0:be73a6efa20b 135 }