File content as of revision 62:bc5caf59fe39:

#include "IMUManager.h"
#include "PinAssignment.h"

//#define SW_DEBUG
#include "SWCommon.h"

#ifdef __cplusplus
extern "C" {
#endif

static const int SLAVE_ADDR_WRITE = (FXOS8700CQ_SLAVE_ADDR << 1);
static const int SLAVE_ADDR_READ  = (FXOS8700CQ_SLAVE_ADDR << 1) | 0x01;

static volatile struct imu_vec3 accel_value;
static volatile struct imu_vec3 magt_value;
static I2C imu_i2c_port(PIN_IMC_SDA, PIN_IMC_SCL);
static DigitalOut imu_accl_sa0(PIN_IMC_ACCL_SA0, ACCEL_MAG_SA0);
static DigitalOut imu_accl_sa1(PIN_IMC_ACCL_SA1, ACCEL_MAG_SA1);
static char imu_data_buffer[FXOS8700CQ_READ_LEN] = { 0 };

inline void imu_i2c_write_8bit(const char addr, const char* buffer)
{
    static char write_buf[2];
    write_buf[0] = addr;
    write_buf[1] = *buffer;
    imu_i2c_port.write(SLAVE_ADDR_WRITE, write_buf,  2, false);
}

inline void imu_i2c_read_8bit(const char addr, char* buffer)
{
    int t1 = imu_i2c_port.write(SLAVE_ADDR_WRITE, &addr,  1, true);
    int t2 = imu_i2c_port.read( SLAVE_ADDR_READ,  buffer, 1, false);
}

inline void imu_i2c_read(const char addr, char* buffer, const int length)
{
    int t1 = imu_i2c_port.write(SLAVE_ADDR_WRITE, &addr,  1, true);
    int t2 = imu_i2c_port.read( SLAVE_ADDR_READ,  buffer, length, false);
}

uint8_t imu_manager_init()
{
    accel_value.x = accel_value.y = accel_value.z = 0;
    magt_value.x = magt_value.y = magt_value.z = 0;
    
    char dataBuf = 0;
    imu_i2c_read_8bit(FXOS8700CQ_WHOAMI, &dataBuf);
    if(dataBuf != FXOS8700CQ_WHOAMI_VAL)
    {
        return dataBuf;
    }
    
    dataBuf = 0x00;
    imu_i2c_write_8bit(FXOS8700CQ_CTRL_REG1, &dataBuf);
    dataBuf = 0x00;
    imu_i2c_write_8bit(FXOS8700CQ_F_SETUP, &dataBuf);
    dataBuf = 0x1F;
    imu_i2c_write_8bit(FXOS8700CQ_M_CTRL_REG1, &dataBuf);
    dataBuf = 0x20;
    imu_i2c_write_8bit(FXOS8700CQ_M_CTRL_REG2, &dataBuf);
    dataBuf = FXOS8700CQ_XYZ_DATA_SC;
    imu_i2c_write_8bit(FXOS8700CQ_XYZ_DATA_CFG, &dataBuf);
    dataBuf = 0x0D;
    imu_i2c_write_8bit(FXOS8700CQ_CTRL_REG1, &dataBuf);
    
    return FXOS8700CQ_WHOAMI_VAL;
}

void imu_manager_update()
{
    imu_i2c_read(FXOS8700CQ_STATUS, imu_data_buffer, FXOS8700CQ_READ_LEN);
    // copy the 14 bit accelerometer byte data into 16 bit words
    static int16_t temp = 0;
    temp = ((((imu_data_buffer[1] << 8) | imu_data_buffer[2])) >> 2);
    if(temp & 0x2000)
    {
        temp = -1 * ((~(temp | 0xC000)) + 1);
    }
    accel_value.x = temp * ACCELER_SCALE_F;
    
    temp = ((((imu_data_buffer[3] << 8) | imu_data_buffer[4])) >> 2);
    if(temp & 0x2000)
    {
        temp = -1 * ((~(temp | 0xC000)) + 1);
    }
    accel_value.y = temp * ACCELER_SCALE_F;
    
    temp = ((((imu_data_buffer[5] << 8) | imu_data_buffer[6])) >> 2);
    if(temp & 0x2000)
    {
        temp = -1 * ((~(temp | 0xC000)) + 1);
    }
    accel_value.z = temp * ACCELER_SCALE_F;
    /*
    // copy the magnetometer byte data into 16 bit words
    magt_value.x = (((imu_data_buffer[7]) << 8) | imu_data_buffer[8]);
    magt_value.y = (((imu_data_buffer[9]) << 8) | imu_data_buffer[10]);
    magt_value.z = (((imu_data_buffer[11]) << 8) | imu_data_buffer[12]);
    */
}

const volatile struct imu_vec3* imu_manager_get_accl()
{
    return &accel_value;
}

const volatile struct imu_vec3* imu_manager_get_magt()
{
    return &magt_value;
}

#ifdef __cplusplus
}
#endif