MMA7361L interface
Dependents: MMA7361L_Example ARLISS2012_Hidaka
Diff: MMA7361L.cpp
- Revision:
- 0:d66a303c5664
- Child:
- 1:19444721f19a
diff -r 000000000000 -r d66a303c5664 MMA7361L.cpp --- /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