Diff: RemovedSources/IMUManager.cpp.txt

Branch:

Drift

Revision:

87:15fcf7891bf9

Parent:

64:43ab429a37e0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RemovedSources/IMUManager.cpp.txt	Wed Apr 19 04:06:01 2017 +0000
@@ -0,0 +1,268 @@
+#if 0
+
+#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 int8_t imu_temp = 0;
+//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 = 0x00; //0x1F;
+    imu_i2c_write_8bit(FXOS8700CQ_M_CTRL_REG1, &dataBuf);
+    dataBuf = 0x00; //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 = 0x10;
+    imu_i2c_write_8bit(FXOS8700CQ_HP_FILTER_CUTOFF, &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 = 1.0f / IMU_UPDATE_TICK_RATE;
+    
+    int16_t temp = 0;
+    for(int k = 0; k < 2; ++k)
+    {
+        double avgX = 0.0;
+        double avgY = 0.0;
+        for(int i = 0; i < calibrate_sample_num; ++i)
+        {
+            imu_data_buffer[0] = 0x00;
+            while(!(imu_data_buffer[0] & 0x0F))
+            {
+                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<double>(temp) * ACCELER_SCALE_F_MG);
+            
+            temp = ((((imu_data_buffer[3] << 8) | imu_data_buffer[4])) >> 2);
+            if(temp & 0x2000)
+            {
+                temp = -1 * ((~(temp | 0xC000)) + 1);
+            }
+            avgY += (static_cast<double>(temp) * ACCELER_SCALE_F_MG);
+            wait(IMU_UPDATE_TICK_RATE);
+        }
+        //Standby mode
+        //char dataBuf = 0x00;
+        //imu_i2c_write_8bit(FXOS8700CQ_CTRL_REG1, &dataBuf);
+        avgX = avgX / calibrate_sample_num;
+        avgY = avgY / calibrate_sample_num;
+        if(k == 0)
+        {
+            accel_offset.x = static_cast<float>(avgX * 0.001);
+            accel_offset.y = static_cast<float>(avgY * 0.001);
+        }
+        else
+        {
+            accel_offset.x = (accel_offset.x + static_cast<float>(avgX * 0.001)) / 2.0f;
+            accel_offset.y = (accel_offset.y + static_cast<float>(avgY * 0.001)) / 2.0f;
+        }
+    }
+    
+    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()
+{
+    imu_i2c_read(FXOS8700CQ_STATUS, imu_data_buffer, FXOS8700CQ_READ_LEN);
+    
+    if(!(imu_data_buffer[0] & 0x0F))
+    {
+        return;
+    }
+    
+    float currentTime = m_imu_update_timer.read();
+    float deltaTime = currentTime - m_imu_update_prev_time;
+    m_imu_update_prev_time = currentTime;
+    
+    
+    // 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);
+    
+    char dataBuf = 0;
+    imu_i2c_read_8bit(FXOS8700CQ_TEMP, &dataBuf);
+    imu_temp = (dataBuf & 0x80) ? (~dataBuf + 1) : dataBuf;
+    
+    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;
+}
+
+float imu_manager_get_temp()
+{
+    return static_cast<float>(imu_temp) * 0.96f;
+}
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif //if 0
\ No newline at end of file