Auberon Tatum
LCD Accelerometer with interrupt fault
SignalConditioning.cpp@0:298e8a54dc2d, 2010-05-12 (annotated)
- Committer:
- Aubs
- Date:
- Wed May 12 20:40:52 2010 +0000
- Revision:
- 0:298e8a54dc2d
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| Aubs | 0:298e8a54dc2d | 1 | #include "header.h" |
| Aubs | 0:298e8a54dc2d | 2 | extern float per_old; |
| Aubs | 0:298e8a54dc2d | 3 | extern float per_new; |
| Aubs | 0:298e8a54dc2d | 4 | const double PI = 3.141592; |
| Aubs | 0:298e8a54dc2d | 5 | extern float Xave; |
| Aubs | 0:298e8a54dc2d | 6 | extern float Yave; |
| Aubs | 0:298e8a54dc2d | 7 | extern float Zave; |
| Aubs | 0:298e8a54dc2d | 8 | extern float Xtemp; //sample hold |
| Aubs | 0:298e8a54dc2d | 9 | extern float Xtest; |
| Aubs | 0:298e8a54dc2d | 10 | extern float Ytemp; |
| Aubs | 0:298e8a54dc2d | 11 | extern float Ytest; |
| Aubs | 0:298e8a54dc2d | 12 | extern float Ztemp; |
| Aubs | 0:298e8a54dc2d | 13 | extern float Ztest; |
| Aubs | 0:298e8a54dc2d | 14 | extern float xout; |
| Aubs | 0:298e8a54dc2d | 15 | extern float yout; |
| Aubs | 0:298e8a54dc2d | 16 | extern float zout; |
| Aubs | 0:298e8a54dc2d | 17 | |
| Aubs | 0:298e8a54dc2d | 18 | extern float Xund; |
| Aubs | 0:298e8a54dc2d | 19 | extern float Yund; |
| Aubs | 0:298e8a54dc2d | 20 | extern float Zund; |
| Aubs | 0:298e8a54dc2d | 21 | extern float Ax2; |
| Aubs | 0:298e8a54dc2d | 22 | extern float Ay2; |
| Aubs | 0:298e8a54dc2d | 23 | extern float Az2; |
| Aubs | 0:298e8a54dc2d | 24 | |
| Aubs | 0:298e8a54dc2d | 25 | extern float X; |
| Aubs | 0:298e8a54dc2d | 26 | extern float Y; |
| Aubs | 0:298e8a54dc2d | 27 | extern float Z; |
| Aubs | 0:298e8a54dc2d | 28 | extern float Xoff; //CHANGE TO ZERO BALL POSITION |
| Aubs | 0:298e8a54dc2d | 29 | extern float Yoff; //CHANGE TO ZERO BALL POSITION |
| Aubs | 0:298e8a54dc2d | 30 | extern float Zoff; //CHANGE TO ZERO BALL |
| Aubs | 0:298e8a54dc2d | 31 | |
| Aubs | 0:298e8a54dc2d | 32 | //Signal Conditioning |
| Aubs | 0:298e8a54dc2d | 33 | |
| Aubs | 0:298e8a54dc2d | 34 | void SigCon(void){ |
| Aubs | 0:298e8a54dc2d | 35 | // averaging using recursive method, a percentage of last iteration data + percentage of raw data |
| Aubs | 0:298e8a54dc2d | 36 | Xave=((Xave)*(per_old))+((acc.x())*(per_new)); |
| Aubs | 0:298e8a54dc2d | 37 | Yave=((Yave)*(per_old))+((acc.y())*(per_new)); |
| Aubs | 0:298e8a54dc2d | 38 | Zave=((Zave)*(per_old))+((acc.z())*(per_new)); |
| Aubs | 0:298e8a54dc2d | 39 | |
| Aubs | 0:298e8a54dc2d | 40 | xout=(Xave+Xoff); |
| Aubs | 0:298e8a54dc2d | 41 | yout=(Yave+Yoff); |
| Aubs | 0:298e8a54dc2d | 42 | zout=(Zave+Zoff); |
| Aubs | 0:298e8a54dc2d | 43 | |
| Aubs | 0:298e8a54dc2d | 44 | Xtest=((Xtemp)-(xout)); //stabalisation routine |
| Aubs | 0:298e8a54dc2d | 45 | Ytest=((Ytemp)-(yout)); |
| Aubs | 0:298e8a54dc2d | 46 | Ztest=((Ztemp)-(zout)); |
| Aubs | 0:298e8a54dc2d | 47 | |
| Aubs | 0:298e8a54dc2d | 48 | if (Xtest>-0.04 && Xtest<0.04){xout=Xtemp;} |
| Aubs | 0:298e8a54dc2d | 49 | if (Ytest>-0.04 && Ytest<0.04){yout=Ytemp;} |
| Aubs | 0:298e8a54dc2d | 50 | if (Ztest>-0.04 && Ztest<0.04){zout=Ztemp;} |
| Aubs | 0:298e8a54dc2d | 51 | |
| Aubs | 0:298e8a54dc2d | 52 | Ax2= pow(xout,2); // first stage calculations square value |
| Aubs | 0:298e8a54dc2d | 53 | Ay2= pow(yout,2); |
| Aubs | 0:298e8a54dc2d | 54 | Az2= pow(zout,2); |
| Aubs | 0:298e8a54dc2d | 55 | |
| Aubs | 0:298e8a54dc2d | 56 | Xund= sqrt(Ay2+Az2); // performs first part equation |
| Aubs | 0:298e8a54dc2d | 57 | Yund= sqrt(Ax2+Az2); |
| Aubs | 0:298e8a54dc2d | 58 | Zund= sqrt(Ax2+Ay2); |
| Aubs | 0:298e8a54dc2d | 59 | |
| Aubs | 0:298e8a54dc2d | 60 | float Xrads = atan(xout/Xund); //calculates andgles in radians |
| Aubs | 0:298e8a54dc2d | 61 | float Yrads = atan(yout/Yund); |
| Aubs | 0:298e8a54dc2d | 62 | float Zrads = atan(zout/Zund); |
| Aubs | 0:298e8a54dc2d | 63 | |
| Aubs | 0:298e8a54dc2d | 64 | X = ((Xrads*180)/PI); // convert from radians to degrees |
| Aubs | 0:298e8a54dc2d | 65 | Y = ((Yrads*180)/PI); |
| Aubs | 0:298e8a54dc2d | 66 | Z = ((Zrads*180)/PI); |
| Aubs | 0:298e8a54dc2d | 67 | |
| Aubs | 0:298e8a54dc2d | 68 | Xtemp=xout; |
| Aubs | 0:298e8a54dc2d | 69 | Ytemp=yout; |
| Aubs | 0:298e8a54dc2d | 70 | Ztemp=zout; |
| Aubs | 0:298e8a54dc2d | 71 | } |
| Aubs | 0:298e8a54dc2d | 72 |