File content as of revision 2:606d0965f546:

/* FXAS21002CQ sensor driver
 * Copyright (c) 2015 WD
 *
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "FXAS21002CQ.h"


struct FXAS21002CQ_F_SETUP_struct F_SETUP = {CIRCULAR_BUFFER, 0};
struct FXAS21002CQ_CTRL_REG0_struct CTRL_REG0;
struct FXAS21002CQ_RT_CFG_struct RT_CFG;
struct FXAS21002CQ_RT_THS_struct RT_THS;
struct FXAS21002CQ_RT_COUNT_struct RT_COUNT;
struct FXAS21002CQ_CTRL_REG1_struct CTRL_REG1;
struct FXAS21002CQ_CTRL_REG2_struct CTRL_REG2;
struct FXAS21002CQ_CTRL_REG3_struct CTRL_REG3;


static int16_t dummy_int16_t = 0;
static float dummy_float = 0.0f;

FXAS21002CQ::FXAS21002CQ(I2C &i2c, const uint8_t addr, const struct FXAS21002CQ_Config_struct &FXAS21002CQ_Config)
{
    _i2c = &i2c;
    _addr = addr;
    /*
    // activate the peripheral
    uint8_t data[2] = {FXOS8700Q_CTRL_REG1, 0x00};
    _i2c->frequency(400000);
    writeRegs(data, 2);
    data[0] = FXOS8700Q_M_CTRL_REG1;
    data[1] = 0x1F;
    writeRegs(data, 2);
    data[0] = FXOS8700Q_M_CTRL_REG2;
    data[1] = 0x20;
    writeRegs(data, 2);
    data[0] = FXOS8700Q_XYZ_DATA_CFG;
    data[1] = 0x00;
    writeRegs(data, 2);
    data[0] = FXOS8700Q_CTRL_REG1;
    data[1] = 0x1C;
    writeRegs(data, 2);
    */
}

FXAS21002CQ::FXAS21002CQ(I2C &i2c, const uint8_t addr)
{
    const struct FXAS21002CQ_Config_struct FXAS21002CQ_Config = 
    {
        {CIRCULAR_BUFFER, 0}, // F_SETUP;
        {HIGH_BW, SPI4, LOW_HP, DISABLED_HP, RANGE_1000}, // CTRL_REG0;
        {DISABLED_ELE, DISABLED_ZTEVE, DISABLED_YTEVE, DISABLED_XTEVE}, // RT_CFG;
        {DECREMENT_CNT, 0}, // RT_THS;
        {0}, // RT_COUNT;
        {NOT_RST, NOT_SELF_TEST, ODR_50, ACTIVE_GYRO, READY_GYRO}, // CTRL_REG1;
        {FIFO_TO_INT2, FIFO_INT_DISABLE, RATE_TO_INT2, RATE_INT_DISABLE, DRDY_TO_INT2, DRDY_INT_DISABLE, INT_ACT_HIGH, INT_PUSH_PULL}, // CTRL_REG2;
        {WRAP_TO_ZERO, INT2_IS_INTERUPT, NOT_DOUBLE_RANGE} // CTRL_REG3;
    };
    
    FXAS21002CQ::FXAS21002CQ(i2c, addr, FXAS21002CQ_Config);
}

FXAS21002CQ::~FXAS21002CQ()
{
    _i2c = 0;
    _addr = 0;
}

void FXAS21002CQ::readRegs(uint8_t addr, uint8_t *data, uint32_t len) 
{
    uint8_t t[1] = {addr};
    _i2c->write(_addr, (char *)t, sizeof(t), true);
    _i2c->read(_addr, (char *)data, len);
}

uint8_t FXAS21002CQ::whoAmI() 
{
    uint8_t who_am_i = 0;
    readRegs(FXAS21002CQ_WHO_AM_I, &who_am_i, sizeof(who_am_i));
    return who_am_i;
}

void FXAS21002CQ::writeRegs(uint8_t * data, uint32_t len) const
{
    _i2c->write(_addr, (char *)data, len);
}

