File content as of revision 0:daea75c21ac1:

#ifndef UTIL_FUNC_H
#define UTIL_FUNC_H

#define Pi  (3.14159)
#define DEG2RAD (Pi/180)
#define RAD2DEG (180/Pi)
inline float saturateVal(float cmd, float max,float min)
{
    if(cmd>max) {
        cmd = max;
    }
    if(cmd < min) {
        cmd = min;
    }
    return cmd;
}
inline float saturateCmd(float cmd)
{
    if(cmd>1.0) {
        cmd = 1.0;
    }
    if(cmd < -1.0) {
        cmd = -1.0;
    }
    return cmd;
}
inline float wrapToPi(float ang)
{

    while(ang > Pi) {

        ang = ang - 2*Pi;
    }
    while(ang < -Pi) {
        ang = ang + 2*Pi;
    }

    return ang;
}
//float norm3D(float x,float y,float z){

//return sqrt((double)(x*x) + (double)(y*y) + (double)(z*z));
//return sqrt(0.2);
//}



inline float firstOrderFilter(float var_new, float var_old,float dt,float tau){

        float Kdc = 1.0;
        float var_dot = -(1/tau)*var_old + (Kdc/tau)*var_new; // first order filter
        float var_out = var_old + var_dot*dt;
        
return var_out;
}

#endif