Diff: Hardwares/IMUManager.cpp

Revision:

64:43ab429a37e0

Parent:

63:d9a81b3d69f5

--- a/Hardwares/IMUManager.cpp	Sun Apr 09 22:08:34 2017 +0000
+++ b/Hardwares/IMUManager.cpp	Mon Apr 10 16:44:31 2017 +0000
@@ -1,3 +1,5 @@
+#if 0
+
 #include "IMUManager.h"
 #include "PinAssignment.h"
 #include "math.h"
@@ -19,6 +21,7 @@
 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);
@@ -78,12 +81,14 @@
     imu_i2c_write_8bit(FXOS8700CQ_CTRL_REG2, &dataBuf);
     dataBuf = 0x00;
     imu_i2c_write_8bit(FXOS8700CQ_F_SETUP, &dataBuf);
-    dataBuf = 0x1F;
+    dataBuf = 0x00; //0x1F;
     imu_i2c_write_8bit(FXOS8700CQ_M_CTRL_REG1, &dataBuf);
-    dataBuf = 0x20;
+    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);
     
@@ -93,41 +98,54 @@
 void imu_manager_calibrate()
 {
     
-    static const int calibrate_sample_num = 10;
-    
+    static const int calibrate_sample_num = 1.0f / IMU_UPDATE_TICK_RATE;
     
     int16_t temp = 0;
-    float avgX = 0.0f;
-    float avgY = 0.0f;
-    
-    for(int i = 0; i < calibrate_sample_num; ++i)
+    for(int k = 0; k < 2; ++k)
     {
-        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)
+        double avgX = 0.0;
+        double avgY = 0.0;
+        for(int i = 0; i < calibrate_sample_num; ++i)
         {
-            temp = -1 * ((~(temp | 0xC000)) + 1);
+            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);
         }
-        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)
+        //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)
         {
-            temp = -1 * ((~(temp | 0xC000)) + 1);
+            accel_offset.x = static_cast<float>(avgX * 0.001);
+            accel_offset.y = static_cast<float>(avgY * 0.001);
         }
-        avgY += (static_cast<float>(temp) * ACCELER_SCALE_F_MG);
-        wait(IMU_UPDATE_TICK_RATE * 2);
+        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;
+        }
     }
-    //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);
@@ -160,11 +178,18 @@
 
 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;
     
-    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);
@@ -172,24 +197,24 @@
     {
         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);
+    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);
+    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);
+    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]);
@@ -203,6 +228,10 @@
     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();
 }
 
@@ -227,6 +256,13 @@
     return &position_value;
 }
 
+float imu_manager_get_temp()
+{
+    return static_cast<float>(imu_temp) * 0.96f;
+}
+
 #ifdef __cplusplus
 }
-#endif
\ No newline at end of file
+#endif
+
+#endif //if 0
\ No newline at end of file