Diff: MMA7361L.cpp

Revision:

0:d66a303c5664

Child:

1:19444721f19a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MMA7361L.cpp	Wed Apr 25 02:33:51 2012 +0000
@@ -0,0 +1,174 @@
+#include "MMA7361L.h"
+
+MMA7361L::MMA7361L(PinName xoutPin, PinName youtPin,PinName zoutPin,
+                   PinName zeroGDetectPin, PinName gSelectPin, PinName sleepPin) :
+        xout(xoutPin), yout(youtPin), zout(zoutPin),
+        zeroGDetect(zeroGDetectPin), gSelect(gSelectPin), sleep(sleepPin),
+        flags(-1) {
+    zeroGDetectEnabled = zeroGDetectPin != NC;
+    gSelectEnabled = gSelectPin != NC;
+    sleepEnabled = sleepPin != NC;
+    setSleep(false);
+    setScale(SCALE_1_5G);
+    for (int i = 0; i < 2; i++) {
+        calib[i].minX = -1;
+        calib[i].maxX = 1;
+        calib[i].minY = -1;
+        calib[i].maxY = 1;
+        calib[i].minZ = -1;
+        calib[i].maxZ = 1;
+    }
+}
+
+float MMA7361L::getAccel(bool forceRead) {
+    prepare(ACCEL, forceRead);
+    return sqrt(accelX * accelX + accelY * accelY + accelZ * accelZ);
+}
+
+float MMA7361L::getAccelX(bool forceRead) {
+    prepare(ACCEL_X, forceRead);
+    return accelX;
+}
+
+float MMA7361L::getAccelY(bool forceRead) {
+    prepare(ACCEL_Y, forceRead);
+    return accelY;
+}
+
+float MMA7361L::getAccelZ(bool forceRead) {
+    prepare(ACCEL_Z, forceRead);
+    return accelZ;
+}
+
+float MMA7361L::getTiltX(bool forceRead) {
+    prepare(TILT_X, forceRead);
+    return asin(accelX / sqrt(accelX * accelX + accelY * accelY + accelZ * accelZ));
+}
+
+float MMA7361L::getTiltY(bool forceRead) {
+    prepare(TILT_Y, forceRead);
+    return asin(accelY / sqrt(accelX * accelX + accelY * accelY + accelZ * accelZ));
+}
+
+float MMA7361L::getTiltZ(bool forceRead) {
+    prepare(TILT_Z, forceRead);
+    return asin(accelZ / sqrt(accelX * accelX + accelY * accelY + accelZ * accelZ));
+}
+
+void MMA7361L::setScale(Scale scale) {
+    this->scale = scale;
+    switch (scale) {
+        case SCALE_1_5G:
+            ratio = 0.8;
+            if (gSelectEnabled)
+                gSelect = 0;
+            break;
+        case SCALE_6G:
+            ratio = 0.206;
+            if (gSelectEnabled)
+                gSelect = 1;
+            break;
+    }
+}
+
+void MMA7361L::setSleep(bool on) {
+    if (sleepEnabled)
+        sleep = !on;
+}
+
+bool MMA7361L::zeroGDetected() {
+    if (zeroGDetectEnabled)
+        return zeroGDetect;
+    else
+        return false;
+}
+
+void MMA7361L::setZeroGDetectListener(void (*func)(void)) {
+    if (zeroGDetectEnabled)
+        zeroGDetect.rise(func);
+}
+
+template<typename T> void MMA7361L::setZeroGDetectListener(T* t, void (T::*func)(void)) {
+    if (zeroGDetectEnabled)
+        zeroG.rise(t, func);
+}
+
+void MMA7361L::prepare(int type, bool forceRead) {
+    if (forceRead) {
+        flags = -1;
+        switch (type) {
+            case ACCEL_X:
+                accelX = calibrate((xout - 0.5) * 3.3 / ratio, ACCEL_X, scale);
+                return;
+            case ACCEL_Y:
+                accelY = calibrate((yout - 0.5) * 3.3 / ratio, ACCEL_Y, scale);
+                return;
+            case ACCEL_Z:
+                accelZ = calibrate((zout - 0.5) * 3.3 / ratio, ACCEL_Z, scale);
+                return;
+        }
+    }
+
+    if (flags & type) {
+        if (!forceRead)
+            flags = type;
+        accelX = calibrate((xout - 0.5) * 3.3 / ratio, ACCEL_X, scale);
+        accelY = calibrate((yout - 0.5) * 3.3 / ratio, ACCEL_Y, scale);
+        accelZ = calibrate((zout - 0.5) * 3.3 / ratio, ACCEL_Z, scale);
+    } else
+        flags |= type;
+}
+
+void MMA7361L::printInfo() {
+    ::fprintf(stderr,
+              "zeroGDetectEnabled       = %d\n"
+              "gSelectEnabled           = %d\n"
+              "sleepEnabled             = %d\n"
+              "zeroGDetect(InterruptIn) = %d\n"
+              "gSelect(DigitalOut)      = %d\n"
+              "sleep(DigitalOut)        = %d\n"
+              "scale                    = %d\n"
+              "accelX, accelY, accelZ   = %4.3f, %4.3f, %4.3f\n"
+              "calib (1.5G)             = %4.3f, %4.3f; %4.3f, %4.3f; %4.3f, %4.3f\n"
+              "calib (6.0G)             = %4.3f, %4.3f; %4.3f, %4.3f; %4.3f, %4.3f\n"
+              "flags = %02X\n",
+              zeroGDetectEnabled,
+              gSelectEnabled,
+              sleepEnabled,
+              zeroGDetectEnabled ? zeroGDetect.read() : -1,
+              gSelectEnabled ? gSelect.read() : -1,
+              sleepEnabled ? sleep.read() : -1,
+              scale,
+              accelX, accelY, accelZ,
+              calib[0].minX, calib[0].maxX, calib[0].minY, calib[0].maxY, calib[0].minZ, calib[0].maxZ,
+              calib[1].minX, calib[1].maxX, calib[1].minY, calib[1].maxY, calib[1].minZ, calib[1].maxZ,
+              flags);
+}
+
+void MMA7361L::calibrate(Scale scale, float minX, float maxX, float minY, float maxY, float minZ, float maxZ) {
+    calib[scale].minX = minX;
+    calib[scale].maxX = maxX;
+    calib[scale].minY = minY;
+    calib[scale].maxY = maxY;
+    calib[scale].minZ = minZ;
+    calib[scale].maxZ = maxZ;
+}
+
+float MMA7361L::calibrate(float value, int type, Scale scale) {
+    float r = 1, s = 0;
+    switch (type) {
+        case ACCEL_X:
+            r = (calib[scale].maxX - calib[scale].minX) / 2;
+            s = (calib[scale].minX + calib[scale].maxX) / 2;
+            break;
+        case ACCEL_Y:
+            r = (calib[scale].maxY - calib[scale].minY) / 2;
+            s = (calib[scale].minY + calib[scale].maxY) / 2;
+            break;
+        case ACCEL_Z:
+            r = (calib[scale].maxZ - calib[scale].minZ) / 2;
+            s = (calib[scale].minZ + calib[scale].maxZ) / 2;
+            break;
+    }
+    return (value - s) / r;
+}
\ No newline at end of file