ok
Dependencies: ADXL345_I2C HMC5843 mbed
main.cpp@1:d47d60db8df3, 2017-10-03 (annotated)
- Committer:
- rulla
- Date:
- Tue Oct 03 16:39:04 2017 +0000
- Revision:
- 1:d47d60db8df3
- Parent:
- 0:af8f98895e2a
Working.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
rulla | 0:af8f98895e2a | 1 | #include "mbed.h" |
rulla | 0:af8f98895e2a | 2 | #include "ADXL345_I2C.h" |
rulla | 0:af8f98895e2a | 3 | #include "HMC5843.h" |
rulla | 0:af8f98895e2a | 4 | |
rulla | 0:af8f98895e2a | 5 | #define N_VAR 15 |
rulla | 0:af8f98895e2a | 6 | |
rulla | 0:af8f98895e2a | 7 | HMC5843 compass(p28, p27); |
rulla | 0:af8f98895e2a | 8 | ADXL345_I2C accelerometer(p28, p27); |
rulla | 0:af8f98895e2a | 9 | Serial pc(USBTX, USBRX); |
rulla | 0:af8f98895e2a | 10 | |
rulla | 0:af8f98895e2a | 11 | int l; |
rulla | 0:af8f98895e2a | 12 | float Complementary(float newAngle, int looptime); |
rulla | 0:af8f98895e2a | 13 | float Complementary2(float newAngle, int looptime); |
rulla | 0:af8f98895e2a | 14 | double kalman_update(int index,double measurement); |
rulla | 0:af8f98895e2a | 15 | float kalmanCalculate(float newAngle, int looptime); |
rulla | 0:af8f98895e2a | 16 | void accMagInit(void); |
rulla | 0:af8f98895e2a | 17 | void ahrs(void); |
rulla | 0:af8f98895e2a | 18 | void kalman_init(void); |
rulla | 0:af8f98895e2a | 19 | |
rulla | 0:af8f98895e2a | 20 | double q[N_VAR]; //process noise covariance |
rulla | 0:af8f98895e2a | 21 | double r[N_VAR]; //measurement noise covariance |
rulla | 0:af8f98895e2a | 22 | double value[N_VAR]; //value |
rulla | 0:af8f98895e2a | 23 | double p[N_VAR]; //estimation error covariance |
rulla | 0:af8f98895e2a | 24 | double k[N_VAR]; //kalman gain |
rulla | 0:af8f98895e2a | 25 | |
rulla | 0:af8f98895e2a | 26 | float Q_angle = 0.01; //0.001 |
rulla | 0:af8f98895e2a | 27 | float R_angle = 0.02; //0.03 |
rulla | 0:af8f98895e2a | 28 | float x_angle = 0.0; |
rulla | 0:af8f98895e2a | 29 | float x_bias = 0.0; |
rulla | 0:af8f98895e2a | 30 | float P_00 = 1.0, P_01 = 0.0, P_10 = 0.0, P_11 = 1.0; |
rulla | 0:af8f98895e2a | 31 | float y, S; |
rulla | 0:af8f98895e2a | 32 | float K_0, K_1; |
rulla | 0:af8f98895e2a | 33 | |
rulla | 0:af8f98895e2a | 34 | int i=0; |
rulla | 0:af8f98895e2a | 35 | int ax=0,ay=0,az=0,bx=0,by=0,bz=0, ax0=0,ay0=0,az0=0; |
rulla | 0:af8f98895e2a | 36 | int mx=0,my=0,mz=0,bmx=0,bmy=0,bmz=0, mx0=0,my0=0,mz0=0; |
rulla | 0:af8f98895e2a | 37 | float x1=0,y1=0,z1=0,x2=0,y2=0,z2=0; |
rulla | 0:af8f98895e2a | 38 | double xd,yd,zd; |
rulla | 0:af8f98895e2a | 39 | double aax,aay,aaz; |
rulla | 0:af8f98895e2a | 40 | int cnt=0; |
rulla | 0:af8f98895e2a | 41 | float thr=10.0; |
rulla | 0:af8f98895e2a | 42 | double ca; double cb; double cc ; |
rulla | 0:af8f98895e2a | 43 | double sa ; double sb ; double sc; |
rulla | 0:af8f98895e2a | 44 | |
rulla | 0:af8f98895e2a | 45 | int main() |
rulla | 0:af8f98895e2a | 46 | { |
rulla | 0:af8f98895e2a | 47 | pc.baud(115200); |
rulla | 0:af8f98895e2a | 48 | |
rulla | 0:af8f98895e2a | 49 | kalman_init(); |
rulla | 0:af8f98895e2a | 50 | accMagInit(); |
rulla | 0:af8f98895e2a | 51 | |
rulla | 0:af8f98895e2a | 52 | while (1) { |
rulla | 0:af8f98895e2a | 53 | ahrs(); |
rulla | 0:af8f98895e2a | 54 | } |
rulla | 0:af8f98895e2a | 55 | } |
rulla | 0:af8f98895e2a | 56 | |
rulla | 0:af8f98895e2a | 57 | |
rulla | 0:af8f98895e2a | 58 | double kalman_update(int index,double measurement) |
rulla | 0:af8f98895e2a | 59 | { |
rulla | 0:af8f98895e2a | 60 | //prediction update |
rulla | 0:af8f98895e2a | 61 | //omit x = x |
rulla | 0:af8f98895e2a | 62 | p[index]+= q[index]; |
rulla | 0:af8f98895e2a | 63 | //measurement update |
rulla | 0:af8f98895e2a | 64 | k[index] = p[index] / (p[index] + r[index]); |
rulla | 0:af8f98895e2a | 65 | value[index] += k[index] * (measurement - value[index]); |
rulla | 0:af8f98895e2a | 66 | p[index] *= (1.0 - k[index]); |
rulla | 0:af8f98895e2a | 67 | //fprintf(out,"%f %f \r\n",p,k); |
rulla | 0:af8f98895e2a | 68 | return value[index]; |
rulla | 0:af8f98895e2a | 69 | } |
rulla | 0:af8f98895e2a | 70 | |
rulla | 0:af8f98895e2a | 71 | void kalman_init(void) |
rulla | 0:af8f98895e2a | 72 | { |
rulla | 0:af8f98895e2a | 73 | float s1,s2; |
rulla | 0:af8f98895e2a | 74 | //pc.scanf("Q=%f r=%f\n",&s1,&s2); |
rulla | 0:af8f98895e2a | 75 | for(l=0; l<N_VAR; l++) { |
rulla | 0:af8f98895e2a | 76 | q[l]=0.01; //process noise covariance |
rulla | 0:af8f98895e2a | 77 | r[l]=0.8; //measurement noise covariance |
rulla | 0:af8f98895e2a | 78 | p[l]=0.0; //estimation error covariance |
rulla | 0:af8f98895e2a | 79 | k[l]=0.0; //kalman gain |
rulla | 0:af8f98895e2a | 80 | value[l]=0.0; |
rulla | 0:af8f98895e2a | 81 | } |
rulla | 0:af8f98895e2a | 82 | } |
rulla | 0:af8f98895e2a | 83 | |
rulla | 0:af8f98895e2a | 84 | float kalmanCalculate(float newAngle, int looptime) |
rulla | 0:af8f98895e2a | 85 | { |
rulla | 0:af8f98895e2a | 86 | float dt =(float)((looptime)/1000.0); |
rulla | 0:af8f98895e2a | 87 | P_00 += - dt * (P_10 + P_01) + Q_angle * dt; |
rulla | 0:af8f98895e2a | 88 | P_01 += - dt * P_11; |
rulla | 0:af8f98895e2a | 89 | P_10 += - dt * P_11; |
rulla | 0:af8f98895e2a | 90 | P_11 += - dt * (P_10 + P_01) + Q_angle * dt; |
rulla | 0:af8f98895e2a | 91 | y = newAngle - x_angle; |
rulla | 0:af8f98895e2a | 92 | S = P_00 + R_angle; |
rulla | 0:af8f98895e2a | 93 | K_0 = P_00 / S; |
rulla | 0:af8f98895e2a | 94 | K_1 = P_10 / S; |
rulla | 0:af8f98895e2a | 95 | x_angle += K_0 * y; |
rulla | 0:af8f98895e2a | 96 | x_bias += K_1 * y; |
rulla | 0:af8f98895e2a | 97 | P_00 -= K_0 * P_00; |
rulla | 0:af8f98895e2a | 98 | P_01 -= K_0 * P_01; |
rulla | 0:af8f98895e2a | 99 | P_10 -= K_1 * P_00; |
rulla | 0:af8f98895e2a | 100 | P_11 -= K_1 * P_01; |
rulla | 0:af8f98895e2a | 101 | //fprintf(out,"%f %f %f %f %f %f %f\r\n",P_00,P_01,P_10,P_11,K_0,K_1,S); |
rulla | 0:af8f98895e2a | 102 | return x_angle*1.5; |
rulla | 0:af8f98895e2a | 103 | } |
rulla | 0:af8f98895e2a | 104 | |
rulla | 0:af8f98895e2a | 105 | |
rulla | 0:af8f98895e2a | 106 | void accMagInit(void) |
rulla | 0:af8f98895e2a | 107 | { |
rulla | 0:af8f98895e2a | 108 | |
rulla | 0:af8f98895e2a | 109 | int breadings[3] = {0, 0, 0}; |
rulla | 0:af8f98895e2a | 110 | int bhmcreadings[3]; |
rulla | 0:af8f98895e2a | 111 | char buffer[3]; |
rulla | 0:af8f98895e2a | 112 | pc.printf("Starting ADXL345 test...\n"); |
rulla | 0:af8f98895e2a | 113 | pc.printf("Device ID is: 0x%02x\n", accelerometer.getDevId()); |
rulla | 0:af8f98895e2a | 114 | accelerometer.setPowerControl(0x00); |
rulla | 0:af8f98895e2a | 115 | //Full resolution, +/-16g, 4mg/LSB. |
rulla | 0:af8f98895e2a | 116 | accelerometer.setDataFormatControl(0x0B); |
rulla | 0:af8f98895e2a | 117 | //3.2kHz data rate. |
rulla | 0:af8f98895e2a | 118 | accelerometer.setDataRate(ADXL345_3200HZ); |
rulla | 0:af8f98895e2a | 119 | accelerometer.setPowerControl(0x08); |
rulla | 0:af8f98895e2a | 120 | compass.getAddress(buffer); |
rulla | 0:af8f98895e2a | 121 | pc.printf("Magnetic Compass Id=%c%c%c \n\r",buffer[0],buffer[1],buffer[2]); |
rulla | 0:af8f98895e2a | 122 | compass.setDefault(); |
rulla | 0:af8f98895e2a | 123 | wait(.1); |
rulla | 0:af8f98895e2a | 124 | for(i=0; i<50; i++) { |
rulla | 0:af8f98895e2a | 125 | accelerometer.getOutput(breadings); |
rulla | 0:af8f98895e2a | 126 | //13-bit, sign extended values. |
rulla | 0:af8f98895e2a | 127 | compass.readData(bhmcreadings); |
rulla | 0:af8f98895e2a | 128 | wait(0.1); |
rulla | 0:af8f98895e2a | 129 | ax+=(int16_t)(breadings[0]); |
rulla | 0:af8f98895e2a | 130 | ay+=(int16_t)(breadings[1]); |
rulla | 0:af8f98895e2a | 131 | az+=(int16_t)(breadings[2]); |
rulla | 0:af8f98895e2a | 132 | mx+=(int16_t)(bhmcreadings[0]); |
rulla | 0:af8f98895e2a | 133 | my+=(int16_t)(bhmcreadings[1]); |
rulla | 0:af8f98895e2a | 134 | mz+=(int16_t)(bhmcreadings[2]); |
rulla | 0:af8f98895e2a | 135 | } |
rulla | 0:af8f98895e2a | 136 | ax0=ax/50; |
rulla | 0:af8f98895e2a | 137 | ay0=ay/50; |
rulla | 0:af8f98895e2a | 138 | az0=az/50; |
rulla | 0:af8f98895e2a | 139 | mx0=mx/50; |
rulla | 0:af8f98895e2a | 140 | my0=my/50; |
rulla | 0:af8f98895e2a | 141 | mz0=mz/50; |
rulla | 0:af8f98895e2a | 142 | ax=0; |
rulla | 0:af8f98895e2a | 143 | ay=0; |
rulla | 0:af8f98895e2a | 144 | az=0; |
rulla | 0:af8f98895e2a | 145 | mx=0; |
rulla | 0:af8f98895e2a | 146 | my=0; |
rulla | 0:af8f98895e2a | 147 | mz=0; |
rulla | 0:af8f98895e2a | 148 | } |
rulla | 0:af8f98895e2a | 149 | |
rulla | 0:af8f98895e2a | 150 | void ahrs(void) |
rulla | 0:af8f98895e2a | 151 | { |
rulla | 0:af8f98895e2a | 152 | cnt=0; |
rulla | 0:af8f98895e2a | 153 | int readings[3] = {0, 0, 0}; |
rulla | 0:af8f98895e2a | 154 | int hmcreadings[3]; |
rulla | 0:af8f98895e2a | 155 | // int creadings[3] = {0, 0, 0}; |
rulla | 0:af8f98895e2a | 156 | // int bhmcreadings[3]; |
rulla | 0:af8f98895e2a | 157 | accelerometer.getOutput(readings); |
rulla | 0:af8f98895e2a | 158 | compass.readData(hmcreadings); |
rulla | 0:af8f98895e2a | 159 | //13-bit, sign extended values. |
rulla | 0:af8f98895e2a | 160 | cnt++; |
rulla | 0:af8f98895e2a | 161 | ax+=(int16_t)(readings[0]); |
rulla | 0:af8f98895e2a | 162 | ay+=(int16_t)(readings[1]); |
rulla | 0:af8f98895e2a | 163 | az+=(int16_t)(readings[2]); |
rulla | 0:af8f98895e2a | 164 | ax-=ax0; |
rulla | 0:af8f98895e2a | 165 | ay-=ay0; |
rulla | 0:af8f98895e2a | 166 | az-=az0; |
rulla | 0:af8f98895e2a | 167 | mx+=(int16_t)(hmcreadings[0]); |
rulla | 0:af8f98895e2a | 168 | my+=(int16_t)(hmcreadings[1]); |
rulla | 0:af8f98895e2a | 169 | mz+=(int16_t)(hmcreadings[2]); |
rulla | 0:af8f98895e2a | 170 | |
rulla | 0:af8f98895e2a | 171 | mx-=mx0; |
rulla | 0:af8f98895e2a | 172 | my-=my0; |
rulla | 0:af8f98895e2a | 173 | mz-=mz0; |
rulla | 0:af8f98895e2a | 174 | |
rulla | 0:af8f98895e2a | 175 | wait(0.1); |
rulla | 0:af8f98895e2a | 176 | |
rulla | 0:af8f98895e2a | 177 | /* |
rulla | 0:af8f98895e2a | 178 | accelerometer.getOutput(creadings); |
rulla | 0:af8f98895e2a | 179 | compass.readData(bhmcreadings); |
rulla | 0:af8f98895e2a | 180 | //13-bit, sign extended values. |
rulla | 0:af8f98895e2a | 181 | cnt++; |
rulla | 0:af8f98895e2a | 182 | bx+=(int16_t)(creadings[0]); |
rulla | 0:af8f98895e2a | 183 | by+=(int16_t)(creadings[1]); |
rulla | 0:af8f98895e2a | 184 | bz+=(int16_t)(creadings[2]); |
rulla | 0:af8f98895e2a | 185 | bx-=ax0; |
rulla | 0:af8f98895e2a | 186 | by-=ay0; |
rulla | 0:af8f98895e2a | 187 | bz-=az0; |
rulla | 0:af8f98895e2a | 188 | bmx+=(int16_t)(bhmcreadings[0]); |
rulla | 0:af8f98895e2a | 189 | bmy+=(int16_t)(bhmcreadings[1]); |
rulla | 0:af8f98895e2a | 190 | bmz+=(int16_t)(bhmcreadings[2]); |
rulla | 0:af8f98895e2a | 191 | bmx-=mx0; |
rulla | 0:af8f98895e2a | 192 | bmy-=my0; |
rulla | 0:af8f98895e2a | 193 | bmz-=mz0; |
rulla | 0:af8f98895e2a | 194 | wait(0.1); |
rulla | 0:af8f98895e2a | 195 | |
rulla | 0:af8f98895e2a | 196 | |
rulla | 0:af8f98895e2a | 197 | // if(cnt%10==0) { |
rulla | 0:af8f98895e2a | 198 | // if(cnt==2) { |
rulla | 0:af8f98895e2a | 199 | |
rulla | 0:af8f98895e2a | 200 | if((bx-ax)<thr) x1=0; |
rulla | 0:af8f98895e2a | 201 | if((by-ay)<thr) y1=0; |
rulla | 0:af8f98895e2a | 202 | if((bz-az)<thr) z1=0; |
rulla | 0:af8f98895e2a | 203 | xd=((float)(ax+bx)/cnt); |
rulla | 0:af8f98895e2a | 204 | yd=((float)(ay+by)/cnt); |
rulla | 0:af8f98895e2a | 205 | zd=((float)(az+bz)/cnt); |
rulla | 0:af8f98895e2a | 206 | x1+=kalman_update(3,xd); |
rulla | 0:af8f98895e2a | 207 | x2+=kalman_update(6,x1); |
rulla | 0:af8f98895e2a | 208 | y1+= kalman_update(4,yd); |
rulla | 0:af8f98895e2a | 209 | y2+=kalman_update(7,y1); |
rulla | 0:af8f98895e2a | 210 | z1+= kalman_update(5,zd); |
rulla | 0:af8f98895e2a | 211 | z2+=kalman_update(8,z1); |
rulla | 0:af8f98895e2a | 212 | // pc.printf("%f %f %f\n",x2,y2,z2); |
rulla | 0:af8f98895e2a | 213 | */ |
rulla | 0:af8f98895e2a | 214 | //pc.printf("%d %d %d\n",ax,ay,az); |
rulla | 0:af8f98895e2a | 215 | |
rulla | 0:af8f98895e2a | 216 | |
rulla | 0:af8f98895e2a | 217 | /* |
rulla | 0:af8f98895e2a | 218 | xd=(float)(ax)*488.288700694; |
rulla | 0:af8f98895e2a | 219 | yd=(float)(ay)*488.288700694; |
rulla | 0:af8f98895e2a | 220 | zd=(float)(az)*0.0488288700694; |
rulla | 0:af8f98895e2a | 221 | */ |
rulla | 0:af8f98895e2a | 222 | xd=(double)(ax); |
rulla | 0:af8f98895e2a | 223 | yd=(double)(ay); |
rulla | 0:af8f98895e2a | 224 | zd=(double)(az); |
rulla | 0:af8f98895e2a | 225 | |
rulla | 0:af8f98895e2a | 226 | |
rulla | 0:af8f98895e2a | 227 | //Roll, Pitch |
rulla | 0:af8f98895e2a | 228 | // aax=(double)xd; |
rulla | 0:af8f98895e2a | 229 | //aay=(double)yd; |
rulla | 0:af8f98895e2a | 230 | //aaz=(double)zd; |
rulla | 0:af8f98895e2a | 231 | |
rulla | 0:af8f98895e2a | 232 | aax=kalman_update(9,xd); //Kalman to accelerometer |
rulla | 0:af8f98895e2a | 233 | aay=kalman_update(10,yd); |
rulla | 0:af8f98895e2a | 234 | aaz=kalman_update(11,-zd); |
rulla | 0:af8f98895e2a | 235 | |
rulla | 0:af8f98895e2a | 236 | /* |
rulla | 0:af8f98895e2a | 237 | x1+=kalman_update(3,aax); |
rulla | 0:af8f98895e2a | 238 | y1+= kalman_update(4,aay); |
rulla | 0:af8f98895e2a | 239 | z1+= kalman_update(5,aaz); |
rulla | 0:af8f98895e2a | 240 | |
rulla | 0:af8f98895e2a | 241 | if(aax<=1.6420 && aax>=-1.6420 ) x1=0.0; |
rulla | 0:af8f98895e2a | 242 | if(aay<=1.2315 && aay>=-1.1325 ) y1=0.0; |
rulla | 0:af8f98895e2a | 243 | if(aaz<=8.374 && aaz>=-8.374 ) z1=0.0; |
rulla | 0:af8f98895e2a | 244 | |
rulla | 0:af8f98895e2a | 245 | x2+=kalman_update(6,x1); |
rulla | 0:af8f98895e2a | 246 | y2+=kalman_update(7,y1); |
rulla | 0:af8f98895e2a | 247 | z2+=kalman_update(8,z1); |
rulla | 0:af8f98895e2a | 248 | |
rulla | 0:af8f98895e2a | 249 | |
rulla | 0:af8f98895e2a | 250 | if(aax<0.01 && aax>-0.01 ) x1=0; |
rulla | 0:af8f98895e2a | 251 | if(aay<0.0075 && aay>-0.0075 ) y1=0; |
rulla | 0:af8f98895e2a | 252 | if(aaz<0.051 && aaz>-0.051 ) z1=0; |
rulla | 0:af8f98895e2a | 253 | |
rulla | 0:af8f98895e2a | 254 | x1+=aax; |
rulla | 0:af8f98895e2a | 255 | x2+=kalman_update(6,x1); |
rulla | 0:af8f98895e2a | 256 | y1+=aay; |
rulla | 0:af8f98895e2a | 257 | y2+=kalman_update(7,y1); |
rulla | 0:af8f98895e2a | 258 | z1+= aaz; |
rulla | 0:af8f98895e2a | 259 | z2+=kalman_update(8,z1); |
rulla | 0:af8f98895e2a | 260 | */ |
rulla | 0:af8f98895e2a | 261 | //double normA=sqrt(pow(aax,2)+pow(aay,2)+pow(aaz,2)); |
rulla | 0:af8f98895e2a | 262 | //aax/=normA; |
rulla | 0:af8f98895e2a | 263 | //aay/=normA; |
rulla | 0:af8f98895e2a | 264 | //aaz/=normA; |
rulla | 0:af8f98895e2a | 265 | // double xAngle = atan2( aax , (sqrt(pow(aay,2) + pow(aaz,2)))); |
rulla | 0:af8f98895e2a | 266 | //double yAngle = atan2( aay , (sqrt(pow(aax,2) + pow(aaz,2)))); |
rulla | 0:af8f98895e2a | 267 | // double yAngle = atan2( (sqrt(pow(aax,2) + pow(aay,2))), aaz); |
rulla | 0:af8f98895e2a | 268 | // double zAngle = atan2( sqrt(pow(aax,2) + pow(aay,2)) , aaz); |
rulla | 0:af8f98895e2a | 269 | |
rulla | 0:af8f98895e2a | 270 | double xAngle=-0.00609*aay; |
rulla | 0:af8f98895e2a | 271 | double yAngle=0.00609*aax; |
rulla | 0:af8f98895e2a | 272 | //double xAngle=-aay; |
rulla | 0:af8f98895e2a | 273 | //double yAngle=aax; |
rulla | 0:af8f98895e2a | 274 | //double yAngle=atan2(aay,aaz); //Roll |
rulla | 0:af8f98895e2a | 275 | //double xAngle=atan(-aax/(aay*sin(yAngle)+aaz*cos(yAngle))); |
rulla | 0:af8f98895e2a | 276 | cnt=0; |
rulla | 0:af8f98895e2a | 277 | ax=0; |
rulla | 0:af8f98895e2a | 278 | ay=0; |
rulla | 0:af8f98895e2a | 279 | az=0; |
rulla | 0:af8f98895e2a | 280 | bx=0; |
rulla | 0:af8f98895e2a | 281 | by=0; |
rulla | 0:af8f98895e2a | 282 | bz=0; |
rulla | 0:af8f98895e2a | 283 | ////////Tilt compensated heading |
rulla | 0:af8f98895e2a | 284 | /* |
rulla | 0:af8f98895e2a | 285 | double mmx=(mx+bmx)*0.5; |
rulla | 0:af8f98895e2a | 286 | double mmy=(my+bmy)*0.5; |
rulla | 0:af8f98895e2a | 287 | double mmz=(mz+bmz)*0.5; //Mag vector components |
rulla | 0:af8f98895e2a | 288 | */ |
rulla | 0:af8f98895e2a | 289 | |
rulla | 0:af8f98895e2a | 290 | double mmx=(double)(mx); |
rulla | 0:af8f98895e2a | 291 | double mmy=(double)(my); |
rulla | 0:af8f98895e2a | 292 | double mmz=(double)(mz); |
rulla | 0:af8f98895e2a | 293 | //double normM=sqrt(pow(mmx,2)+pow(mmy,2)+pow(mmz,2)); |
rulla | 0:af8f98895e2a | 294 | //mmx/=normM; |
rulla | 0:af8f98895e2a | 295 | //mmy/=normM; |
rulla | 0:af8f98895e2a | 296 | //mmz/=normM; |
rulla | 0:af8f98895e2a | 297 | double fx=kalman_update(0,xAngle); //Kalman to angles |
rulla | 0:af8f98895e2a | 298 | double fy=kalman_update(1,yAngle); |
rulla | 0:af8f98895e2a | 299 | double fz=0.0; |
rulla | 0:af8f98895e2a | 300 | |
rulla | 0:af8f98895e2a | 301 | //double fx=xAngle; //PITCH |
rulla | 0:af8f98895e2a | 302 | //double fy=yAngle; //ROLL |
rulla | 0:af8f98895e2a | 303 | |
rulla | 0:af8f98895e2a | 304 | //double yh=mmy*cos(fx)+mmz*sin(fx); |
rulla | 0:af8f98895e2a | 305 | // double xh=mmx*sin(fy)*sin(fx)+mmy*cos(fy)-mmz*sin(fy)*cos(fx); |
rulla | 0:af8f98895e2a | 306 | //double xh=mmy*sin(fy)*sin(fx)+mmx*cos(fy)+mmz*sin(fy)*cos(fx); |
rulla | 0:af8f98895e2a | 307 | //double yh=mmy*cos(fx)-mmz*sin(fx); |
rulla | 0:af8f98895e2a | 308 | //double xh=mmx*sin(fy)*sin(fx)+mmy*cos(fy)-mmz*sin(fy)*cos(fx); |
rulla | 0:af8f98895e2a | 309 | //double yh=mmx*cos(fx)+mmz*sin(fx); |
rulla | 0:af8f98895e2a | 310 | |
rulla | 0:af8f98895e2a | 311 | ca = cos(fx);//theta |
rulla | 0:af8f98895e2a | 312 | cb = cos(fy);//phi |
rulla | 0:af8f98895e2a | 313 | sa = sin(fx); |
rulla | 0:af8f98895e2a | 314 | sb = sin(fy); |
rulla | 0:af8f98895e2a | 315 | |
rulla | 0:af8f98895e2a | 316 | double yh=mmz*sb-mmy*cb; |
rulla | 0:af8f98895e2a | 317 | double xh=+mmx*ca+mmy*sa*sb+mmz*sa*cb; |
rulla | 0:af8f98895e2a | 318 | // double normM=sqrt(pow(xh,2)+pow(yh,2)); |
rulla | 0:af8f98895e2a | 319 | //xh/=normM; |
rulla | 0:af8f98895e2a | 320 | //yh/=normM; |
rulla | 0:af8f98895e2a | 321 | // double head=0.3*atan2(yh,xh); |
rulla | 0:af8f98895e2a | 322 | double head=atan2(yh,xh); |
rulla | 0:af8f98895e2a | 323 | //double head=atan2(mmy,mmx); |
rulla | 0:af8f98895e2a | 324 | |
rulla | 0:af8f98895e2a | 325 | |
rulla | 0:af8f98895e2a | 326 | const float pi=3.14; |
rulla | 0:af8f98895e2a | 327 | // if (fz > pi) fz -= (2.0*pi); |
rulla | 0:af8f98895e2a | 328 | //if (fz < -pi) fz += (2.0*pi); |
rulla | 0:af8f98895e2a | 329 | //if (fz < 0.0) fz += 2.0*pi; |
rulla | 0:af8f98895e2a | 330 | |
rulla | 0:af8f98895e2a | 331 | //head=atan2(aay,(sqrt(pow(aax,2) + pow(aaz,2)))); |
rulla | 0:af8f98895e2a | 332 | |
rulla | 0:af8f98895e2a | 333 | /* |
rulla | 0:af8f98895e2a | 334 | double xh=mmx*cos(fx)+mmy*sin(fx)*sin(fy)+mmz*cos(fx)*sin(fy); |
rulla | 0:af8f98895e2a | 335 | double yh=mmy*cos(fy)-mmz*sin(fy); |
rulla | 0:af8f98895e2a | 336 | double head=atan2(-yh,xh); |
rulla | 0:af8f98895e2a | 337 | */ |
rulla | 0:af8f98895e2a | 338 | fz=kalman_update(2,head); |
rulla | 0:af8f98895e2a | 339 | //fz=head; |
rulla | 0:af8f98895e2a | 340 | //double fz=head; |
rulla | 0:af8f98895e2a | 341 | |
rulla | 0:af8f98895e2a | 342 | /* |
rulla | 0:af8f98895e2a | 343 | cc = cos(fz); |
rulla | 0:af8f98895e2a | 344 | sc = sin(fz); |
rulla | 0:af8f98895e2a | 345 | double x=1.0,y=1.0,z=1.0; |
rulla | 0:af8f98895e2a | 346 | double xrot=x2+cb*cc*x+(-cc*sa*sb-ca*sc)*y+(ca*cc*sb-sa*sc)*z; |
rulla | 0:af8f98895e2a | 347 | double yrot=y2+cb*sc*x+(ca*cc-sa*sb*sc)*y+(cc*sa+ca*sb*sc)*z; |
rulla | 0:af8f98895e2a | 348 | double zrot=z2+sb*x-cb*sa*y+ca*cb*z; |
rulla | 0:af8f98895e2a | 349 | |
rulla | 0:af8f98895e2a | 350 | xrot=cb*cc*x+(-cc*sa*sb-ca*sc)*y+(ca*cc*sb-sa*sc)*z; |
rulla | 0:af8f98895e2a | 351 | yrot=cb*sc*x+(ca*cc-sa*sb*sc)*y+(cc*sa+ca*sb*sc)*z; |
rulla | 0:af8f98895e2a | 352 | zrot=sb*x-cb*sa*y+ca*cb*z; |
rulla | 0:af8f98895e2a | 353 | */ |
rulla | 0:af8f98895e2a | 354 | // pc.printf("%f %f %f\n",xrot,yrot,zrot); |
rulla | 0:af8f98895e2a | 355 | pc.printf("%f %f %f %f %f %f %f %f %f\n",fx,fy,fz,mmx,mmy,atan2(mmy,mmx),aax,aay,aaz); |
rulla | 0:af8f98895e2a | 356 | // pc.printf("%f %f %f %f %f %f %f %f %f\n",fx,fy,fz,x2,y2,z2,kalman_update(9,x2),kalman_update(10,y2),kalman_update(11,z2)); |
rulla | 0:af8f98895e2a | 357 | |
rulla | 0:af8f98895e2a | 358 | mx=0; |
rulla | 0:af8f98895e2a | 359 | my=0; |
rulla | 0:af8f98895e2a | 360 | mz=0; |
rulla | 0:af8f98895e2a | 361 | bmx=0; |
rulla | 0:af8f98895e2a | 362 | bmy=0; |
rulla | 0:af8f98895e2a | 363 | bmz=0; |
rulla | 0:af8f98895e2a | 364 | } |