angelo mottolese / Mbed 2 deprecated Kalman_IMU_ADX345_HMC5843_copy

ok

Dependencies:   ADXL345_I2C HMC5843 mbed

main.cpp@0:af8f98895e2a, 2017-10-03 (annotated)

Committer:
rulla
Date:
Tue Oct 03 16:37:07 2017 +0000
Revision:
0:af8f98895e2a
working

Who changed what in which revision?

UserRevisionLine numberNew 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 }