Push for Students
Dependencies: BNO055_fusion Madpulse ServoIn ServoOut mavlink_emaxx mbed
Fork of Madpulse_Speed_Control_temp by
utilityFunctions.h
- Committer:
- jdawkins
- Date:
- 2017-11-15
- Revision:
- 2:2bb375ab3b2b
- Parent:
- 0:daea75c21ac1
File content as of revision 2:2bb375ab3b2b:
#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