Revision 0:be73a6efa20b, committed 2020-08-21

Comitter:

aktk

Date:

Fri Aug 21 03:11:36 2020 +0000

Commit message:

The first commit.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BMX055.cpp	Fri Aug 21 03:11:36 2020 +0000
@@ -0,0 +1,135 @@
+#include "BMX055.hpp"
+
+void BMX055::Init_I2C(PinName arg_SDA, PinName arg_SCL)
+{
+    i2c = new I2C(arg_SDA, arg_SCL);
+    i2c->frequency(400000);//BMX055 MAX I2C Speed is 400kHz
+    char cmd[2];
+    
+    // Accelerometer initialization
+    cmd[0] = 0x0f; // Select PMU_Range register
+    cmd[1] = 0x03; // Range = +/- 2g
+    i2c->write(m_addr_acc, cmd, 2);
+    wait_ms(100);
+    
+    cmd[0] = 0x10; // Select PMU_BW register
+    cmd[1] = 0xff; // Bandwidth = 1000 Hz
+    i2c->write(m_addr_acc, cmd, 2);
+    wait_ms(100);
+    
+    cmd[0] = 0x11; // Select PMU_LPW register
+    cmd[1] = 0x00; // Normal mode, Sleep duration = 0.5ms
+    i2c->write(m_addr_acc, cmd, 2);
+    wait_ms(100);
+    
+    cmd[0] = 0x3e; // Select FIFO_CONFIG_1 register
+    cmd[1] = 0x00; // BYPASS mode, The mode a frame has X-Y-Z data (6bytes)
+    i2c->write(m_addr_acc, cmd, 2);
+    wait_ms(100);
+
+
+    // gyrometer initialization
+    cmd[0] = 0x0f;  // Select Range register
+    //cmd[1] = 0x04;  // Full scale = +/- 125 degree/s 3.8 m deg/s/LSB
+    cmd[1] = 0x02;  // Full scale = +/- 500 degree/s; 15.3 m deg/s/LSB
+    i2c->write(m_addr_gyr, cmd, 2);
+    wait_ms(100);
+    
+    cmd[0] = 0x10;  // Select Bandwidth register
+    cmd[1] = 0x02;  // ODR = 1000 Hz
+    i2c->write(m_addr_gyr, cmd, 2);
+    wait_ms(100);
+    
+    cmd[0] = 0x11;  // Select LPM1 register
+    cmd[1] = 0x00;  // Normal mode, Sleep duration = 2ms
+    i2c->write(m_addr_gyr, cmd, 2);
+    wait_ms(100);
+    
+    cmd[0] = 0x3e; // Select FIFO_CONFIG_1 register
+    cmd[1] = 0x00; // BYPASS mode, The mode a frame has XYZ data + stata (8bytes)
+    i2c->write(m_addr_gyr, cmd, 2);
+    wait_ms(100);
+
+
+    // magnetic sensor initialization
+    cmd[0] = 0x4b;  // Select Mag register
+    cmd[1] = 0x83;  // Soft reset
+    i2c->write(m_addr_mag, cmd, 2);
+    wait_ms(100);
+    cmd[0] = 0x4B;  // Select Mag register
+    cmd[1] = 0x01;  // Soft reset
+    i2c->write(m_addr_mag, cmd, 2);
+    wait_ms(100);
+    cmd[0] = 0x4c;  // Select Mag register
+    cmd[1] = 0x00;  // Normal Mode, Output Data Rate = 10 Hz
+    i2c->write(m_addr_mag, cmd, 2);
+    wait_ms(100);
+    cmd[0] = 0x4e;  // Select Mag register
+    cmd[1] = 0x84;  // X, Y, Z-Axis enabled
+    i2c->write(m_addr_mag, cmd, 2);
+    wait_ms(100);
+    cmd[0] = 0x51;  // Select Mag register
+    cmd[1] = 0x04;  // No. of Repetitions for X-Y Axis = 9
+    i2c->write(m_addr_mag, cmd, 2);
+    wait_ms(100);
+    cmd[0] = 0x52;  // Select Mag register
+    cmd[1] = 0x16;  // No. of Repetitions for Z-Axis = 15
+    i2c->write(m_addr_mag, cmd, 2);
+    wait_ms(100);
+}
+
+
+void BMX055::UpdateIMU()
+{
+    static char data_acc[6] = {0,0,0,0,0,0};
+    static char data_gyr[8] = {0,0,0,0,0,0,0,0};
+    
+    // Request Read out from FIFO buff
+    char reg = 0x3f;
+    i2c->write(m_addr_acc, &reg, 1);// Select data_acc register
+    //  - Read out 6 bytes of data_acc X-Y-Z frame
+    //  - FRAME:
+    //  [xAccl lsb | xAccl msb | yAccl lsb | yAccl msb | zAccl lsb | zAccl msb]
+    i2c->read(m_addr_acc, data_acc, 6);
+    
+    i2c->write(m_addr_gyr, &reg, 1);// Select data_acc register
+    //  - Read 6 bytes out of a frame which contain 8 bytes
+    //  - FRAME:
+    //  [xGyro lsb | xGyro msb | yGyro lsb | yGyro msb | zGyro lsb | zGyro msb | 
+    //    status-0 | status-1  ]
+    //  - Discurding the frame it takes 1.5us 
+    //      but it is less than reading 2 bytes via I2C
+    i2c->read(m_addr_gyr, data_gyr, 8);
+    
+    // Convert the data_acc to 12-bits
+    //  top 4 bits should be same (Two's complement)
+    acc.x = (signed short)(((unsigned short)data_acc[1] << 8) | (unsigned short)(data_acc[0] & 0xf0)) >> 4;
+    acc.y = (signed short)(((unsigned short)data_acc[3] << 8) | (unsigned short)(data_acc[2] & 0xf0)) >> 4;
+    acc.z = (signed short)(((unsigned short)data_acc[5] << 8) | (unsigned short)(data_acc[4] & 0xf0)) >> 4;
+    // Convert the data_gyr to 16-bits
+    gyr.x = (signed short)(((unsigned short)data_gyr[1] << 8) | (unsigned short)data_gyr[0]);
+    gyr.y = (signed short)(((unsigned short)data_gyr[3] << 8) | (unsigned short)data_gyr[2]);
+    gyr.z = (signed short)(((unsigned short)data_gyr[5] << 8) | (unsigned short)data_gyr[4]);
+}
+
+void BMX055::Update()
+{
+    UpdateIMU();
+    
+    char buf;
+    unsigned short data_mag[8];
+    for (int i = 0; i < 8; i++) {
+        char reg = 0x42 + i;
+        i2c->write(m_addr_mag, &reg, 1);// Select data register
+        // Read 8 bytes of data
+        // xMag lsb, xMag msb, yMag lsb, yMag msb, zMag lsb, zMag msb
+        // + 2 bytes for control bits
+        i2c->read(m_addr_mag, &buf, 1);// Request 1 byte of data
+        data_mag[i] = (unsigned char)buf;
+    }
+
+    // Convert the data
+    mag.x = ((signed short)((data_mag[1] << 8) | data_mag[0])) >> 3;
+    mag.y = ((signed short)((data_mag[3] << 8) | data_mag[2])) >> 3;
+    mag.z = ((signed short)((data_mag[5] << 8) | data_mag[4])) >> 1;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BMX055.hpp	Fri Aug 21 03:11:36 2020 +0000
@@ -0,0 +1,84 @@
+#ifndef BMX055_HPP
+#define BMX055_HPP
+
+#include "mbed.h"
+
+//  Aux class
+template <typename T = float, unsigned char D = 3>
+class FixedLengthVector
+{
+public:
+    const unsigned char degree; // length of array, or num of elements
+    
+    T * const e; // elements
+    
+    //elements' alias
+    T &w, &x, &y, &z;
+    
+
+    FixedLengthVector (T* arg_init_vec = array_filled_0(D))
+        :degree(value_checked_degree(D))
+        ,e(new T[degree])
+        ,w(e[0])
+        ,x((degree > 3 ? e[1] : e[0]))
+        ,y((degree > 3 ? e[2] : e[1]))
+        ,z((degree > 3 ? e[3] : (degree == 3 ? e[2] : e[1])))
+    {        
+        for (int i = 0; i < degree; i++) e[i] = arg_init_vec[i];
+    }
+
+
+private:
+    static inline unsigned char value_checked_degree (unsigned char arg_degree)
+    {
+        if (arg_degree < 2) return 2;
+        else return arg_degree;
+    }
+    
+    static inline T * array_filled_0 (unsigned char arg_degree)
+    {
+        unsigned char l_degree = value_checked_degree(arg_degree);
+        T* tmp = new T[l_degree];
+
+        for(int i = 0; i < l_degree; i++) tmp[i] = 0;
+        return tmp;
+    }
+
+};
+
+class BMX055
+{
+public:
+    /** Main buffers which store 3 cubic data
+     * 
+     *  Acc: all x,y,z has 12 bits payload
+     *  Gyr: all x,y,z has 16 bits payload
+     *  Mag: x,y have 13 bits and z have 15 bits payload
+     */
+    FixedLengthVector<signed short, 3> acc, gyr, mag;
+    
+    // I2C pin of LPC 1768 is (p9, p10) or (p28, p27);
+    BMX055(
+        const char arg_addr_acc = 0x19, 
+        const char arg_addr_gyr = 0x69, 
+        const char arg_addr_mag = 0x13,
+        PinName arg_SDA = p9, 
+        PinName arg_SCL = p10)
+    : m_addr_acc(arg_addr_acc << 1)
+    , m_addr_gyr(arg_addr_gyr << 1)
+    , m_addr_mag(arg_addr_mag << 1)
+    {
+        Init_I2C(arg_SDA, arg_SCL);
+    }
+    void Update();
+    void UpdateIMU();
+    
+private:
+    I2C* i2c;
+    const char m_addr_acc;
+    const char m_addr_gyr;
+    const char m_addr_mag;
+    void Init_I2C(PinName arg_SDA, PinName arg_SCL);
+
+};
+#endif
\ No newline at end of file