Quadrotor stabi
Dependencies: ESC HMC5883L MPU6050 MadgwickAHRS MemLog Servo mbed ms5611
utils.h
- Committer:
- Decimus
- Date:
- 2016-05-30
- Revision:
- 1:4a8469635592
- Parent:
- 0:05cb0f94324f
File content as of revision 1:4a8469635592:
#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