Push for Students
Dependencies: BNO055_fusion Madpulse ServoIn ServoOut mavlink_emaxx mbed
Fork of Madpulse_Speed_Control_temp by
Diff: utilityFunctions.h
- Revision:
- 0:daea75c21ac1
diff -r 000000000000 -r daea75c21ac1 utilityFunctions.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/utilityFunctions.h Tue Nov 14 13:51:29 2017 +0000 @@ -0,0 +1,57 @@ +#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 \ No newline at end of file