Diff: BM1422AGMV.cpp

Revision:

0:203311da9813

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BM1422AGMV.cpp	Mon Feb 18 13:00:55 2019 +0900
@@ -0,0 +1,143 @@
+/* Copyright (c) 2015 ARM Ltd., MIT License
+*
+* Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+* and associated documentation files (the "Software"), to deal in the Software without
+* restriction, including without limitation the rights to use, copy, modify, merge, publish,
+* distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
+* Software is furnished to do so, subject to the following conditions:
+*
+* The above copyright notice and this permission notice shall be included in all copies or
+* substantial portions of the Software.
+*
+* 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 "mbed.h"
+#include "BM1422AGMV.h"
+
+BM1422AGMV::BM1422AGMV(PinName sda, PinName scl, int addr) : m_i2c(sda, scl), m_addr(addr)
+{
+    initialize();
+}
+
+BM1422AGMV::BM1422AGMV(I2C &i2c_obj, int addr) : m_i2c(i2c_obj), m_addr(addr)
+{
+    initialize();
+}
+
+BM1422AGMV::~BM1422AGMV()
+{
+    /* do nothing */
+}
+
+void BM1422AGMV::initialize(void)
+{
+    uint8_t reg;
+    uint8_t buf[3];
+
+    DEBUG_PRINT("BM1422AGMV init started\n\r");
+
+    readRegs(BM1422AGMV_WIA, &reg, 1);
+
+    if ( reg != BM1422AGMV_WIA_VAL) {
+        DEBUG_PRINT("BM1422AGMV initialization error. (WAI %d, not %d)\n\r", buf, BM1422AGMV_WIA);
+        DEBUG_PRINT("Trying to config anyway, in case there is some compatible sensor connected.\n\r");
+    }
+
+    // Step1
+    buf[0] = BM1422AGMV_CNTL1;
+    buf[1] = BM1422AGMV_CNTL1_VAL;
+    writeRegs(buf, 2);
+
+    // Check 12bit or 14bit
+    reg = (BM1422AGMV_CNTL1_VAL & BM1422AGMV_CNTL1_OUT_BIT);
+    if (reg == BM1422AGMV_CNTL1_OUT_BIT) {
+        _sens = BM1422AGMV_14BIT_SENS;
+    } else {
+        _sens = BM1422AGMV_12BIT_SENS;
+    }
+
+    wait(1);
+
+    buf[0] = BM1422AGMV_CNTL4;
+    buf[1] = (BM1422AGMV_CNTL4_VAL >> 8) & 0xFF;
+    buf[2] = (BM1422AGMV_CNTL4_VAL & 0xFF);
+    writeRegs(buf, 3);
+
+    // Step2
+    buf[0] = BM1422AGMV_CNTL2;
+    buf[1] = BM1422AGMV_CNTL2_VAL;
+    writeRegs(buf, 2);
+
+    // Step3
+    
+    // Option
+    buf[0] = BM1422AGMV_AVE_A;
+    buf[1] = BM1422AGMV_AVE_A_VAL;
+    writeRegs(buf, 2);
+}
+
+void BM1422AGMV::get_rawval(unsigned char *data)
+{
+    uint8_t reg;
+    uint8_t buf[2];
+
+    // Step 4
+    buf[0] = BM1422AGMV_CNTL3;
+    buf[1] = BM1422AGMV_CNTL3_VAL;
+    writeRegs(buf, 2);
+
+    while(1) {
+        readRegs(BM1422AGMV_STA1, &reg, 1);
+        if ( (reg>>6) == 1) {
+            break;
+        }
+        wait_ms(100);
+    }
+
+    readRegs(BM1422AGMV_DATAX, data, 6);
+}
+
+void BM1422AGMV::get_val(float *data)
+{
+    uint8_t val[6];
+    int16_t mag[3];
+    
+    get_rawval(val);
+    
+    mag[0] = ((int16_t)val[1] << 8) | (int16_t)(val[0]);
+    mag[1] = ((int16_t)val[3] << 8) | (int16_t)(val[2]);
+    mag[2] = ((int16_t)val[5] << 8) | (int16_t)(val[4]);
+    
+    convert_uT(mag, data);
+}
+
+void BM1422AGMV::convert_uT(int16_t *rawdata, float *data)
+{
+  // LSB to uT
+  data[0] = (float)rawdata[0] / _sens;
+  data[1] = (float)rawdata[1] / _sens;
+  data[2] = (float)rawdata[2] / _sens;
+}
+
+void BM1422AGMV::readRegs(int addr, uint8_t * data, int len)
+{
+    int read_nok;
+    char t[1] = {addr};
+    
+    m_i2c.write(m_addr, t, 1, true);
+    read_nok = m_i2c.read(m_addr, (char *)data, len);
+    if (read_nok){
+        DEBUG_PRINT("Read fail\n\r");
+        }
+}
+
+void BM1422AGMV::writeRegs(uint8_t * data, int len)
+{
+    m_i2c.write(m_addr, (char *)data, len);
+}