File content as of revision 0:2562215f5bc0:

#include "FXOS8700Q.h"


FXOS8700Q::FXOS8700Q(PinName sda, PinName scl) : _i2c(sda, scl) {
 
    begin();
}
        
void FXOS8700Q::RegRead( char reg, char * d, int len) {
    char cmd[1];
    cmd[0] = reg;
    char i2c_addr = FXOS8700CQ_SLAVE_ADDR;
    _i2c.write( i2c_addr, cmd, 1);
    _i2c.read ( i2c_addr, d, len);
}

void FXOS8700Q::begin(void)
{
    char data[2];
    // write 0000 0000 = 0x00 to accelerometer control register 1 to place FXOS8700CQ into
    // standby
    // [7-1] = 0000 000
    // [0]: active=0
    data[0] = FXOS8700CQ_CTRL_REG1;
    data[1] = 0x00;
    _i2c.write( FXOS8700CQ_SLAVE_ADDR, data, 2);

    // write 0001 1111 = 0x1F to magnetometer control register 1
    // [7]: m_acal=0: auto calibration disabled
    // [6]: m_rst=0: no one-shot magnetic reset
    // [5]: m_ost=0: no one-shot magnetic measurement
    // [4-2]: m_os=111=7: 8x oversampling (for 200Hz) to reduce magnetometer noise
    // [1-0]: m_hms=11=3: select hybrid mode with accel and magnetometer active
    data[0] = FXOS8700CQ_M_CTRL_REG1;
    data[1] = 0x03; //0x1F;
    _i2c.write( FXOS8700CQ_SLAVE_ADDR, data, 2);
    
    // write 0010 0000 = 0x20 to magnetometer control register 2
    // [7]: reserved
    // [6]: reserved
    // [5]: hyb_autoinc_mode=1 to map the magnetometer registers to follow the
    // accelerometer registers
    // [4]: m_maxmin_dis=0 to retain default min/max latching even though not used
    // [3]: m_maxmin_dis_ths=0
    // [2]: m_maxmin_rst=0
    // [1-0]: m_rst_cnt=00 to enable magnetic reset each cycle
    data[0] = FXOS8700CQ_M_CTRL_REG2;
    data[1] = 0x20;
    _i2c.write( FXOS8700CQ_SLAVE_ADDR, data, 2);
    
    // write 0000 0001= 0x01 to XYZ_DATA_CFG register
    // [7]: reserved
    // [6]: reserved
    // [5]: reserved
    // [4]: hpf_out=0
    // [3]: reserved
    // [2]: reserved
    // [1-0]: fs=00 for accelerometer range of +/-2g range with 0.244mg/LSB
    data[0] = FXOS8700CQ_XYZ_DATA_CFG;
    data[1] = 0x00;
    _i2c.write( FXOS8700CQ_SLAVE_ADDR, data, 2);

    // write 0000 1101 = 0x0D to accelerometer control register 1
    // [7-6]: aslp_rate=00
    // [5-3]: dr=011 for 50Hz data rate (when in hybrid mode)
    // [2]: lnoise=1 for low noise mode
    // [1]: f_read=0 for normal 16 bit reads
    // [0]: active=1 to take the part out of standby and enable sampling
    data[0] = FXOS8700CQ_CTRL_REG1;
    data[1] = 0x19;//0x1D;
    _i2c.write( FXOS8700CQ_SLAVE_ADDR, data, 2);
}

void FXOS8700Q::ReadXYZ(float * a, float * m)
{
    char d[13];
    int16_t t[6];

    RegRead( FXOS8700CQ_STATUS, d, 13);
    t[0] = ((d[1] * 256) + ((unsigned short) d[2]));
    t[1] = ((d[3] * 256) + ((unsigned short) d[4]));
    t[2] = ((d[5] * 256) + ((unsigned short) d[6]));
    t[3] = ((d[7] * 256) + ((unsigned short) d[8]));
    t[4] = ((d[9] * 256) + ((unsigned short) d[10]));
    t[5] = ((d[11] * 256) + ((unsigned short) d[12]));

    a[0] = (float) t[0] / 16384.0;
    a[1] = (float) t[1] / 16384.0;
    a[2] = (float) t[2] / 16384.0;
    m[0] = (float) t[3] / 10.0;
    m[1] = (float) t[4] / 10.0;
    m[2] = (float) t[5] / 10.0;   
}