utilityFunctions.h

00001 #ifndef UTIL_FUNC_H
00002 #define UTIL_FUNC_H
00003 
00004 #define Pi  (3.14159)
00005 #define DEG2RAD (Pi/180)
00006 #define RAD2DEG (180/Pi)
00007 inline float saturateVal(float cmd, float max,float min)
00008 {
00009     if(cmd>max) {
00010         cmd = max;
00011     }
00012     if(cmd < min) {
00013         cmd = min;
00014     }
00015     return cmd;
00016 }
00017 inline float saturateCmd(float cmd)
00018 {
00019     if(cmd>1.0) {
00020         cmd = 1.0;
00021     }
00022     if(cmd < -1.0) {
00023         cmd = -1.0;
00024     }
00025     return cmd;
00026 }
00027 inline float wrapToPi(float ang)
00028 {
00029 
00030     while(ang > Pi) {
00031 
00032         ang = ang - 2*Pi;
00033     }
00034     while(ang < -Pi) {
00035         ang = ang + 2*Pi;
00036     }
00037 
00038     return ang;
00039 }
00040 //float norm3D(float x,float y,float z){
00041 
00042 //return sqrt((double)(x*x) + (double)(y*y) + (double)(z*z));
00043 //return sqrt(0.2);
00044 //}
00045 
00046 
00047 
00048 inline float firstOrderFilter(float var_new, float var_old,float dt,float tau){
00049 
00050         float Kdc = 1.0;
00051         float var_dot = -(1/tau)*var_old + (Kdc/tau)*var_new; // first order filter
00052         float var_out = var_old + var_dot*dt;
00053         
00054 return var_out;
00055 }
00056 
00057 #endif