Diff: BMC050.cpp

Revision:

0:8de5e2fd5c48

Child:

1:b022f8d7884d

diff -r 000000000000 -r 8de5e2fd5c48 BMC050.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BMC050.cpp	Sun Jul 20 13:16:25 2014 +0000
@@ -0,0 +1,293 @@
+/*
+ * mbed library program 
+ *  BMC050 COMPASS 6 AXIS, made by Bosch Sensortec
+ *      http://jp.bosch-sensortec.com/content/language1/html/5033.htm
+ *
+ * Copyright (c) 2014 Kenji Arai / JH1PJL
+ *  http://www.page.sannet.ne.jp/kenjia/index.html
+ *  http://mbed.org/users/kenjiArai/
+ *      Created: July      19th, 2014 
+ *      Revised: July      20th, 2014
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
+ * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE
+ * AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+ * DAMAGES OR OTHER  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#include "BMC050.h"
+
+BMC050::BMC050 (PinName p_sda, PinName p_scl,
+    const BMC050ACC_TypeDef *acc_parameter, const BMC050MAG_TypeDef *mag_parameter) :
+    i2c(p_sda, p_scl) {
+    initialize (acc_parameter, mag_parameter);
+}
+
+BMC050::BMC050 (I2C& p_i2c,
+    const BMC050ACC_TypeDef *acc_parameter, const BMC050MAG_TypeDef *mag_parameter) : i2c(p_i2c) {
+    initialize (acc_parameter, mag_parameter);
+}
+
+void BMC050::initialize (const BMC050ACC_TypeDef *acc_parameter,
+                         const BMC050MAG_TypeDef *mag_parameter) {
+    /////////////// Magnetometer Configuration /////////////////
+    // after power-on, mag chip is keeping "Suspend mode"!!
+    //      -> Need to go "Normal mode" via "Sleep mode"
+    // step 1/start 
+    mag_addr = mag_parameter->addr;
+    dbf[0] = BMC050_M_POWER_MODE;
+    dbf[1] = 0x01;  // Power control bit on
+    i2c_write_n_bytes(mag_addr, dbf, 2);
+    dbf[0] = BMC050_M_OPERATION;
+    dbf[1] = 0;
+    i2c_write_n_bytes(mag_addr, dbf, 2);
+    /////////////// Accelerometer configuration ////////////////
+    // Check acc chip is available of not
+    // step 1/start
+    acc_addr = acc_parameter->addr;
+    dbf[0] = BMC050_A_WHO_AM_I; 
+    i2c_write_n_bytes(acc_addr, dbf, 1); 
+    i2c_read_n_bytes(acc_addr, dbf, 1);
+    if (dbf[0] == I_AM_BMC050_ACC){ acc_ready = 1;
+    } else {                        acc_ready = 0; }
+    /////////////// Magnetometer Configuration /////////////////
+    // after power-on, mag chip is keeping "Suspend mode"!!
+    //      -> Need to go "Normal mode" via "Sleep mode"
+    // step 2 
+    i2c_write_n_bytes(mag_addr, dbf, 2);
+    dbf[0] = BMC050_M_OPERATION;
+    dbf[1] = 0;
+    i2c_write_n_bytes(mag_addr, dbf, 2);
+    /////////////// Accelerometer configuration ////////////////
+    // step 2/last
+    if ( acc_ready == 1){
+        // set g-range
+        dbf[0] = BMC050_A_G_RANGE;
+        dbf[1] = acc_parameter->g_range;
+        switch (dbf[1]){
+        case BMC050_FS_2G:
+            fs_factor_acc = 256;
+            break;
+        case BMC050_FS_4G:
+            fs_factor_acc = 128;
+            break;
+        case BMC050_FS_8G:
+            fs_factor_acc = 64;
+            break;
+        case BMC050_FS_16G:
+            fs_factor_acc = 32;
+            break;
+        default:
+            fs_factor_acc = 256;
+            dbf[1] = BMC050_FS_2G;
+        }          
+        i2c_write_n_bytes(acc_addr, dbf, 2);
+        // set bandwidth
+        dbf[1] = acc_parameter->bandwith;
+        if (dbf[1] == BMC050_NOT_FILTERED){
+            dbf[0] = BMC050_A_FILTER;
+            dbf[1] = 0x80;
+        } else {
+            dbf[0] = BMC050_A_BANDWIDTH;
+        }
+        i2c_write_n_bytes(acc_addr, dbf, 2);
+    }
+    /////////////// Magnetometer Configuration /////////////////
+    // Check mag chip is available of not
+    // step 3/last
+    dbf[0] = BMC050_M_WHO_AM_I; 
+    i2c_write_n_bytes(mag_addr, dbf, 1); 
+    i2c_read_n_bytes(mag_addr, dbf, 1);
+    if (dbf[0] == I_AM_BMC050_MAG){ mag_ready = 1;
+    } else {                        mag_ready = 0; }
+    if ( mag_ready == 1){
+        // set output data rate
+        dbf[0] = BMC050_M_OPERATION;
+        dbf[1] = mag_parameter->data_rate;
+        i2c_write_n_bytes(mag_addr, dbf, 2);
+    }      
+}
+
+/////////////// Accelerometer //////////////////////////////////////////////////////
+void BMC050::read_data_acc(float *dt) {
+char data[6];
+
+    if (acc_ready == 0){
+        dt[0] = dt[1] = dt[2] = 0;
+        return;  
+    }
+    // X
+    dbf[0] = BMC050_A_OUT_X_L; 
+    i2c_write_n_bytes(acc_addr, dbf, 1); 
+    i2c_read_n_bytes(acc_addr, dbf, 1);
+    data[0] = dbf[0];
+    dbf[0] = BMC050_A_OUT_X_H         ; 
+    i2c_write_n_bytes(acc_addr, dbf, 1); 
+    i2c_read_n_bytes(acc_addr, dbf, 1);
+    data[1] = dbf[0];
+    // Y
+    dbf[0] = BMC050_A_OUT_Y_L; 
+    i2c_write_n_bytes(acc_addr, dbf, 1); 
+    i2c_read_n_bytes(acc_addr, dbf, 1);
+    data[2] = dbf[0];
+    dbf[0] = BMC050_A_OUT_Y_H; 
+    i2c_write_n_bytes(acc_addr, dbf, 1); 
+    i2c_read_n_bytes(acc_addr, dbf, 1);
+    data[3] = dbf[0];
+    // Z
+    dbf[0] = BMC050_A_OUT_Z_L; 
+    i2c_write_n_bytes(acc_addr, dbf, 1); 
+    i2c_read_n_bytes(acc_addr, dbf, 1);
+    data[4] = dbf[0];
+    dbf[0] = BMC050_A_OUT_Z_H; 
+    i2c_write_n_bytes(acc_addr, dbf, 1); 
+    i2c_read_n_bytes(acc_addr, dbf, 1);
+    data[5] = dbf[0];
+    // change data type
+    dt[0] = float((short(data[1] << 8 | data[0] & 0xc0))>> 6) * GRAVITY / fs_factor_acc;
+    dt[1] = float((short(data[3] << 8 | data[2] & 0xc0))>> 6) * GRAVITY / fs_factor_acc;
+    dt[2] = float((short(data[5] << 8 | data[4] & 0xc0))>> 6) * GRAVITY / fs_factor_acc;
+}
+
+/////////////// Magnetometer ///////////////////////////////////////////////////////
+void BMC050::read_data_mag(float *dt) {
+char data[6];
+
+    if (mag_ready == 0){
+        dt[0] = dt[1] = dt[2] = 0;
+        return;  
+    }
+    // X
+    dbf[0] = BMC050_M_OUT_X_L; 
+    i2c_write_n_bytes(mag_addr, dbf, 1); 
+    i2c_read_n_bytes(mag_addr, dbf, 1);
+    data[0] = dbf[0];
+    dbf[0] = BMC050_M_OUT_X_H         ; 
+    i2c_write_n_bytes(mag_addr, dbf, 1); 
+    i2c_read_n_bytes(mag_addr, dbf, 1);
+    data[1] = dbf[0];
+    // Y
+    dbf[0] = BMC050_M_OUT_Y_L; 
+    i2c_write_n_bytes(mag_addr, dbf, 1); 
+    i2c_read_n_bytes(mag_addr, dbf, 1);
+    data[2] = dbf[0];
+    dbf[0] = BMC050_M_OUT_Y_H; 
+    i2c_write_n_bytes(mag_addr, dbf, 1); 
+    i2c_read_n_bytes(mag_addr, dbf, 1);
+    data[3] = dbf[0];
+    // Z
+    dbf[0] = BMC050_M_OUT_Z_L; 
+    i2c_write_n_bytes(mag_addr, dbf, 1); 
+    i2c_read_n_bytes(mag_addr, dbf, 1);
+    data[4] = dbf[0];
+    dbf[0] = BMC050_M_OUT_Z_H; 
+    i2c_write_n_bytes(mag_addr, dbf, 1); 
+    i2c_read_n_bytes(mag_addr, dbf, 1);
+    data[5] = dbf[0];
+    // change data type
+    dt[0] = float((short(data[1] << 8 | data[0] & 0xf8))>> 3);
+    dt[1] = float((short(data[3] << 8 | data[2] & 0xf8))>> 3);
+    dt[2] = float((short(data[5] << 8 | data[4] & 0xfe))>> 1);
+}
+
+/////////////// Accelerometer //////////////////////////////////////////////////////
+float BMC050::read_temp() {
+    dbf[0] = BMC050_A_OUT_TEMP; 
+    i2c_write_n_bytes(acc_addr, dbf, 1); 
+    i2c_read_n_bytes(acc_addr, dbf, 1);
+    return ((float)dbf[0] * 0.5 + 24.0);
+}
+
+/////////////// Accelerometer //////////////////////////////////////////////////////
+uint8_t BMC050::read_id_acc() {
+    dbf[0] = BMC050_A_WHO_AM_I; 
+    i2c_write_n_bytes(acc_addr, dbf, 1); 
+    i2c_read_n_bytes(acc_addr, dbf, 1);
+    return (uint8_t)dbf[0];
+}
+
+/////////////// Magnetometer ///////////////////////////////////////////////////////
+uint8_t BMC050::read_id_mag() {
+    dbf[0] = BMC050_M_WHO_AM_I; 
+    i2c_write_n_bytes(mag_addr, dbf, 1); 
+    i2c_read_n_bytes(mag_addr, dbf, 1);
+    return (uint8_t)dbf[0];
+}
+
+/////////////// Accelerometer //////////////////////////////////////////////////////
+uint8_t BMC050::data_ready_acc() {
+    if (acc_ready == 1){
+        dbf[0] = BMC050_A_OUT_X_L; 
+        i2c_write_n_bytes(acc_addr, dbf, 1); 
+        i2c_read_n_bytes(acc_addr, dbf, 1);
+        if (!(dbf[0] & 0x01)){
+            return 0;
+        }
+    }
+    return 1;
+}
+
+/////////////// Magnetometer ///////////////////////////////////////////////////////
+uint8_t BMC050::data_ready_mag() {
+    if (mag_ready == 1){
+        dbf[0] = BMC050_M_HALL_L; 
+        i2c_write_n_bytes(mag_addr, dbf, 1); 
+        i2c_read_n_bytes(mag_addr, dbf, 1);
+        if (!(dbf[0] & 0x01)){
+            return 0;
+        }
+    }
+    return 1;
+}
+
+/////////////// Accelerometer //////////////////////////////////////////////////////
+uint8_t BMC050::read_reg_acc(uint8_t addr) {
+    if (acc_ready == 1){
+        dbf[0] = addr; 
+        i2c_write_n_bytes(acc_addr, dbf, 1); 
+        i2c_read_n_bytes(acc_addr, dbf, 1);
+    } else {
+        dbf[0] = 0xff;
+    }
+    return (uint8_t)dbf[0];
+}
+
+/////////////// Magnetometer ///////////////////////////////////////////////////////
+uint8_t BMC050::read_reg_mag(uint8_t addr) {
+    if (mag_ready == 1){
+        dbf[0] = addr; 
+        i2c_write_n_bytes(mag_addr, dbf, 1); 
+        i2c_read_n_bytes(mag_addr, dbf, 1);
+    } else {
+        dbf[0] = 0xff;
+    }
+    return (uint8_t)dbf[0];
+}
+
+/////////////// Accelerometer //////////////////////////////////////////////////////
+void BMC050::write_reg_acc(uint8_t addr, uint8_t data) {
+    if (acc_ready == 1){
+        dbf[0] = addr;
+        dbf[1] = data; 
+        i2c_write_n_bytes(acc_addr, dbf, 2); 
+    }
+}
+
+/////////////// Magnetometer ///////////////////////////////////////////////////////
+void BMC050::write_reg_mag(uint8_t addr, uint8_t data) {
+    if (mag_ready == 1){
+        dbf[0] = addr;
+        dbf[1] = data; 
+        i2c_write_n_bytes(mag_addr, dbf, 2); 
+    }
+}
+
+/////////////// Low level interface ////////////////////////////////////////////////
+void BMC050::i2c_read_n_bytes (int addr, char* dbf, int n) {
+    i2c.read(addr, dbf, n);
+}
+
+void BMC050::i2c_write_n_bytes (int addr, char* dbf, int n) {
+    i2c.write(addr, dbf, n);
+}