SignalConditioning.cpp

00001 #include "header.h"
00002 extern float per_old;
00003 extern float per_new;
00004 const double PI = 3.141592;
00005 extern float Xave;
00006 extern float Yave;
00007 extern float Zave;
00008 extern float Xtemp;                        //sample hold
00009 extern float Xtest;
00010 extern float Ytemp;
00011 extern float Ytest;
00012 extern float Ztemp;
00013 extern float Ztest;
00014 extern float xout;
00015 extern float yout;
00016 extern float zout;
00017 
00018 extern float Xund;
00019 extern float Yund;
00020 extern float Zund;
00021 extern float Ax2;
00022 extern float Ay2;
00023 extern float Az2;
00024 
00025 extern float X;
00026 extern float Y;
00027 extern float Z;
00028 extern float Xoff;             //CHANGE TO ZERO BALL POSITION
00029 extern float Yoff; //CHANGE TO ZERO BALL POSITION
00030 extern float Zoff; //CHANGE TO ZERO BALL
00031 
00032 //Signal Conditioning
00033 
00034 void SigCon(void){    
00035 // averaging using recursive method, a percentage of last iteration data + percentage of raw data  
00036     Xave=((Xave)*(per_old))+((acc.x())*(per_new));
00037     Yave=((Yave)*(per_old))+((acc.y())*(per_new));
00038     Zave=((Zave)*(per_old))+((acc.z())*(per_new));
00039     
00040     xout=(Xave+Xoff);  
00041     yout=(Yave+Yoff);
00042     zout=(Zave+Zoff);
00043     
00044         Xtest=((Xtemp)-(xout));                 //stabalisation routine
00045         Ytest=((Ytemp)-(yout));
00046         Ztest=((Ztemp)-(zout));
00047 
00048                     if (Xtest>-0.04 && Xtest<0.04){xout=Xtemp;}
00049                     if (Ytest>-0.04 && Ytest<0.04){yout=Ytemp;}   
00050                     if (Ztest>-0.04 && Ztest<0.04){zout=Ztemp;}
00051 
00052         Ax2= pow(xout,2);           // first stage calculations square value
00053         Ay2= pow(yout,2);
00054         Az2= pow(zout,2);              
00055         
00056         Xund= sqrt(Ay2+Az2);        // performs first part equation
00057         Yund= sqrt(Ax2+Az2);
00058         Zund= sqrt(Ax2+Ay2);
00059         
00060         float Xrads = atan(xout/Xund);  //calculates andgles in radians
00061         float Yrads = atan(yout/Yund);
00062         float Zrads = atan(zout/Zund);
00063         
00064         X = ((Xrads*180)/PI);     // convert from radians to degrees
00065         Y = ((Yrads*180)/PI);
00066         Z = ((Zrads*180)/PI);
00067             
00068         Xtemp=xout;
00069         Ytemp=yout;
00070         Ztemp=zout;
00071     }
00072