File content as of revision 2:ce1a0d55f3c9:

#include "MMA7361L.h"

MMA7361L::MMA7361L(PinName xoutPin, PinName youtPin,PinName zoutPin,
                   PinName zeroGDetectPin, PinName gSelectPin, PinName nSleepPin) :
        xout(xoutPin), yout(youtPin), zout(zoutPin),
        zeroGDetect(zeroGDetectPin), gSelect(gSelectPin), nSleep(nSleepPin),
        flags(-1) {
    zeroGDetectEnabled = zeroGDetectPin != NC;
    gSelectEnabled = gSelectPin != NC;
    sleepEnabled = nSleepPin != 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;
    }
    wait_ms(10);
}

void MMA7361L::setSleep(bool on) {
    if (sleepEnabled)
        nSleep = !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"
              "nSleep(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 ? nSleep.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;
}