MMA7361L interface
Dependents: MMA7361L_Example ARLISS2012_Hidaka
MMA7361L.cpp@1:19444721f19a, 2012-04-25 (annotated)
- Committer:
- yamaguch
- Date:
- Wed Apr 25 10:13:30 2012 +0000
- Revision:
- 1:19444721f19a
- Parent:
- 0:d66a303c5664
- Child:
- 2:ce1a0d55f3c9
added 10 ms wait after scale change
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
yamaguch | 0:d66a303c5664 | 1 | #include "MMA7361L.h" |
yamaguch | 0:d66a303c5664 | 2 | |
yamaguch | 0:d66a303c5664 | 3 | MMA7361L::MMA7361L(PinName xoutPin, PinName youtPin,PinName zoutPin, |
yamaguch | 0:d66a303c5664 | 4 | PinName zeroGDetectPin, PinName gSelectPin, PinName sleepPin) : |
yamaguch | 0:d66a303c5664 | 5 | xout(xoutPin), yout(youtPin), zout(zoutPin), |
yamaguch | 0:d66a303c5664 | 6 | zeroGDetect(zeroGDetectPin), gSelect(gSelectPin), sleep(sleepPin), |
yamaguch | 0:d66a303c5664 | 7 | flags(-1) { |
yamaguch | 0:d66a303c5664 | 8 | zeroGDetectEnabled = zeroGDetectPin != NC; |
yamaguch | 0:d66a303c5664 | 9 | gSelectEnabled = gSelectPin != NC; |
yamaguch | 0:d66a303c5664 | 10 | sleepEnabled = sleepPin != NC; |
yamaguch | 0:d66a303c5664 | 11 | setSleep(false); |
yamaguch | 0:d66a303c5664 | 12 | setScale(SCALE_1_5G); |
yamaguch | 0:d66a303c5664 | 13 | for (int i = 0; i < 2; i++) { |
yamaguch | 0:d66a303c5664 | 14 | calib[i].minX = -1; |
yamaguch | 0:d66a303c5664 | 15 | calib[i].maxX = 1; |
yamaguch | 0:d66a303c5664 | 16 | calib[i].minY = -1; |
yamaguch | 0:d66a303c5664 | 17 | calib[i].maxY = 1; |
yamaguch | 0:d66a303c5664 | 18 | calib[i].minZ = -1; |
yamaguch | 0:d66a303c5664 | 19 | calib[i].maxZ = 1; |
yamaguch | 0:d66a303c5664 | 20 | } |
yamaguch | 0:d66a303c5664 | 21 | } |
yamaguch | 0:d66a303c5664 | 22 | |
yamaguch | 0:d66a303c5664 | 23 | float MMA7361L::getAccel(bool forceRead) { |
yamaguch | 0:d66a303c5664 | 24 | prepare(ACCEL, forceRead); |
yamaguch | 0:d66a303c5664 | 25 | return sqrt(accelX * accelX + accelY * accelY + accelZ * accelZ); |
yamaguch | 0:d66a303c5664 | 26 | } |
yamaguch | 0:d66a303c5664 | 27 | |
yamaguch | 0:d66a303c5664 | 28 | float MMA7361L::getAccelX(bool forceRead) { |
yamaguch | 0:d66a303c5664 | 29 | prepare(ACCEL_X, forceRead); |
yamaguch | 0:d66a303c5664 | 30 | return accelX; |
yamaguch | 0:d66a303c5664 | 31 | } |
yamaguch | 0:d66a303c5664 | 32 | |
yamaguch | 0:d66a303c5664 | 33 | float MMA7361L::getAccelY(bool forceRead) { |
yamaguch | 0:d66a303c5664 | 34 | prepare(ACCEL_Y, forceRead); |
yamaguch | 0:d66a303c5664 | 35 | return accelY; |
yamaguch | 0:d66a303c5664 | 36 | } |
yamaguch | 0:d66a303c5664 | 37 | |
yamaguch | 0:d66a303c5664 | 38 | float MMA7361L::getAccelZ(bool forceRead) { |
yamaguch | 0:d66a303c5664 | 39 | prepare(ACCEL_Z, forceRead); |
yamaguch | 0:d66a303c5664 | 40 | return accelZ; |
yamaguch | 0:d66a303c5664 | 41 | } |
yamaguch | 0:d66a303c5664 | 42 | |
yamaguch | 0:d66a303c5664 | 43 | float MMA7361L::getTiltX(bool forceRead) { |
yamaguch | 0:d66a303c5664 | 44 | prepare(TILT_X, forceRead); |
yamaguch | 0:d66a303c5664 | 45 | return asin(accelX / sqrt(accelX * accelX + accelY * accelY + accelZ * accelZ)); |
yamaguch | 0:d66a303c5664 | 46 | } |
yamaguch | 0:d66a303c5664 | 47 | |
yamaguch | 0:d66a303c5664 | 48 | float MMA7361L::getTiltY(bool forceRead) { |
yamaguch | 0:d66a303c5664 | 49 | prepare(TILT_Y, forceRead); |
yamaguch | 0:d66a303c5664 | 50 | return asin(accelY / sqrt(accelX * accelX + accelY * accelY + accelZ * accelZ)); |
yamaguch | 0:d66a303c5664 | 51 | } |
yamaguch | 0:d66a303c5664 | 52 | |
yamaguch | 0:d66a303c5664 | 53 | float MMA7361L::getTiltZ(bool forceRead) { |
yamaguch | 0:d66a303c5664 | 54 | prepare(TILT_Z, forceRead); |
yamaguch | 0:d66a303c5664 | 55 | return asin(accelZ / sqrt(accelX * accelX + accelY * accelY + accelZ * accelZ)); |
yamaguch | 0:d66a303c5664 | 56 | } |
yamaguch | 0:d66a303c5664 | 57 | |
yamaguch | 0:d66a303c5664 | 58 | void MMA7361L::setScale(Scale scale) { |
yamaguch | 0:d66a303c5664 | 59 | this->scale = scale; |
yamaguch | 0:d66a303c5664 | 60 | switch (scale) { |
yamaguch | 0:d66a303c5664 | 61 | case SCALE_1_5G: |
yamaguch | 0:d66a303c5664 | 62 | ratio = 0.8; |
yamaguch | 0:d66a303c5664 | 63 | if (gSelectEnabled) |
yamaguch | 0:d66a303c5664 | 64 | gSelect = 0; |
yamaguch | 0:d66a303c5664 | 65 | break; |
yamaguch | 0:d66a303c5664 | 66 | case SCALE_6G: |
yamaguch | 0:d66a303c5664 | 67 | ratio = 0.206; |
yamaguch | 0:d66a303c5664 | 68 | if (gSelectEnabled) |
yamaguch | 0:d66a303c5664 | 69 | gSelect = 1; |
yamaguch | 0:d66a303c5664 | 70 | break; |
yamaguch | 0:d66a303c5664 | 71 | } |
yamaguch | 1:19444721f19a | 72 | wait_ms(10); |
yamaguch | 0:d66a303c5664 | 73 | } |
yamaguch | 0:d66a303c5664 | 74 | |
yamaguch | 0:d66a303c5664 | 75 | void MMA7361L::setSleep(bool on) { |
yamaguch | 0:d66a303c5664 | 76 | if (sleepEnabled) |
yamaguch | 0:d66a303c5664 | 77 | sleep = !on; |
yamaguch | 0:d66a303c5664 | 78 | } |
yamaguch | 0:d66a303c5664 | 79 | |
yamaguch | 0:d66a303c5664 | 80 | bool MMA7361L::zeroGDetected() { |
yamaguch | 0:d66a303c5664 | 81 | if (zeroGDetectEnabled) |
yamaguch | 0:d66a303c5664 | 82 | return zeroGDetect; |
yamaguch | 0:d66a303c5664 | 83 | else |
yamaguch | 0:d66a303c5664 | 84 | return false; |
yamaguch | 0:d66a303c5664 | 85 | } |
yamaguch | 0:d66a303c5664 | 86 | |
yamaguch | 0:d66a303c5664 | 87 | void MMA7361L::setZeroGDetectListener(void (*func)(void)) { |
yamaguch | 0:d66a303c5664 | 88 | if (zeroGDetectEnabled) |
yamaguch | 0:d66a303c5664 | 89 | zeroGDetect.rise(func); |
yamaguch | 0:d66a303c5664 | 90 | } |
yamaguch | 0:d66a303c5664 | 91 | |
yamaguch | 0:d66a303c5664 | 92 | template<typename T> void MMA7361L::setZeroGDetectListener(T* t, void (T::*func)(void)) { |
yamaguch | 0:d66a303c5664 | 93 | if (zeroGDetectEnabled) |
yamaguch | 0:d66a303c5664 | 94 | zeroG.rise(t, func); |
yamaguch | 0:d66a303c5664 | 95 | } |
yamaguch | 0:d66a303c5664 | 96 | |
yamaguch | 0:d66a303c5664 | 97 | void MMA7361L::prepare(int type, bool forceRead) { |
yamaguch | 0:d66a303c5664 | 98 | if (forceRead) { |
yamaguch | 0:d66a303c5664 | 99 | flags = -1; |
yamaguch | 0:d66a303c5664 | 100 | switch (type) { |
yamaguch | 0:d66a303c5664 | 101 | case ACCEL_X: |
yamaguch | 0:d66a303c5664 | 102 | accelX = calibrate((xout - 0.5) * 3.3 / ratio, ACCEL_X, scale); |
yamaguch | 0:d66a303c5664 | 103 | return; |
yamaguch | 0:d66a303c5664 | 104 | case ACCEL_Y: |
yamaguch | 0:d66a303c5664 | 105 | accelY = calibrate((yout - 0.5) * 3.3 / ratio, ACCEL_Y, scale); |
yamaguch | 0:d66a303c5664 | 106 | return; |
yamaguch | 0:d66a303c5664 | 107 | case ACCEL_Z: |
yamaguch | 0:d66a303c5664 | 108 | accelZ = calibrate((zout - 0.5) * 3.3 / ratio, ACCEL_Z, scale); |
yamaguch | 0:d66a303c5664 | 109 | return; |
yamaguch | 0:d66a303c5664 | 110 | } |
yamaguch | 0:d66a303c5664 | 111 | } |
yamaguch | 0:d66a303c5664 | 112 | |
yamaguch | 0:d66a303c5664 | 113 | if (flags & type) { |
yamaguch | 0:d66a303c5664 | 114 | if (!forceRead) |
yamaguch | 0:d66a303c5664 | 115 | flags = type; |
yamaguch | 0:d66a303c5664 | 116 | accelX = calibrate((xout - 0.5) * 3.3 / ratio, ACCEL_X, scale); |
yamaguch | 0:d66a303c5664 | 117 | accelY = calibrate((yout - 0.5) * 3.3 / ratio, ACCEL_Y, scale); |
yamaguch | 0:d66a303c5664 | 118 | accelZ = calibrate((zout - 0.5) * 3.3 / ratio, ACCEL_Z, scale); |
yamaguch | 0:d66a303c5664 | 119 | } else |
yamaguch | 0:d66a303c5664 | 120 | flags |= type; |
yamaguch | 0:d66a303c5664 | 121 | } |
yamaguch | 0:d66a303c5664 | 122 | |
yamaguch | 0:d66a303c5664 | 123 | void MMA7361L::printInfo() { |
yamaguch | 0:d66a303c5664 | 124 | ::fprintf(stderr, |
yamaguch | 0:d66a303c5664 | 125 | "zeroGDetectEnabled = %d\n" |
yamaguch | 0:d66a303c5664 | 126 | "gSelectEnabled = %d\n" |
yamaguch | 0:d66a303c5664 | 127 | "sleepEnabled = %d\n" |
yamaguch | 0:d66a303c5664 | 128 | "zeroGDetect(InterruptIn) = %d\n" |
yamaguch | 0:d66a303c5664 | 129 | "gSelect(DigitalOut) = %d\n" |
yamaguch | 0:d66a303c5664 | 130 | "sleep(DigitalOut) = %d\n" |
yamaguch | 0:d66a303c5664 | 131 | "scale = %d\n" |
yamaguch | 0:d66a303c5664 | 132 | "accelX, accelY, accelZ = %4.3f, %4.3f, %4.3f\n" |
yamaguch | 0:d66a303c5664 | 133 | "calib (1.5G) = %4.3f, %4.3f; %4.3f, %4.3f; %4.3f, %4.3f\n" |
yamaguch | 0:d66a303c5664 | 134 | "calib (6.0G) = %4.3f, %4.3f; %4.3f, %4.3f; %4.3f, %4.3f\n" |
yamaguch | 0:d66a303c5664 | 135 | "flags = %02X\n", |
yamaguch | 0:d66a303c5664 | 136 | zeroGDetectEnabled, |
yamaguch | 0:d66a303c5664 | 137 | gSelectEnabled, |
yamaguch | 0:d66a303c5664 | 138 | sleepEnabled, |
yamaguch | 0:d66a303c5664 | 139 | zeroGDetectEnabled ? zeroGDetect.read() : -1, |
yamaguch | 0:d66a303c5664 | 140 | gSelectEnabled ? gSelect.read() : -1, |
yamaguch | 0:d66a303c5664 | 141 | sleepEnabled ? sleep.read() : -1, |
yamaguch | 0:d66a303c5664 | 142 | scale, |
yamaguch | 0:d66a303c5664 | 143 | accelX, accelY, accelZ, |
yamaguch | 0:d66a303c5664 | 144 | calib[0].minX, calib[0].maxX, calib[0].minY, calib[0].maxY, calib[0].minZ, calib[0].maxZ, |
yamaguch | 0:d66a303c5664 | 145 | calib[1].minX, calib[1].maxX, calib[1].minY, calib[1].maxY, calib[1].minZ, calib[1].maxZ, |
yamaguch | 0:d66a303c5664 | 146 | flags); |
yamaguch | 0:d66a303c5664 | 147 | } |
yamaguch | 0:d66a303c5664 | 148 | |
yamaguch | 0:d66a303c5664 | 149 | void MMA7361L::calibrate(Scale scale, float minX, float maxX, float minY, float maxY, float minZ, float maxZ) { |
yamaguch | 0:d66a303c5664 | 150 | calib[scale].minX = minX; |
yamaguch | 0:d66a303c5664 | 151 | calib[scale].maxX = maxX; |
yamaguch | 0:d66a303c5664 | 152 | calib[scale].minY = minY; |
yamaguch | 0:d66a303c5664 | 153 | calib[scale].maxY = maxY; |
yamaguch | 0:d66a303c5664 | 154 | calib[scale].minZ = minZ; |
yamaguch | 0:d66a303c5664 | 155 | calib[scale].maxZ = maxZ; |
yamaguch | 0:d66a303c5664 | 156 | } |
yamaguch | 0:d66a303c5664 | 157 | |
yamaguch | 0:d66a303c5664 | 158 | float MMA7361L::calibrate(float value, int type, Scale scale) { |
yamaguch | 0:d66a303c5664 | 159 | float r = 1, s = 0; |
yamaguch | 0:d66a303c5664 | 160 | switch (type) { |
yamaguch | 0:d66a303c5664 | 161 | case ACCEL_X: |
yamaguch | 0:d66a303c5664 | 162 | r = (calib[scale].maxX - calib[scale].minX) / 2; |
yamaguch | 0:d66a303c5664 | 163 | s = (calib[scale].minX + calib[scale].maxX) / 2; |
yamaguch | 0:d66a303c5664 | 164 | break; |
yamaguch | 0:d66a303c5664 | 165 | case ACCEL_Y: |
yamaguch | 0:d66a303c5664 | 166 | r = (calib[scale].maxY - calib[scale].minY) / 2; |
yamaguch | 0:d66a303c5664 | 167 | s = (calib[scale].minY + calib[scale].maxY) / 2; |
yamaguch | 0:d66a303c5664 | 168 | break; |
yamaguch | 0:d66a303c5664 | 169 | case ACCEL_Z: |
yamaguch | 0:d66a303c5664 | 170 | r = (calib[scale].maxZ - calib[scale].minZ) / 2; |
yamaguch | 0:d66a303c5664 | 171 | s = (calib[scale].minZ + calib[scale].maxZ) / 2; |
yamaguch | 0:d66a303c5664 | 172 | break; |
yamaguch | 0:d66a303c5664 | 173 | } |
yamaguch | 0:d66a303c5664 | 174 | return (value - s) / r; |
yamaguch | 0:d66a303c5664 | 175 | } |