Diff: ENGO333_MPU9150.cpp

Revision:

0:3bffc1862262

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ENGO333_MPU9150.cpp	Thu Nov 24 23:02:42 2016 +0000
@@ -0,0 +1,72 @@
+#include "ENGO333_MPU9150.h"
+
+const float GRAVITY = 9.8086;
+
+ENGO333_MPU9150::ENGO333_MPU9150() : i2c(p28, p27)
+{
+    i2c.setSpeed(MPU9150_I2C_FAST_MODE);
+    setSleepMode(false);
+    setAccelRange(MPU9150_ACCEL_RANGE_2G);
+    
+    measAccel[0] = measAccel[1] = measAccel[2] = 0;
+}
+
+void ENGO333_MPU9150::setSleepMode(bool state)
+{
+    char temp;
+    temp = i2c.readOneByte(MPU9150_ADDRESS, MPU9150_PWR_MGMT_1_REG);
+    if (state == true)
+        temp |= 1 << MPU9150_SLEEP_BIT;
+    if (state == false)
+        temp &= ~(1 << MPU9150_SLEEP_BIT);
+    i2c.writeOneByte(MPU9150_ADDRESS, MPU9150_PWR_MGMT_1_REG, temp);
+}
+
+bool ENGO333_MPU9150::TestConnection()
+{
+    char temp;
+    temp = i2c.readOneByte(MPU9150_ADDRESS, MPU9150_WHO_AM_I_REG);
+    return (temp == (0x68 & 0xFE));
+}
+
+void ENGO333_MPU9150::setAccelRange(char range)
+{
+    char temp;
+    range = range & 0x03;
+    
+    temp = i2c.readOneByte(MPU9150_ADDRESS, MPU9150_ACCEL_CONFIG_REG);
+    temp &= ~(3 << 3);
+    temp = temp + (range << 3);
+    i2c.writeOneByte(MPU9150_ADDRESS, MPU9150_ACCEL_CONFIG_REG, temp);
+}
+
+void ENGO333_MPU9150::ReadAccelerometers()
+{
+    char temp[6];
+    
+    while ((i2c.readOneByte(MPU9150_ADDRESS, MPU9150_INT_STATUS_REG) & 0x01) != 0x01)
+    {
+        // wait data to be ready
+    }
+    
+    i2c.readBytes(MPU9150_ADDRESS, MPU9150_ACCEL_XOUT_H_REG, temp, 6);
+    measAccel[0] = ((short)((temp[0] << 8) + temp[1])) / 16384.0 * GRAVITY;
+    measAccel[1] = ((short)((temp[2] << 8) + temp[3])) / 16384.0 * GRAVITY;
+    measAccel[2] = ((short)((temp[4] << 8) + temp[5])) / 16384.0 * GRAVITY;
+}
+
+float ENGO333_MPU9150::GetAccelX() const
+{
+    return measAccel[0];
+}
+
+float ENGO333_MPU9150::GetAccelY() const
+{
+    return measAccel[1];
+}
+
+float ENGO333_MPU9150::GetAccelZ() const
+{
+    return measAccel[2];
+}
+