UAVRO / Mbed 2 deprecated Q2_Stabi

Quadrotor stabi

Dependencies:   ESC HMC5883L MPU6050 MadgwickAHRS MemLog Servo mbed ms5611

utils.h@1:4a8469635592, 2016-05-30 (annotated)

Committer:
Decimus
Date:
Mon May 30 08:16:04 2016 +0000
Revision:
1:4a8469635592
Parent:
0:05cb0f94324f 
[+]

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Decimus 0:05cb0f94324f 1 #ifndef UTILS_H
Decimus 0:05cb0f94324f 2 #define UTILS_H
Decimus 0:05cb0f94324f 3
Decimus 0:05cb0f94324f 4 #include "mbed.h"
Decimus 0:05cb0f94324f 5
Decimus 0:05cb0f94324f 6 #define TO_DEG 57.29577951308232087679815481410517033f
Decimus 0:05cb0f94324f 7 #define TO_RAD 0.01745329252
Decimus 0:05cb0f94324f 8 #define GRAD5 0.0872664625
Decimus 0:05cb0f94324f 9
Decimus 0:05cb0f94324f 10 const float PI = 3.14159265358979323846264338327950288f;
Decimus 0:05cb0f94324f 11 const float PI_SQR = 9.86960440108935861883449099987615114f;
Decimus 0:05cb0f94324f 12 const float S22 = 0.70710678118654752440084436210484904f;
Decimus 0:05cb0f94324f 13
Decimus 0:05cb0f94324f 14 const float B = 4/PI;
Decimus 0:05cb0f94324f 15 const float C = -4/(PI_SQR);
Decimus 0:05cb0f94324f 16
Decimus 0:05cb0f94324f 17 float get_mfiltered( float v, float vs[5] ){
Decimus 0:05cb0f94324f 18 int min, i, k;
Decimus 0:05cb0f94324f 19 float tmp, srt[5];
Decimus 0:05cb0f94324f 20
Decimus 0:05cb0f94324f 21 for( i=0; i<4; i++)
Decimus 0:05cb0f94324f 22 vs[i] = vs[i+1];
Decimus 0:05cb0f94324f 23 vs[4] = v;
Decimus 0:05cb0f94324f 24
Decimus 0:05cb0f94324f 25 for( i=0; i<5; i++ )
Decimus 0:05cb0f94324f 26 srt[i] = vs[i];
Decimus 0:05cb0f94324f 27
Decimus 0:05cb0f94324f 28 for( i=0; i<5-1; i++ ){
Decimus 0:05cb0f94324f 29 min = i;
Decimus 0:05cb0f94324f 30 for( k=i+1; k<5; k++ )
Decimus 0:05cb0f94324f 31 if( srt[k] < srt[min] )
Decimus 0:05cb0f94324f 32 min = k;
Decimus 0:05cb0f94324f 33 if( min != i ){
Decimus 0:05cb0f94324f 34 tmp = srt[i];
Decimus 0:05cb0f94324f 35 srt[i] = srt[min];
Decimus 0:05cb0f94324f 36 srt[min] = tmp;
Decimus 0:05cb0f94324f 37 }
Decimus 0:05cb0f94324f 38 }
Decimus 0:05cb0f94324f 39
Decimus 0:05cb0f94324f 40 return srt[2];
Decimus 0:05cb0f94324f 41 }
Decimus 0:05cb0f94324f 42
Decimus 0:05cb0f94324f 43 float acose( float x ){
Decimus 0:05cb0f94324f 44 return sqrt(1-x)*(1.5707963267948966192313216916398f + x*(-0.213300989f + x*(0.077980478f + x*-0.02164095f)));
Decimus 0:05cb0f94324f 45 }
Decimus 0:05cb0f94324f 46
Decimus 0:05cb0f94324f 47 float sine( float x ){
Decimus 0:05cb0f94324f 48 float y = B * x + C * x * abs(x);
Decimus 0:05cb0f94324f 49
Decimus 0:05cb0f94324f 50 #ifdef EXTRA_PRECISION
Decimus 0:05cb0f94324f 51 // const float Q = 0.775;
Decimus 0:05cb0f94324f 52 const float P = 0.225;
Decimus 0:05cb0f94324f 53
Decimus 0:05cb0f94324f 54 y = P * (y * abs(y) - y) + y; // Q * y + P * y * abs(y)
Decimus 0:05cb0f94324f 55 #endif
Decimus 0:05cb0f94324f 56
Decimus 0:05cb0f94324f 57 return y;
Decimus 0:05cb0f94324f 58 }
Decimus 0:05cb0f94324f 59
Decimus 0:05cb0f94324f 60 float rotx( float x, float y ){
Decimus 0:05cb0f94324f 61 return S22*(x-y);
Decimus 0:05cb0f94324f 62 }
Decimus 0:05cb0f94324f 63
Decimus 0:05cb0f94324f 64 float roty( float x, float y ){
Decimus 0:05cb0f94324f 65 return S22*(x+y);
Decimus 0:05cb0f94324f 66 }
Decimus 0:05cb0f94324f 67
Decimus 0:05cb0f94324f 68 float norm(float v, float range){
Decimus 0:05cb0f94324f 69 return v/range;
Decimus 0:05cb0f94324f 70 }
Decimus 0:05cb0f94324f 71
Decimus 0:05cb0f94324f 72 float clamp(float v, float min, float max){
Decimus 0:05cb0f94324f 73 if( v < min )
Decimus 0:05cb0f94324f 74 return min;
Decimus 0:05cb0f94324f 75 if( v > max )
Decimus 0:05cb0f94324f 76 return max;
Decimus 0:05cb0f94324f 77 return v;
Decimus 0:05cb0f94324f 78 }
Decimus 0:05cb0f94324f 79
Decimus 0:05cb0f94324f 80 float clamp_bot(float v, float min){
Decimus 0:05cb0f94324f 81 if( v < min )
Decimus 0:05cb0f94324f 82 return min;
Decimus 0:05cb0f94324f 83 return v;
Decimus 0:05cb0f94324f 84 }
Decimus 0:05cb0f94324f 85
Decimus 0:05cb0f94324f 86 float deadband(float v, float db){
Decimus 0:05cb0f94324f 87 if(abs(v) < db) {
Decimus 0:05cb0f94324f 88 return 0.0f;
Decimus 0:05cb0f94324f 89 } else if(v > 0){
Decimus 0:05cb0f94324f 90 return v - db;
Decimus 0:05cb0f94324f 91 } else if(v < 0){
Decimus 0:05cb0f94324f 92 return v + db;
Decimus 0:05cb0f94324f 93 }
Decimus 0:05cb0f94324f 94 return 0;
Decimus 0:05cb0f94324f 95 }
Decimus 0:05cb0f94324f 96
Decimus 0:05cb0f94324f 97 // size - number of short values
Decimus 0:05cb0f94324f 98 // start - start byte
Decimus 0:05cb0f94324f 99 void char2short( char *buf, short *res, char start, char size ){
Decimus 0:05cb0f94324f 100 char idx = 0;
Decimus 0:05cb0f94324f 101 for( char i=start; i<size; i+=2 ){
Decimus 0:05cb0f94324f 102 res[idx] = buf[i] | (buf[i+1]<<8);
Decimus 0:05cb0f94324f 103 idx++;
Decimus 0:05cb0f94324f 104 }
Decimus 0:05cb0f94324f 105 }
Decimus 0:05cb0f94324f 106
Decimus 0:05cb0f94324f 107 void short2char( short *buf, char *res, char start, char size ){
Decimus 0:05cb0f94324f 108 char idx = 0;
Decimus 0:05cb0f94324f 109 for( char i=0; i<size*2; i+=2 ){
Decimus 0:05cb0f94324f 110 res[start+i] = buf[idx] & 0xFF;
Decimus 0:05cb0f94324f 111 res[start+i+1] = (buf[idx]>>8) & 0xFF;
Decimus 0:05cb0f94324f 112 idx++;
Decimus 0:05cb0f94324f 113 }
Decimus 0:05cb0f94324f 114 }
Decimus 0:05cb0f94324f 115
Decimus 0:05cb0f94324f 116 void clearbuf( char *buf ){
Decimus 0:05cb0f94324f 117 for( char i=0; i<sizeof(buf); i++ )
Decimus 0:05cb0f94324f 118 buf[i] = 0;
Decimus 0:05cb0f94324f 119 }
Decimus 0:05cb0f94324f 120
Decimus 0:05cb0f94324f 121 void quat_2_euler( float q[4], float e[3] ){
Decimus 0:05cb0f94324f 122 /*
Decimus 0:05cb0f94324f 123 double test = q[1]*q[2] + q[3]*q[0];
Decimus 0:05cb0f94324f 124 if (test > 0.499) { // singularity at north pole
Decimus 0:05cb0f94324f 125 e[2] = 2.f * atan2(q[1],q[0]);
Decimus 0:05cb0f94324f 126 e[1] = PI/2.f;
Decimus 0:05cb0f94324f 127 e[0] = 0;
Decimus 0:05cb0f94324f 128 return;
Decimus 0:05cb0f94324f 129 }
Decimus 0:05cb0f94324f 130 if (test < -0.499) { // singularity at south pole
Decimus 0:05cb0f94324f 131 e[2] = -2.f * atan2(q[1],q[0]);
Decimus 0:05cb0f94324f 132 e[1] = - PI/2.f;
Decimus 0:05cb0f94324f 133 e[0] = 0;
Decimus 0:05cb0f94324f 134 return;
Decimus 0:05cb0f94324f 135 }
Decimus 0:05cb0f94324f 136 */
Decimus 0:05cb0f94324f 137
Decimus 0:05cb0f94324f 138 //double sqw = q[0]*q[0];
Decimus 0:05cb0f94324f 139 double sqx = q[1]*q[1];
Decimus 0:05cb0f94324f 140 double sqy = q[2]*q[2];
Decimus 0:05cb0f94324f 141 double sqz = q[3]*q[3];
Decimus 0:05cb0f94324f 142
Decimus 0:05cb0f94324f 143 e[0] = atan2f(2.f * (q[2]*q[3] + q[1]*q[0]), 1 - 2.f * (sqx + sqy));// -sqx - sqy + sqz + sqw);
Decimus 0:05cb0f94324f 144 e[1] = asinf(-2.f * (q[1]*q[3] - q[2]*q[0]));
Decimus 0:05cb0f94324f 145 e[2] = atan2f(2.f * (q[1]*q[2] + q[3]*q[0]), 1 - 2.f * (sqy + sqz));//sqx - sqy - sqz + sqw);
Decimus 0:05cb0f94324f 146 }
Decimus 0:05cb0f94324f 147
Decimus 0:05cb0f94324f 148 void quat_2_euler_( float q0, float q1, float q2, float q3, float e[3] ){
Decimus 0:05cb0f94324f 149 float sqw = q0*q0;
Decimus 0:05cb0f94324f 150 float sqx = q1*q1;
Decimus 0:05cb0f94324f 151 float sqy = q2*q2;
Decimus 0:05cb0f94324f 152 float sqz = q3*q3;
Decimus 0:05cb0f94324f 153 e[0] = atan2f(2.f * (q1*q2 + q3*q0), sqx - sqy - sqz + sqw);
Decimus 0:05cb0f94324f 154 e[1] = asinf(-2.f * (q1*q3 - q2*q0));
Decimus 0:05cb0f94324f 155 e[2] = atan2f(2.f * (q2*q3 + q1*q0), -sqx - sqy + sqz + sqw);
Decimus 0:05cb0f94324f 156 }
Decimus 0:05cb0f94324f 157
Decimus 0:05cb0f94324f 158 float randf(void)
Decimus 0:05cb0f94324f 159 {
Decimus 0:05cb0f94324f 160 return (float)rand()/(float)RAND_MAX;
Decimus 0:05cb0f94324f 161 }
Decimus 0:05cb0f94324f 162
Decimus 0:05cb0f94324f 163 float acc_noise(void)
Decimus 0:05cb0f94324f 164 {
Decimus 0:05cb0f94324f 165 float noise = 0.1;
Decimus 0:05cb0f94324f 166 return noise / 2.0f - randf() * noise;
Decimus 0:05cb0f94324f 167 }
Decimus 0:05cb0f94324f 168
Decimus 0:05cb0f94324f 169 void beep(uint8_t samples, uint16_t delay, DigitalOut* buzz){
Decimus 0:05cb0f94324f 170 for(uint8_t i=0; i<samples; i++) {
Decimus 0:05cb0f94324f 171 *buzz = 1;
Decimus 0:05cb0f94324f 172 wait_ms(delay);
Decimus 0:05cb0f94324f 173 *buzz = 0;
Decimus 0:05cb0f94324f 174 wait_ms(delay);
Decimus 0:05cb0f94324f 175 }
Decimus 0:05cb0f94324f 176 }
Decimus 0:05cb0f94324f 177
Decimus 0:05cb0f94324f 178 void blink(uint16_t delay, DigitalOut* led){
Decimus 0:05cb0f94324f 179 for(uint8_t i=0; i<delay/100; i++) {
Decimus 0:05cb0f94324f 180 *led = 1;
Decimus 0:05cb0f94324f 181 wait_ms(50);
Decimus 0:05cb0f94324f 182 *led = 0;
Decimus 0:05cb0f94324f 183 wait_ms(50);
Decimus 0:05cb0f94324f 184 }
Decimus 0:05cb0f94324f 185 }
Decimus 0:05cb0f94324f 186
Decimus 0:05cb0f94324f 187 #endif