Diff: ENGO333_MMA7660.cpp

Revision:

0:3bffc1862262

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ENGO333_MMA7660.cpp	Thu Nov 24 23:02:42 2016 +0000
@@ -0,0 +1,82 @@
+#include "ENGO333_MMA7660.h"
+
+const float GRAVITY = 9.8086;
+
+ENGO333_MMA7660::ENGO333_MMA7660() : i2c(p28, p27)
+{
+    SetActiveMode();
+    SetSamplingRateAM(MMA7660_AMSR8);
+    measAccel[0] = measAccel[1] = measAccel[2] = 0;
+}
+
+bool ENGO333_MMA7660::TestConnection() 
+{
+    char data[3] = {128, 128, 128};
+    bool alert;
+
+    // Check data validity
+    alert = false;
+    i2c.readBytes(MMA7660_ADDRESS, MMA7660_XOUT_REG, data, 3);
+    for (int i = 0; i < 3; ++i) {
+        if (data[i] > 63)
+            alert = true;
+    }
+    
+    if (alert == false) {
+        return true;
+    } else {
+        return false;
+    }
+}
+
+void ENGO333_MMA7660::SetActiveMode()
+{
+    i2c.writeOneByte(MMA7660_ADDRESS, MMA7660_MODE_REG, 0x01);
+}
+
+void ENGO333_MMA7660::SetSamplingRateAM(MMA7660_AMSR_t samplingRate)
+{
+    char oldValue, newValue;
+    oldValue = i2c.readOneByte(MMA7660_ADDRESS, MMA7660_SR_REG);
+    oldValue = oldValue & 0x07;
+    newValue = oldValue | samplingRate;
+    i2c.writeOneByte(MMA7660_ADDRESS, MMA7660_SR_REG, newValue);
+}
+
+void ENGO333_MMA7660::ReadAccelerometers()
+{
+    char data[3];
+    bool alert;
+
+    // Check data validity
+    do {
+        alert = false;
+        i2c.readBytes(MMA7660_ADDRESS, MMA7660_XOUT_REG, data, 3);
+        for (int i = 0; i < 3; ++i) {
+            if (data[i] > 63)
+                alert = true;
+            if (data[i] > 31)
+                data[i] += 128 + 64;
+        }
+    } while (alert);
+    
+    measAccel[0] = (signed char)(data[0]) / MMA7660_SCALE * GRAVITY;
+    measAccel[1] = (signed char)(data[1]) / MMA7660_SCALE * GRAVITY;
+    measAccel[2] = (signed char)(data[2]) / MMA7660_SCALE * GRAVITY;
+}
+
+float ENGO333_MMA7660::GetAccelX() const
+{
+    return measAccel[0];
+}
+
+float ENGO333_MMA7660::GetAccelY() const
+{
+    return measAccel[1];
+}
+
+float ENGO333_MMA7660::GetAccelZ() const
+{
+    return measAccel[2];
+}
+