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