File content as of revision 63:d9a81b3d69f5:

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

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

#ifdef __cplusplus
extern "C" {
#endif

#define KEEP_TWO_DECIMAL(X) ((float)((int)(X * 100)) / 100)

static const int SLAVE_ADDR_WRITE = (FXOS8700CQ_SLAVE_ADDR << 1);
static const int SLAVE_ADDR_READ  = (FXOS8700CQ_SLAVE_ADDR << 1) | 0x01;
static const float ACCELER_SCALE_F_G = ACCELER_SCALE_F_MG * 0.001f;

static volatile struct imu_vec3 accel_value;
static volatile struct imu_vec3 accel_offset;
static volatile struct imu_vec3 velocity_value;
static volatile struct imu_vec3 position_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 };
static Ticker m_imu_update_tick;
static Timer m_imu_update_timer;
static float m_imu_update_prev_time;

//Debug
static DebugCounter counter(10, PTE4);

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;
    velocity_value.x = velocity_value.y = velocity_value.z = 0;
    position_value.x = position_value.y = position_value.z = 0;
    
    char dataBuf = 0;
    imu_i2c_read_8bit(FXOS8700CQ_WHOAMI, &dataBuf);
    if(dataBuf != FXOS8700CQ_WHOAMI_VAL)
    {
        return dataBuf;
    }
    //standby
    dataBuf = 0x00;
    imu_i2c_write_8bit(FXOS8700CQ_CTRL_REG1, &dataBuf);
    //reset
    dataBuf = FXOS8700CQ_RESET_MASK;
    imu_i2c_write_8bit(FXOS8700CQ_CTRL_REG2, &dataBuf);
    wait(0.5);
    //standby
    dataBuf = 0x00;
    imu_i2c_write_8bit(FXOS8700CQ_CTRL_REG1, &dataBuf);
    
    dataBuf = 0x09;
    imu_i2c_write_8bit(FXOS8700CQ_CTRL_REG2, &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 = FXOS8700CQ_CTRL_REG1_v;
    imu_i2c_write_8bit(FXOS8700CQ_CTRL_REG1, &dataBuf);
    
    return FXOS8700CQ_WHOAMI_VAL;
}

void imu_manager_calibrate()
{
    
    static const int calibrate_sample_num = 10;
    
    
    int16_t temp = 0;
    float avgX = 0.0f;
    float avgY = 0.0f;
    
    for(int i = 0; i < calibrate_sample_num; ++i)
    {
        imu_i2c_read(FXOS8700CQ_STATUS, imu_data_buffer, FXOS8700CQ_READ_LEN);
        temp = ((((imu_data_buffer[1] << 8) | imu_data_buffer[2])) >> 2);
        if(temp & 0x2000)
        {
            temp = -1 * ((~(temp | 0xC000)) + 1);
            
        }
        avgX += (static_cast<float>(temp) * ACCELER_SCALE_F_MG);
        //LOGI("I: %5.3f, %d  ", avgX, temp);
        //wait(5.0f);
        temp = ((((imu_data_buffer[3] << 8) | imu_data_buffer[4])) >> 2);
        if(temp & 0x2000)
        {
            temp = -1 * ((~(temp | 0xC000)) + 1);
        }
        avgY += (static_cast<float>(temp) * ACCELER_SCALE_F_MG);
        wait(IMU_UPDATE_TICK_RATE * 2);
    }
    //Standby mode
    //char dataBuf = 0x00;
    //imu_i2c_write_8bit(FXOS8700CQ_CTRL_REG1, &dataBuf);
    
    avgX = avgX / calibrate_sample_num;
    avgY = avgY / calibrate_sample_num;
    accel_offset.x = avgX * -0.001;
    accel_offset.y = avgY * -0.001;
    accel_offset.z = 0;
    /*
    dataBuf = static_cast<char>((abs(avgX)) / OFFSET_SCALE_F);
    if(avgX > 0)
    {
        dataBuf = (~dataBuf) + 1;
    }
    //imu_i2c_write_8bit(FXOS8700CQ_OFF_X, &dataBuf);
    //LOGI("I: %5.3f, %d  ", avgX, dataBuf);
    //wait(5.0f);
    
    dataBuf = static_cast<char>((abs(avgY)) / OFFSET_SCALE_F);
    if(avgY > 0)
    {
        dataBuf = (~dataBuf) + 1;
    }
    //imu_i2c_write_8bit(FXOS8700CQ_OFF_Y, &dataBuf);
    */
    //Avtive mode.
    //dataBuf = FXOS8700CQ_CTRL_REG1_v;
    //imu_i2c_write_8bit(FXOS8700CQ_CTRL_REG1, &dataBuf);
}

void imu_manager_begin_tick()
{
    m_imu_update_prev_time = 0.0f;
    m_imu_update_tick.attach(&imu_manager_update, IMU_UPDATE_TICK_RATE);
    m_imu_update_timer.start();
}

void imu_manager_update()
{
    float currentTime = m_imu_update_timer.read();
    float deltaTime = currentTime - m_imu_update_prev_time;
    m_imu_update_prev_time = currentTime;
    
    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 = (static_cast<float>(temp) * ACCELER_SCALE_F_G) + accel_offset.x;
    accel_value.x = KEEP_TWO_DECIMAL(accel_value.x);
    
    temp = ((((imu_data_buffer[3] << 8) | imu_data_buffer[4])) >> 2);
    if(temp & 0x2000)
    {
        temp = -1 * ((~(temp | 0xC000)) + 1);
    }
    accel_value.y = (static_cast<float>(temp) * ACCELER_SCALE_F_G) + accel_offset.y;
    accel_value.y = KEEP_TWO_DECIMAL(accel_value.y);
    
    temp = ((((imu_data_buffer[5] << 8) | imu_data_buffer[6])) >> 2);
    if(temp & 0x2000)
    {
        temp = -1 * ((~(temp | 0xC000)) + 1);
    }
    accel_value.z = (static_cast<float>(temp) * ACCELER_SCALE_F_G) + accel_offset.z;
    accel_value.z = KEEP_TWO_DECIMAL(accel_value.z);
    /*
    // 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]);
    */
    
    velocity_value.x = velocity_value.x + (accel_value.x * IMU_DEFAULT_G * deltaTime);
    velocity_value.y = velocity_value.y + (accel_value.y * IMU_DEFAULT_G * deltaTime);
    
    position_value.x = position_value.x + (velocity_value.x * deltaTime);
    position_value.y = position_value.y + (velocity_value.y * deltaTime);
    
    counter.Update();
}

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

const volatile struct imu_vec3* imu_manager_get_velocity()
{
    return &velocity_value;
}

const volatile struct imu_vec3* imu_manager_get_position()
{
    return &position_value;
}

#ifdef __cplusplus
}
#endif