Erik van de Coevering / quadV3

This is my quadcopter prototype software, still in development!

quadv3/gyro.h@1:ac68f0368a77, 2013-07-23 (annotated)

Committer:
Anaesthetix
Date:
Tue Jul 23 14:01:42 2013 +0000
Revision:
1:ac68f0368a77
Parent:
0:978110f7f027 
Other accelerometer added

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Anaesthetix 0:978110f7f027 1 #ifndef GYRO_H
Anaesthetix 0:978110f7f027 2 #define GYRO_H
Anaesthetix 0:978110f7f027 3 #include "IOMacros.h"
Anaesthetix 0:978110f7f027 4 ITG3200 gyro(p9, p10);
Anaesthetix 0:978110f7f027 5 DigitalOut looptime2(p18);
Anaesthetix 0:978110f7f027 6
Anaesthetix 0:978110f7f027 7 SPI spi(p5,p6,p7);
Anaesthetix 0:978110f7f027 8 BMA180 acc1(spi, p8);
Anaesthetix 0:978110f7f027 9
Anaesthetix 0:978110f7f027 10 Timer dtgyro;
Anaesthetix 0:978110f7f027 11 #define PI 3.141592654
Anaesthetix 0:978110f7f027 12
Anaesthetix 0:978110f7f027 13
Anaesthetix 0:978110f7f027 14 int xg= 0;
Anaesthetix 0:978110f7f027 15 int yg= 0;
Anaesthetix 0:978110f7f027 16 int zg= 0;
Anaesthetix 0:978110f7f027 17 float temphoekx=0;
Anaesthetix 0:978110f7f027 18 float temp6=0;
Anaesthetix 0:978110f7f027 19 float newax = 0;
Anaesthetix 0:978110f7f027 20 float neway = 0;
Anaesthetix 0:978110f7f027 21 float newax1 = 0;
Anaesthetix 0:978110f7f027 22 float neway1 = 0;
Anaesthetix 0:978110f7f027 23 float tempx1 = 0;
Anaesthetix 0:978110f7f027 24 float tempy1 = 0;
Anaesthetix 0:978110f7f027 25 int flag = 0;
Anaesthetix 0:978110f7f027 26 float xag = 0;
Anaesthetix 0:978110f7f027 27 float xarg = 0;
Anaesthetix 0:978110f7f027 28 float yarg = 0;
Anaesthetix 0:978110f7f027 29 float zarg = 0;
Anaesthetix 0:978110f7f027 30 float yag = 0;
Anaesthetix 0:978110f7f027 31 float xagoffset = 0;
Anaesthetix 0:978110f7f027 32 float yagoffset = 0;
Anaesthetix 0:978110f7f027 33 float zagoffset = -0.75;
Anaesthetix 0:978110f7f027 34 float axoffset = 0;
Anaesthetix 0:978110f7f027 35 float ayoffset = 0;
Anaesthetix 0:978110f7f027 36 float xangle[40] = {0};
Anaesthetix 0:978110f7f027 37 float yangle[40] = {0};
Anaesthetix 0:978110f7f027 38 float x1 = 0;
Anaesthetix 0:978110f7f027 39 float x2 = 0;
Anaesthetix 0:978110f7f027 40 float y1 = 0;
Anaesthetix 0:978110f7f027 41 float y2 = 0;
Anaesthetix 0:978110f7f027 42 float ax1 = 0;
Anaesthetix 0:978110f7f027 43 float ay1 = 0;
Anaesthetix 0:978110f7f027 44 float ax2 = 0;
Anaesthetix 0:978110f7f027 45 float ax = 0;
Anaesthetix 0:978110f7f027 46 float ay = 0;
Anaesthetix 0:978110f7f027 47 float ay2 = 0;
Anaesthetix 0:978110f7f027 48 int dtgyrtemp;
Anaesthetix 0:978110f7f027 49 float dtgyr;
Anaesthetix 0:978110f7f027 50
Anaesthetix 0:978110f7f027 51 void gyrostart(void);
Anaesthetix 0:978110f7f027 52 void gyrocal(void);
Anaesthetix 0:978110f7f027 53 void gyrosample(void);
Anaesthetix 0:978110f7f027 54
Anaesthetix 0:978110f7f027 55 void gyrostart(void) {
Anaesthetix 0:978110f7f027 56 gyro.setLpBandwidth(LPFBW_42HZ);
Anaesthetix 0:978110f7f027 57 }
Anaesthetix 0:978110f7f027 58
Anaesthetix 0:978110f7f027 59
Anaesthetix 0:978110f7f027 60 void gyrocal(void) {
Anaesthetix 0:978110f7f027 61 for (int i=0; i<100; i++) {
Anaesthetix 0:978110f7f027 62 gyrosample();
Anaesthetix 0:978110f7f027 63 xagoffset += (0.01*xag);
Anaesthetix 0:978110f7f027 64 yagoffset += (0.01*yag);
Anaesthetix 0:978110f7f027 65 // zagoffset += (0.01*zarg);
Anaesthetix 0:978110f7f027 66
Anaesthetix 0:978110f7f027 67 }
Anaesthetix 0:978110f7f027 68 }
Anaesthetix 0:978110f7f027 69
Anaesthetix 0:978110f7f027 70
Anaesthetix 0:978110f7f027 71 void gyrosample(void) {
Anaesthetix 0:978110f7f027 72 //Timer for filtervar
Anaesthetix 0:978110f7f027 73 dtgyro.stop();
Anaesthetix 0:978110f7f027 74 dtgyrtemp = dtgyro.read_us();
Anaesthetix 0:978110f7f027 75 dtgyr = dtgyrtemp;
Anaesthetix 0:978110f7f027 76 dtgyro.reset();
Anaesthetix 0:978110f7f027 77 dtgyro.start();
Anaesthetix 0:978110f7f027 78
Anaesthetix 0:978110f7f027 79 //Get gyro & accelero data
Anaesthetix 0:978110f7f027 80 __disable_irq(); //For some reason deadlocks without this
Anaesthetix 0:978110f7f027 81 xg = gyro.getGyroX();
Anaesthetix 0:978110f7f027 82 yg = gyro.getGyroY();
Anaesthetix 0:978110f7f027 83 zg = gyro.getGyroZ();
Anaesthetix 0:978110f7f027 84 __enable_irq();
Anaesthetix 0:978110f7f027 85 //accsample();
Anaesthetix 0:978110f7f027 86 acc1.readRaw();
Anaesthetix 0:978110f7f027 87
Anaesthetix 0:978110f7f027 88 //Cast int to float for better precision
Anaesthetix 0:978110f7f027 89 xarg = xg;
Anaesthetix 0:978110f7f027 90 yarg = yg;
Anaesthetix 0:978110f7f027 91 zarg = zg;
Anaesthetix 0:978110f7f027 92
Anaesthetix 0:978110f7f027 93 //Calc angular rate in degrees/second
Anaesthetix 0:978110f7f027 94 xarg = (xarg/14.375);
Anaesthetix 0:978110f7f027 95 yarg = (yarg/14.375);
Anaesthetix 0:978110f7f027 96 zarg = (zarg/14.375);
Anaesthetix 0:978110f7f027 97
Anaesthetix 0:978110f7f027 98 zarg -= zagoffset;
Anaesthetix 0:978110f7f027 99 xag = xarg;
Anaesthetix 0:978110f7f027 100 yag = yarg;
Anaesthetix 0:978110f7f027 101
Anaesthetix 0:978110f7f027 102 y1 = acc1.y;
Anaesthetix 0:978110f7f027 103 y2 = acc1.y;
Anaesthetix 0:978110f7f027 104 x1 = acc1.x;
Anaesthetix 0:978110f7f027 105 x2 = acc1.x;
Anaesthetix 0:978110f7f027 106
Anaesthetix 0:978110f7f027 107
Anaesthetix 0:978110f7f027 108
Anaesthetix 0:978110f7f027 109 //Calc y angle
Anaesthetix 0:978110f7f027 110 y1 = y1*y1;
Anaesthetix 0:978110f7f027 111 y1 = 1-y1;
Anaesthetix 0:978110f7f027 112 y1 = sqrt(y1);
Anaesthetix 0:978110f7f027 113 y1 = y2/y1;
Anaesthetix 0:978110f7f027 114
Anaesthetix 0:978110f7f027 115 ay = atan(y1);
Anaesthetix 0:978110f7f027 116 ay = ay*(180/PI);
Anaesthetix 0:978110f7f027 117
Anaesthetix 0:978110f7f027 118 //Calc x angle
Anaesthetix 0:978110f7f027 119 x1 = x1*x1;
Anaesthetix 0:978110f7f027 120 x1 = 1-x1;
Anaesthetix 0:978110f7f027 121 x1 = sqrt(x1);
Anaesthetix 0:978110f7f027 122 x1 = x2/x1;
Anaesthetix 0:978110f7f027 123
Anaesthetix 0:978110f7f027 124 ax = atan(x1);
Anaesthetix 0:978110f7f027 125 ax = ax*(180/PI);
Anaesthetix 0:978110f7f027 126
Anaesthetix 0:978110f7f027 127
Anaesthetix 0:978110f7f027 128 //if [angle] is NAN use previous value
Anaesthetix 0:978110f7f027 129 if (ax != ax) ax = ax2;
Anaesthetix 0:978110f7f027 130 if (ay != ay) ay = ay2;
Anaesthetix 0:978110f7f027 131 if (ax == ax) ax2 = ax;
Anaesthetix 0:978110f7f027 132 if (ay == ay) ay2 = ay;
Anaesthetix 0:978110f7f027 133
Anaesthetix 0:978110f7f027 134
Anaesthetix 0:978110f7f027 135 //Moving avarage filter
Anaesthetix 1:ac68f0368a77 136 xangle[0] = (ax + 6);
Anaesthetix 0:978110f7f027 137 ax1 = ((0.025*xangle[0]) + (0.025*xangle[1]) + (0.025*xangle[2]) + (0.025*xangle[3]) + (0.025*xangle[4]) + (0.025*xangle[5]) + (0.025*xangle[6]) + (0.025*xangle[7]) + (0.025*xangle[8]) + (0.025*xangle[9]) + (0.025*xangle[10]) + (0.025*xangle[11]) + (0.025*xangle[12]) + (0.025*xangle[13]) + (0.025*xangle[14]) + (0.025*xangle[15]) + (0.025*xangle[16]) + (0.025*xangle[17]) + (0.025*xangle[18]) + (0.025*xangle[19]) + (0.025*xangle[20]) + (0.025*xangle[21]) + (0.025*xangle[22]) + (0.025*xangle[23]) + (0.025*xangle[24]) + (0.025*xangle[25]) + (0.025*xangle[26]) + (0.025*xangle[27]) + (0.025*xangle[28]) + (0.025*xangle[29]) + (0.025*xangle[30]) + (0.025*xangle[31]) + (0.025*xangle[32]) + (0.025*xangle[33]) + (0.025*xangle[34]) + (0.025*xangle[35]) + (0.025*xangle[36]) + (0.025*xangle[37]) + (0.025*xangle[38]) + (0.025*xangle[39]));
Anaesthetix 0:978110f7f027 138 xangle[39] = xangle[38];
Anaesthetix 0:978110f7f027 139 xangle[38] = xangle[37];
Anaesthetix 0:978110f7f027 140 xangle[37] = xangle[36];
Anaesthetix 0:978110f7f027 141 xangle[36] = xangle[35];
Anaesthetix 0:978110f7f027 142 xangle[35] = xangle[34];
Anaesthetix 0:978110f7f027 143 xangle[34] = xangle[33];
Anaesthetix 0:978110f7f027 144 xangle[33] = xangle[32];
Anaesthetix 0:978110f7f027 145 xangle[32] = xangle[31];
Anaesthetix 0:978110f7f027 146 xangle[31] = xangle[30];
Anaesthetix 0:978110f7f027 147 xangle[30] = xangle[29];
Anaesthetix 0:978110f7f027 148 xangle[29] = xangle[28];
Anaesthetix 0:978110f7f027 149 xangle[28] = xangle[27];
Anaesthetix 0:978110f7f027 150 xangle[27] = xangle[26];
Anaesthetix 0:978110f7f027 151 xangle[26] = xangle[25];
Anaesthetix 0:978110f7f027 152 xangle[25] = xangle[24];
Anaesthetix 0:978110f7f027 153 xangle[24] = xangle[23];
Anaesthetix 0:978110f7f027 154 xangle[23] = xangle[22];
Anaesthetix 0:978110f7f027 155 xangle[22] = xangle[21];
Anaesthetix 0:978110f7f027 156 xangle[21] = xangle[20];
Anaesthetix 0:978110f7f027 157 xangle[20] = xangle[19];
Anaesthetix 0:978110f7f027 158 xangle[19] = xangle[18];
Anaesthetix 0:978110f7f027 159 xangle[18] = xangle[17];
Anaesthetix 0:978110f7f027 160 xangle[17] = xangle[16];
Anaesthetix 0:978110f7f027 161 xangle[16] = xangle[15];
Anaesthetix 0:978110f7f027 162 xangle[15] = xangle[14];
Anaesthetix 0:978110f7f027 163 xangle[14] = xangle[13];
Anaesthetix 0:978110f7f027 164 xangle[13] = xangle[12];
Anaesthetix 0:978110f7f027 165 xangle[12] = xangle[11];
Anaesthetix 0:978110f7f027 166 xangle[11] = xangle[10];
Anaesthetix 0:978110f7f027 167 xangle[10] = xangle[9];
Anaesthetix 0:978110f7f027 168 xangle[9] = xangle[8];
Anaesthetix 0:978110f7f027 169 xangle[8] = xangle[7];
Anaesthetix 0:978110f7f027 170 xangle[7] = xangle[6];
Anaesthetix 0:978110f7f027 171 xangle[6] = xangle[5];
Anaesthetix 0:978110f7f027 172 xangle[5] = xangle[4];
Anaesthetix 0:978110f7f027 173 xangle[4] = xangle[3];
Anaesthetix 0:978110f7f027 174 xangle[3] = xangle[2];
Anaesthetix 0:978110f7f027 175 xangle[2] = xangle[1];
Anaesthetix 0:978110f7f027 176 xangle[1] = xangle[0];
Anaesthetix 0:978110f7f027 177
Anaesthetix 0:978110f7f027 178
Anaesthetix 0:978110f7f027 179 yangle[0] = ay;
Anaesthetix 0:978110f7f027 180 ay1 = ((0.025*yangle[0]) + (0.025*yangle[1]) + (0.025*yangle[2]) + (0.025*yangle[3]) + (0.025*yangle[4]) + (0.025*yangle[5]) + (0.025*yangle[6]) + (0.025*yangle[7]) + (0.025*yangle[8]) + (0.025*yangle[9]) + (0.025*yangle[10]) + (0.025*yangle[11]) + (0.025*yangle[12]) + (0.025*yangle[13]) + (0.025*yangle[14]) + (0.025*yangle[15]) + (0.025*yangle[16]) + (0.025*yangle[17]) + (0.025*yangle[18]) + (0.025*yangle[19]) + (0.025*yangle[20]) + (0.025*yangle[21]) + (0.025*yangle[22]) + (0.025*yangle[23]) + (0.025*yangle[24]) + (0.025*yangle[25]) + (0.025*yangle[26]) + (0.025*yangle[27]) + (0.025*yangle[28]) + (0.025*yangle[29]) + (0.025*yangle[30]) + (0.025*yangle[31]) + (0.025*yangle[32]) + (0.025*yangle[33]) + (0.025*yangle[34]) + (0.025*yangle[35]) + (0.025*yangle[36]) + (0.025*yangle[37]) + (0.025*yangle[38]) + (0.025*yangle[39]));
Anaesthetix 0:978110f7f027 181 yangle[39] = yangle[38];
Anaesthetix 0:978110f7f027 182 yangle[38] = yangle[37];
Anaesthetix 0:978110f7f027 183 yangle[37] = yangle[36];
Anaesthetix 0:978110f7f027 184 yangle[36] = yangle[35];
Anaesthetix 0:978110f7f027 185 yangle[35] = yangle[34];
Anaesthetix 0:978110f7f027 186 yangle[34] = yangle[33];
Anaesthetix 0:978110f7f027 187 yangle[33] = yangle[32];
Anaesthetix 0:978110f7f027 188 yangle[32] = yangle[31];
Anaesthetix 0:978110f7f027 189 yangle[31] = yangle[30];
Anaesthetix 0:978110f7f027 190 yangle[30] = yangle[29];
Anaesthetix 0:978110f7f027 191 yangle[29] = yangle[28];
Anaesthetix 0:978110f7f027 192 yangle[28] = yangle[27];
Anaesthetix 0:978110f7f027 193 yangle[27] = yangle[26];
Anaesthetix 0:978110f7f027 194 yangle[26] = yangle[25];
Anaesthetix 0:978110f7f027 195 yangle[25] = yangle[24];
Anaesthetix 0:978110f7f027 196 yangle[24] = yangle[23];
Anaesthetix 0:978110f7f027 197 yangle[23] = yangle[22];
Anaesthetix 0:978110f7f027 198 yangle[22] = yangle[21];
Anaesthetix 0:978110f7f027 199 yangle[21] = yangle[20];
Anaesthetix 0:978110f7f027 200 yangle[20] = yangle[19];
Anaesthetix 0:978110f7f027 201 yangle[19] = yangle[18];
Anaesthetix 0:978110f7f027 202 yangle[18] = yangle[17];
Anaesthetix 0:978110f7f027 203 yangle[17] = yangle[16];
Anaesthetix 0:978110f7f027 204 yangle[16] = yangle[15];
Anaesthetix 0:978110f7f027 205 yangle[15] = yangle[14];
Anaesthetix 0:978110f7f027 206 yangle[14] = yangle[13];
Anaesthetix 0:978110f7f027 207 yangle[13] = yangle[12];
Anaesthetix 0:978110f7f027 208 yangle[12] = yangle[11];
Anaesthetix 0:978110f7f027 209 yangle[11] = yangle[10];
Anaesthetix 0:978110f7f027 210 yangle[10] = yangle[9];
Anaesthetix 0:978110f7f027 211 yangle[9] = yangle[8];
Anaesthetix 0:978110f7f027 212 yangle[8] = yangle[7];
Anaesthetix 0:978110f7f027 213 yangle[7] = yangle[6];
Anaesthetix 0:978110f7f027 214 yangle[6] = yangle[5];
Anaesthetix 0:978110f7f027 215 yangle[5] = yangle[4];
Anaesthetix 0:978110f7f027 216 yangle[4] = yangle[3];
Anaesthetix 0:978110f7f027 217 yangle[3] = yangle[2];
Anaesthetix 0:978110f7f027 218 yangle[2] = yangle[1];
Anaesthetix 0:978110f7f027 219 yangle[1] = yangle[0];
Anaesthetix 0:978110f7f027 220
Anaesthetix 0:978110f7f027 221
Anaesthetix 0:978110f7f027 222
Anaesthetix 0:978110f7f027 223
Anaesthetix 0:978110f7f027 224 //Substract angular rate offsets
Anaesthetix 0:978110f7f027 225 xag += xagoffset;
Anaesthetix 0:978110f7f027 226 yag += yagoffset;
Anaesthetix 0:978110f7f027 227 // xag = (xag*-1);
Anaesthetix 0:978110f7f027 228 yag = (yag*-1);
Anaesthetix 0:978110f7f027 229
Anaesthetix 0:978110f7f027 230
Anaesthetix 0:978110f7f027 231 //calc filtervar
Anaesthetix 0:978110f7f027 232 temp6 = 5000000; //time constant [us]
Anaesthetix 0:978110f7f027 233 temp6 = temp6/(temp6+dtgyr);
Anaesthetix 0:978110f7f027 234
Anaesthetix 0:978110f7f027 235
Anaesthetix 0:978110f7f027 236 //complementary filter
Anaesthetix 0:978110f7f027 237 newax = (temp6*(newax + (yag*(dtgyr/1000000)))) + ((1-temp6)*ax1);
Anaesthetix 0:978110f7f027 238 neway = (temp6*(neway + (xag*(dtgyr/1000000)))) + ((1-temp6)*ay1);
Anaesthetix 0:978110f7f027 239 // neway = neway*-1;
Anaesthetix 0:978110f7f027 240
Anaesthetix 0:978110f7f027 241 //if [angle] is NAN use previous value
Anaesthetix 0:978110f7f027 242 if (newax != newax) newax = tempx1;
Anaesthetix 0:978110f7f027 243 if (neway != neway) neway = tempy1;
Anaesthetix 0:978110f7f027 244 if (newax == newax) tempx1 = newax;
Anaesthetix 0:978110f7f027 245 if (neway == neway) tempy1 = neway;
Anaesthetix 0:978110f7f027 246 }
Anaesthetix 0:978110f7f027 247
Anaesthetix 0:978110f7f027 248
Anaesthetix 0:978110f7f027 249
Anaesthetix 0:978110f7f027 250 #endif