BM1422AGMV.cpp

00001 /* Copyright (c) 2015 ARM Ltd., MIT License
00002 *
00003 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
00004 * and associated documentation files (the "Software"), to deal in the Software without
00005 * restriction, including without limitation the rights to use, copy, modify, merge, publish,
00006 * distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
00007 * Software is furnished to do so, subject to the following conditions:
00008 *
00009 * The above copyright notice and this permission notice shall be included in all copies or
00010 * substantial portions of the Software.
00011 *
00012 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
00013 * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
00014 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
00015 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00016 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
00017 *
00018 */
00019 
00020 #include "mbed.h"
00021 #include "BM1422AGMV.h"
00022 
00023 BM1422AGMV::BM1422AGMV(PinName sda, PinName scl, int addr) : m_i2c(sda, scl), m_addr(addr)
00024 {
00025     initialize();
00026 }
00027 
00028 BM1422AGMV::BM1422AGMV(I2C &i2c_obj, int addr) : m_i2c(i2c_obj), m_addr(addr)
00029 {
00030     initialize();
00031 }
00032 
00033 BM1422AGMV::~BM1422AGMV()
00034 {
00035     /* do nothing */
00036 }
00037 
00038 void BM1422AGMV::initialize(void)
00039 {
00040     uint8_t reg;
00041     uint8_t buf[3];
00042 
00043     DEBUG_PRINT("BM1422AGMV init started\n\r");
00044 
00045     readRegs(BM1422AGMV_WIA, &reg, 1);
00046 
00047     if ( reg != BM1422AGMV_WIA_VAL) {
00048         DEBUG_PRINT("BM1422AGMV initialization error. (WAI %d, not %d)\n\r", buf, BM1422AGMV_WIA);
00049         DEBUG_PRINT("Trying to config anyway, in case there is some compatible sensor connected.\n\r");
00050     }
00051 
00052     // Step1
00053     buf[0] = BM1422AGMV_CNTL1;
00054     buf[1] = BM1422AGMV_CNTL1_VAL;
00055     writeRegs(buf, 2);
00056 
00057     // Check 12bit or 14bit
00058     reg = (BM1422AGMV_CNTL1_VAL & BM1422AGMV_CNTL1_OUT_BIT);
00059     if (reg == BM1422AGMV_CNTL1_OUT_BIT) {
00060         _sens = BM1422AGMV_14BIT_SENS;
00061     } else {
00062         _sens = BM1422AGMV_12BIT_SENS;
00063     }
00064 
00065     wait(1);
00066 
00067     buf[0] = BM1422AGMV_CNTL4;
00068     buf[1] = (BM1422AGMV_CNTL4_VAL >> 8) & 0xFF;
00069     buf[2] = (BM1422AGMV_CNTL4_VAL & 0xFF);
00070     writeRegs(buf, 3);
00071 
00072     // Step2
00073     buf[0] = BM1422AGMV_CNTL2;
00074     buf[1] = BM1422AGMV_CNTL2_VAL;
00075     writeRegs(buf, 2);
00076 
00077     // Step3
00078     
00079     // Option
00080     buf[0] = BM1422AGMV_AVE_A;
00081     buf[1] = BM1422AGMV_AVE_A_VAL;
00082     writeRegs(buf, 2);
00083 }
00084 
00085 void BM1422AGMV::get_rawval(unsigned char *data)
00086 {
00087     uint8_t reg;
00088     uint8_t buf[2];
00089 
00090     // Step 4
00091     buf[0] = BM1422AGMV_CNTL3;
00092     buf[1] = BM1422AGMV_CNTL3_VAL;
00093     writeRegs(buf, 2);
00094 
00095     while(1) {
00096         readRegs(BM1422AGMV_STA1, &reg, 1);
00097         if ( (reg>>6) == 1) {
00098             break;
00099         }
00100         wait_ms(100);
00101     }
00102 
00103     readRegs(BM1422AGMV_DATAX, data, 6);
00104 }
00105 
00106 void BM1422AGMV::get_val(float *data)
00107 {
00108     uint8_t val[6];
00109     int16_t mag[3];
00110     
00111     get_rawval(val);
00112     
00113     mag[0] = ((int16_t)val[1] << 8) | (int16_t)(val[0]);
00114     mag[1] = ((int16_t)val[3] << 8) | (int16_t)(val[2]);
00115     mag[2] = ((int16_t)val[5] << 8) | (int16_t)(val[4]);
00116     
00117     convert_uT(mag, data);
00118 }
00119 
00120 void BM1422AGMV::convert_uT(int16_t *rawdata, float *data)
00121 {
00122   // LSB to uT
00123   data[0] = (float)rawdata[0] / _sens;
00124   data[1] = (float)rawdata[1] / _sens;
00125   data[2] = (float)rawdata[2] / _sens;
00126 }
00127 
00128 void BM1422AGMV::readRegs(int addr, uint8_t * data, int len)
00129 {
00130     int read_nok;
00131     char t[1] = {addr};
00132     
00133     m_i2c.write(m_addr, t, 1, true);
00134     read_nok = m_i2c.read(m_addr, (char *)data, len);
00135     if (read_nok){
00136         DEBUG_PRINT("Read fail\n\r");
00137         }
00138 }
00139 
00140 void BM1422AGMV::writeRegs(uint8_t * data, int len)
00141 {
00142     m_i2c.write(m_addr, (char *)data, len);
00143 }