Diff: utils.h

Revision:

0:05cb0f94324f

diff -r 000000000000 -r 05cb0f94324f utils.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/utils.h	Mon May 30 08:11:32 2016 +0000
@@ -0,0 +1,187 @@
+#ifndef UTILS_H
+#define UTILS_H
+
+#include "mbed.h"
+
+#define TO_DEG 57.29577951308232087679815481410517033f
+#define TO_RAD 0.01745329252
+#define GRAD5 0.0872664625
+
+const float PI          = 3.14159265358979323846264338327950288f;
+const float PI_SQR      = 9.86960440108935861883449099987615114f;
+const float S22         = 0.70710678118654752440084436210484904f;
+
+const float B = 4/PI;
+const float C = -4/(PI_SQR);
+
+float get_mfiltered( float v, float vs[5] ){
+    int min, i, k;
+    float tmp, srt[5];
+    
+    for( i=0; i<4; i++)
+        vs[i] = vs[i+1];
+    vs[4] = v;
+    
+    for( i=0; i<5; i++ )
+        srt[i] = vs[i];
+    
+    for( i=0; i<5-1; i++ ){
+        min = i;
+        for( k=i+1; k<5; k++ )
+            if( srt[k] < srt[min] )
+                min = k;
+        if( min != i ){
+            tmp = srt[i];
+            srt[i] = srt[min];
+            srt[min] = tmp;
+        }
+    }    
+
+    return srt[2];
+}
+
+float acose( float x ){
+    return sqrt(1-x)*(1.5707963267948966192313216916398f + x*(-0.213300989f + x*(0.077980478f + x*-0.02164095f)));
+}
+
+float sine( float x ){
+    float y = B * x + C * x * abs(x);
+
+    #ifdef EXTRA_PRECISION
+    //  const float Q = 0.775;
+        const float P = 0.225;
+
+        y = P * (y * abs(y) - y) + y;   // Q * y + P * y * abs(y)
+    #endif
+    
+    return y;
+}
+
+float rotx( float x, float y ){
+    return S22*(x-y);
+}
+
+float roty( float x, float y ){
+    return S22*(x+y);
+}
+
+float norm(float v, float range){
+    return v/range;
+}
+
+float clamp(float v, float min, float max){
+    if( v < min )
+        return min;
+    if( v > max )
+        return max;
+    return v;
+}
+
+float clamp_bot(float v, float min){
+    if( v < min )
+        return min;
+    return v;
+}
+
+float deadband(float v, float db){
+  if(abs(v) < db) {
+    return 0.0f;
+  } else if(v > 0){
+    return v - db;
+  } else if(v < 0){
+    return v + db;
+  }
+  return 0;
+}
+
+// size - number of short values
+// start - start byte
+void char2short( char *buf, short *res, char start, char size ){
+    char idx = 0;
+    for( char i=start; i<size; i+=2 ){
+        res[idx] = buf[i] | (buf[i+1]<<8);
+        idx++;
+    }
+}
+
+void short2char( short *buf, char *res, char start, char size ){
+    char idx = 0;
+    for( char i=0; i<size*2; i+=2 ){
+        res[start+i] = buf[idx] & 0xFF;
+        res[start+i+1] = (buf[idx]>>8) & 0xFF;
+        idx++;
+    }
+}
+
+void clearbuf( char *buf ){
+    for( char i=0; i<sizeof(buf); i++ )
+        buf[i] = 0;
+}
+
+void quat_2_euler( float q[4], float e[3] ){
+  /*
+  double test = q[1]*q[2] + q[3]*q[0];
+   if (test > 0.499) { // singularity at north pole
+       e[2] = 2.f * atan2(q[1],q[0]);
+       e[1] = PI/2.f;
+       e[0] = 0;
+       return;
+   }
+   if (test < -0.499) { // singularity at south pole
+       e[2] = -2.f * atan2(q[1],q[0]);
+       e[1] = - PI/2.f;
+       e[0] = 0;
+       return;
+   }
+   */
+
+   //double sqw = q[0]*q[0];
+   double sqx = q[1]*q[1];
+   double sqy = q[2]*q[2];
+   double sqz = q[3]*q[3];
+
+   e[0] = atan2f(2.f * (q[2]*q[3] + q[1]*q[0]), 1 - 2.f * (sqx + sqy));// -sqx - sqy + sqz + sqw);
+   e[1] = asinf(-2.f * (q[1]*q[3] - q[2]*q[0]));
+   e[2] = atan2f(2.f * (q[1]*q[2] + q[3]*q[0]), 1 - 2.f * (sqy + sqz));//sqx - sqy - sqz + sqw);
+}
+
+void quat_2_euler_( float q0, float q1, float q2, float q3, float e[3] ){
+   float sqw = q0*q0;
+   float sqx = q1*q1;
+   float sqy = q2*q2;
+   float sqz = q3*q3;
+   e[0] = atan2f(2.f * (q1*q2 + q3*q0), sqx - sqy - sqz + sqw);
+   e[1] = asinf(-2.f * (q1*q3 - q2*q0));
+   e[2] = atan2f(2.f * (q2*q3 + q1*q0), -sqx - sqy + sqz + sqw);
+}
+
+float randf(void)
+{
+   return (float)rand()/(float)RAND_MAX;
+}
+
+float acc_noise(void)
+{
+   float noise = 0.1;
+   return noise / 2.0f - randf() * noise;
+}
+
+void beep(uint8_t samples, uint16_t delay, DigitalOut* buzz){
+    for(uint8_t i=0; i<samples; i++) {
+        *buzz = 1;
+        wait_ms(delay);                     
+        *buzz = 0; 
+        wait_ms(delay);
+    }
+}
+
+void blink(uint16_t delay, DigitalOut* led){
+    for(uint8_t i=0; i<delay/100; i++) {
+        *led = 1;
+        wait_ms(50);                     
+        *led = 0; 
+        wait_ms(50);
+    }
+}
+
+#endif
\ No newline at end of file