File content as of revision 0:05cb0f94324f:

#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