BMX055.cpp

00001 #include "BMX055.hpp"
00002 
00003 void BMX055::Init_I2C(PinName arg_SDA, PinName arg_SCL)
00004 {
00005     i2c = new I2C(arg_SDA, arg_SCL);
00006     i2c->frequency(400000);//BMX055 MAX I2C Speed is 400kHz
00007     char cmd[2];
00008     
00009     // Accelerometer initialization
00010     cmd[0] = 0x0f; // Select PMU_Range register
00011     cmd[1] = 0x03; // Range = +/- 2g
00012     i2c->write(m_addr_acc, cmd, 2);
00013     wait_ms(100);
00014     
00015     cmd[0] = 0x10; // Select PMU_BW register
00016     cmd[1] = 0xff; // Bandwidth = 1000 Hz
00017     i2c->write(m_addr_acc, cmd, 2);
00018     wait_ms(100);
00019     
00020     cmd[0] = 0x11; // Select PMU_LPW register
00021     cmd[1] = 0x00; // Normal mode, Sleep duration = 0.5ms
00022     i2c->write(m_addr_acc, cmd, 2);
00023     wait_ms(100);
00024     
00025     cmd[0] = 0x3e; // Select FIFO_CONFIG_1 register
00026     cmd[1] = 0x00; // BYPASS mode, The mode a frame has X-Y-Z data (6bytes)
00027     i2c->write(m_addr_acc, cmd, 2);
00028     wait_ms(100);
00029 
00030 
00031     // gyrometer initialization
00032     cmd[0] = 0x0f;  // Select Range register
00033     //cmd[1] = 0x04;  // Full scale = +/- 125 degree/s 3.8 m deg/s/LSB
00034     cmd[1] = 0x02;  // Full scale = +/- 500 degree/s; 15.3 m deg/s/LSB
00035     i2c->write(m_addr_gyr, cmd, 2);
00036     wait_ms(100);
00037     
00038     cmd[0] = 0x10;  // Select Bandwidth register
00039     cmd[1] = 0x02;  // ODR = 1000 Hz
00040     i2c->write(m_addr_gyr, cmd, 2);
00041     wait_ms(100);
00042     
00043     cmd[0] = 0x11;  // Select LPM1 register
00044     cmd[1] = 0x00;  // Normal mode, Sleep duration = 2ms
00045     i2c->write(m_addr_gyr, cmd, 2);
00046     wait_ms(100);
00047     
00048     cmd[0] = 0x3e; // Select FIFO_CONFIG_1 register
00049     cmd[1] = 0x00; // BYPASS mode, The mode a frame has XYZ data + stata (8bytes)
00050     i2c->write(m_addr_gyr, cmd, 2);
00051     wait_ms(100);
00052 
00053 
00054     // magnetic sensor initialization
00055     cmd[0] = 0x4b;  // Select Mag register
00056     cmd[1] = 0x83;  // Soft reset
00057     i2c->write(m_addr_mag, cmd, 2);
00058     wait_ms(100);
00059     cmd[0] = 0x4B;  // Select Mag register
00060     cmd[1] = 0x01;  // Soft reset
00061     i2c->write(m_addr_mag, cmd, 2);
00062     wait_ms(100);
00063     cmd[0] = 0x4c;  // Select Mag register
00064     cmd[1] = 0x00;  // Normal Mode, Output Data Rate = 10 Hz
00065     i2c->write(m_addr_mag, cmd, 2);
00066     wait_ms(100);
00067     cmd[0] = 0x4e;  // Select Mag register
00068     cmd[1] = 0x84;  // X, Y, Z-Axis enabled
00069     i2c->write(m_addr_mag, cmd, 2);
00070     wait_ms(100);
00071     cmd[0] = 0x51;  // Select Mag register
00072     cmd[1] = 0x04;  // No. of Repetitions for X-Y Axis = 9
00073     i2c->write(m_addr_mag, cmd, 2);
00074     wait_ms(100);
00075     cmd[0] = 0x52;  // Select Mag register
00076     cmd[1] = 0x16;  // No. of Repetitions for Z-Axis = 15
00077     i2c->write(m_addr_mag, cmd, 2);
00078     wait_ms(100);
00079 }
00080 
00081 
00082 void BMX055::UpdateIMU()
00083 {
00084     static char data_acc[6] = {0,0,0,0,0,0};
00085     static char data_gyr[8] = {0,0,0,0,0,0,0,0};
00086     
00087     // Request Read out from FIFO buff
00088     char reg = 0x3f;
00089     i2c->write(m_addr_acc, &reg, 1);// Select data_acc register
00090     //  - Read out 6 bytes of data_acc X-Y-Z frame
00091     //  - FRAME:
00092     //  [xAccl lsb | xAccl msb | yAccl lsb | yAccl msb | zAccl lsb | zAccl msb]
00093     i2c->read(m_addr_acc, data_acc, 6);
00094     
00095     i2c->write(m_addr_gyr, &reg, 1);// Select data_acc register
00096     //  - Read 6 bytes out of a frame which contain 8 bytes
00097     //  - FRAME:
00098     //  [xGyro lsb | xGyro msb | yGyro lsb | yGyro msb | zGyro lsb | zGyro msb | 
00099     //    status-0 | status-1  ]
00100     //  - Discurding the frame it takes 1.5us 
00101     //      but it is less than reading 2 bytes via I2C
00102     i2c->read(m_addr_gyr, data_gyr, 8);
00103     
00104     // Convert the data_acc to 12-bits
00105     //  top 4 bits should be same (Two's complement)
00106     acc.x = (signed short)(((unsigned short)data_acc[1] << 8) | (unsigned short)(data_acc[0] & 0xf0)) >> 4;
00107     acc.y = (signed short)(((unsigned short)data_acc[3] << 8) | (unsigned short)(data_acc[2] & 0xf0)) >> 4;
00108     acc.z = (signed short)(((unsigned short)data_acc[5] << 8) | (unsigned short)(data_acc[4] & 0xf0)) >> 4;
00109     // Convert the data_gyr to 16-bits
00110     gyr.x = (signed short)(((unsigned short)data_gyr[1] << 8) | (unsigned short)data_gyr[0]);
00111     gyr.y = (signed short)(((unsigned short)data_gyr[3] << 8) | (unsigned short)data_gyr[2]);
00112     gyr.z = (signed short)(((unsigned short)data_gyr[5] << 8) | (unsigned short)data_gyr[4]);
00113 }
00114 
00115 void BMX055::Update()
00116 {
00117     UpdateIMU();
00118     
00119     char buf;
00120     unsigned short data_mag[8];
00121     for (int i = 0; i < 8; i++) {
00122         char reg = 0x42 + i;
00123         i2c->write(m_addr_mag, &reg, 1);// Select data register
00124         // Read 8 bytes of data
00125         // xMag lsb, xMag msb, yMag lsb, yMag msb, zMag lsb, zMag msb
00126         // + 2 bytes for control bits
00127         i2c->read(m_addr_mag, &buf, 1);// Request 1 byte of data
00128         data_mag[i] = (unsigned char)buf;
00129     }
00130 
00131     // Convert the data
00132     mag.x = ((signed short)((data_mag[1] << 8) | data_mag[0])) >> 3;
00133     mag.y = ((signed short)((data_mag[3] << 8) | data_mag[2])) >> 3;
00134     mag.z = ((signed short)((data_mag[5] << 8) | data_mag[4])) >> 1;
00135 }