int16_t FXAS21002CQ::getSensorAxis(uint8_t addr) const
{
    uint8_t res[2];
//    readRegs(addr, res, sizeof(res));
    return static_cast<int16_t>((res[0] << 8) | res[1]);
}

void FXAS21002CQ::enable(void) const
{
    uint8_t data[2];
//    readRegs(FXOS8700Q_CTRL_REG1, &data[1], 1);
//    data[1] |= 0x01;
//    data[0] = FXOS8700Q_CTRL_REG1;
//    writeRegs(data, sizeof(data));
}

void FXAS21002CQ::disable(void) const
{
    uint8_t data[2];
//    readRegs(FXOS8700Q_CTRL_REG1, &data[1], 1);
//    data[1] &= 0xFE;
//    data[0] = FXOS8700Q_CTRL_REG1;
//    writeRegs(data, sizeof(data));
}

uint32_t FXAS21002CQ::dataReady(void) const
{
//    uint8_t stat = 0;
//    readRegs(FXOS8700Q_STATUS, &stat, 1);
    return (uint32_t)1;
}

uint32_t FXAS21002CQ::sampleRate(uint32_t frequency) const
{
    return(50); // for now sample rate is fixed at 50Hz
}

int16_t FXAS21002CQ::getX(int16_t &x = dummy_int16_t) const
{
 //   x = getSensorAxis(FXOS8700Q_OUT_X_MSB) >> 2;
 //   normalize_14bits(x);
    return x;
}

int16_t FXAS21002CQ::getY(int16_t &y = dummy_int16_t) const
{
//    y = getSensorAxis(FXOS8700Q_OUT_Y_MSB) >> 2;
//    normalize_14bits(y);
    return y;
}

int16_t FXAS21002CQ::getZ(int16_t &z = dummy_int16_t) const
{
//    z = getSensorAxis(FXOS8700Q_OUT_Z_MSB) >> 2;
//    normalize_14bits(z);
    return z;
}

float FXAS21002CQ::getX(float &x = dummy_float) const
{
//    int16_t val = getSensorAxis(FXOS8700Q_OUT_X_MSB) >> 2;
//    normalize_14bits(val);
//    x = val / 4096.0f;
    return x;
}

float FXAS21002CQ::getY(float &y = dummy_float) const
{
//    int16_t val = getSensorAxis(FXOS8700Q_OUT_Y_MSB) >> 2;
//    normalize_14bits(val);
//    y = val / 4096.0f;
    return y;
}

float FXAS21002CQ::getZ(float &z = dummy_float) const
{
//    int16_t val = getSensorAxis(FXOS8700Q_OUT_Z_MSB) >> 2;
//    normalize_14bits(val);
//    z = val / 4096.0f;
    return z;
}

void FXAS21002CQ::getAxis(motion_data_counts_t &xyz) const
{
    uint8_t res[6];
//    readRegs(FXOS8700Q_OUT_X_MSB, res, sizeof(res));
//    xyz.x = static_cast<int16_t>((res[0] << 8) | res[1]) >> 2;
//    xyz.y = static_cast<int16_t>((res[2] << 8) | res[3]) >> 2;
//    xyz.z = static_cast<int16_t>((res[4] << 8) | res[5]) >> 2;
//    normalize_14bits(xyz.x);
//    normalize_14bits(xyz.y);
//    normalize_14bits(xyz.z);
}

void FXAS21002CQ::getAxis(motion_data_units_t &xyz) const
{
//    motion_data_counts_t _xyz;
//    FXOS8700QAccelerometer::getAxis(_xyz);
//    xyz.x = _xyz.x / 4096.0f;
//    xyz.y = _xyz.y / 4096.0f;
//    xyz.z = _xyz.z / 4096.0f;
}

int8_t FXAS21002CQ::getTemperature(void)
{
    uint8_t res;
    readRegs(FXAS21002CQ_TEMP, &res, sizeof(res));
    return (int8_t)res;
}