Diff: BMX055.cpp

Revision:

25:fe14dbcef82d

Child:

26:1da7c6204775

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BMX055.cpp	Mon Jan 06 12:49:38 2020 +0000
@@ -0,0 +1,417 @@
+/*
+ * mbed library program
+ *  BMX055 Small, versatile 9-axis sensor module
+ *  by Bosch Sensortec
+ *
+ * Copyright (c) 2018,'19 Kenji Arai / JH1PJL
+ *  http://www.page.sannet.ne.jp/kenjia/index.html
+ *  http://mbed.org/users/kenjiArai/
+ *      Started:    October   24th, 2018
+ *      Revised:    March      3rd, 2019
+ */
+ 
+#include "mbed.h"
+#include "BMX055.h"
+ 
+#define DEBUG 0
+ 
+#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 (I2C &i2c) : i2c(i2c)
+{
+    //this->i2c = &i2c;
+    bmx055_parameters = bmx055_std_paramtr;
+    initialize ();
+}
+
+uint16_t BMX055::begin(void){
+    return 1;
+    }
+
+
+/////////////// Set parameter /////////////////////////////
+void BMX055::set_parameter(const BMX055_TypeDef *bmx055_parameter)
+{
+    bmx055_parameters = *bmx055_parameter;
+    set_parameters_to_regs();
+    
+}
+ 
+/////////////// Read data & normalize /////////////////////
+void BMX055::readAccel(void)
+{
+    int16_t x,y,z;
+ 
+    chip_addr = inf_addr.acc_addr;
+    dt[0] = 0x02;
+    i2c.write(chip_addr, dt, 1, true);
+    i2c.read(chip_addr, dt, 6, false);
+    x = dt[1] << 8 | (dt[0] & 0xf0);
+    y = dt[3] << 8 | (dt[2] & 0xf0);
+    z = dt[5] << 8 | (dt[4] & 0xf0);
+    accX = (double)x * acc_factor;
+    accY = (double)y * acc_factor;
+    accZ = (double)z * acc_factor;
+}
+ 
+void BMX055::readGyro(void)
+{
+    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);
+    x = dt[1] << 8 | dt[0];
+    y = dt[3] << 8 | dt[2];
+    z = dt[5] << 8 | dt[4];
+
+    gyroX = (double)x * gyr_factor;
+    gyroY = (double)y * gyr_factor;
+    gyroZ = (double)z * gyr_factor;
+}
+ 
+void BMX055::readMag(void)
+{
+    int16_t x,y,z,msb_data;
+    chip_addr = inf_addr.mag_addr;
+    dt[0] = 0x42;
+    i2c.write(chip_addr, dt, 1, true);
+    i2c.read(chip_addr, dt, 6, false);
+    // folgende Implementierung ist im original verkehrt!!!, verwende von https://github.com/kriswiner/BMX-055/blob/master/BMX055_MS5637_BasicAHRS_t3.ino
+    // Zeile 797 (altb Dez 2019)
+    x = (int16_t) (((int16_t)dt[1] << 8) | dt[0]) >> 3;  // 13-bit signed integer for x-axis field
+    y = (int16_t) (((int16_t)dt[3] << 8) | dt[2]) >> 3;  // 13-bit signed integer for x-axis field
+    z = (int16_t) (((int16_t)dt[5] << 8) | dt[4]) >> 1;  // 13-bit signed integer for x-axis field
+    
+    magX = (double)x * 0.01f;
+    magY = (double)y * 0.01f;
+    magZ = (double)z * 0.01f;
+}
+ 
+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(400000);
+    // Check Acc & Mag & Gyro are available of not
+    check_id();
+    if (ready_flag == 0x07){
+    }
+    // Set initial data
+    set_parameters_to_regs();
+    switch(bmx055_parameters.gyr_fs){
+        case GYR_2000DPS:
+            gyr_factor = 2000.0f*0.01745329f / 32768.0f;
+            break;
+        case GYR_1000DPS:
+            gyr_factor = 1000.0f*0.01745329f / 32768.0f;
+            break;
+        case GYR_500DPS:
+            gyr_factor = 500.0f*0.01745329f / 32768.0f;
+            break;
+        case GYR_250DPS:
+            gyr_factor = 250.0f*0.01745329f / 32768.0f;
+            break;
+        case GYR_125DPS:
+            gyr_factor = 125.0f*0.01745329f / 32768.0f;
+            break;
+        default:
+            gyr_factor = 0;
+            break;
+    }
+    switch(bmx055_parameters.acc_fs){
+        case ACC_2G:
+            acc_factor = 2.0f * 9.81f / 2048.0f / 16.0f;
+            break;
+        case ACC_4G:
+            acc_factor = 4.0f * 9.81f / 2048.0f / 16.0f;
+            break;
+        case ACC_8G:
+            acc_factor = 8.0f * 9.81f / 2048.0f / 16.0f;
+            break;
+        case ACC_16G:
+            acc_factor = 16.0f * 9.81f / 2048.0f / 16.0f;
+            break;
+        default:
+            acc_factor = 0;
+            break;
+    }
+
+
+}
+ 
+////// 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;
+                }
+            }
+        }
+    }
+#if DEBUG
+    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);
+#endif
+}
+ 
+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;
+}
\ No newline at end of file