Diff: Hardwares/IMUManager.cpp

Revision:

62:bc5caf59fe39

Parent:

59:b709711bc566

Child:

63:d9a81b3d69f5

--- a/Hardwares/IMUManager.cpp	Sat Apr 08 17:40:13 2017 +0000
+++ b/Hardwares/IMUManager.cpp	Sun Apr 09 18:20:57 2017 +0000
@@ -1,6 +1,9 @@
 #include "IMUManager.h"
 #include "PinAssignment.h"
 
+//#define SW_DEBUG
+#include "SWCommon.h"
+
 #ifdef __cplusplus
 extern "C" {
 #endif
@@ -13,66 +16,87 @@
 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];
+static char imu_data_buffer[FXOS8700CQ_READ_LEN] = { 0 };
 
 inline void imu_i2c_write_8bit(const char addr, const char* buffer)
 {
-    imu_i2c_port.lock();
-    imu_i2c_port.write(SLAVE_ADDR_WRITE, &addr,  1, true);
-    imu_i2c_port.write(SLAVE_ADDR_WRITE, buffer, 1, false);
-    imu_i2c_port.unlock();
+    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)
 {
-    imu_i2c_port.lock();
-    imu_i2c_port.write(SLAVE_ADDR_WRITE, &addr,  1, true);
-    imu_i2c_port.read( SLAVE_ADDR_READ,  buffer, 1, false);
-    imu_i2c_port.unlock();
+    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)
 {
-    imu_i2c_port.lock();
-    imu_i2c_port.write(SLAVE_ADDR_WRITE, &addr,  1, true);
-    imu_i2c_port.read( SLAVE_ADDR_READ,  buffer, length, false);
-    imu_i2c_port.unlock();
+    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 0;   
+        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 = 0x01;
+    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 1;
+    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
-    accel_value.x = (((static_cast<uint16_t>(imu_data_buffer[1]) << 8) | imu_data_buffer[2]))>> 2;
-    accel_value.y = (((static_cast<uint16_t>(imu_data_buffer[3]) << 8) | imu_data_buffer[4]))>> 2;
-    accel_value.z = (((static_cast<uint16_t>(imu_data_buffer[5]) << 8) | imu_data_buffer[6]))>> 2;
+    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 = (static_cast<uint16_t>(imu_data_buffer[7]) << 8) | imu_data_buffer[8];
-    magt_value.y = (static_cast<uint16_t>(imu_data_buffer[9]) << 8) | imu_data_buffer[10];
-    magt_value.z = (static_cast<uint16_t>(imu_data_buffer[11]) << 8) | imu_data_buffer[12];
+    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()