Diff: Hardwares/IMUManager.cpp

Revision:

59:b709711bc566

Child:

62:bc5caf59fe39

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Hardwares/IMUManager.cpp	Sat Apr 08 17:40:13 2017 +0000
@@ -0,0 +1,90 @@
+#include "IMUManager.h"
+#include "PinAssignment.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];
+
+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();
+}
+
+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();
+}
+
+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();
+}
+
+uint8_t imu_manager_init()
+{
+    char dataBuf = 0;
+    imu_i2c_read_8bit(FXOS8700CQ_WHOAMI, &dataBuf);
+    if(dataBuf != FXOS8700CQ_WHOAMI_VAL)
+    {
+        return 0;   
+    }
+    
+    dataBuf = 0x00;
+    imu_i2c_write_8bit(FXOS8700CQ_CTRL_REG1, &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;
+    imu_i2c_write_8bit(FXOS8700CQ_XYZ_DATA_CFG, &dataBuf);
+    dataBuf = 0x0D;
+    imu_i2c_write_8bit(FXOS8700CQ_CTRL_REG1, &dataBuf);
+    
+    return 1;
+}
+
+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;
+    // 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];
+}
+
+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
\ No newline at end of file