New heading fusion improvement!

treygerman · Dec 13, 2012 · 355eaf8 · 355eaf8
1 parent 2144c7b
commit 355eaf8
Showing 1 changed file with 8 additions and 13 deletions.
diff --git a/Libraries/AQ_Kinematics/HeadingFusionProcessorMARG.h b/Libraries/AQ_Kinematics/HeadingFusionProcessorMARG.h
@@ -35,8 +35,6 @@ float compassDeclination = 0.0;
 
 float headingAngle[3] = {0.0,0.0,0.0};
 
-float lkpAcc = 0.0;                					// proportional gain governs rate of convergence to accelerometer
-float lkiAcc = 0.0;                					// integral gain governs rate of convergence of gyroscope biases
 float lkpMag = 0.0;                					// proportional gain governs rate of convergence to magnetometer
 float lkiMag = 0.0;                					// integral gain governs rate of convergence of gyroscope biases
 float lhalfT = 0.0;                					// half the sample period
@@ -86,14 +84,14 @@ void headingUpdate(float gx, float gy, float gz, float mx, float my, float mz, f
   ezMag = (mx*wy - my*wx);
 
   // integral error scaled integral gain
-  lexInt += exAcc*lkiAcc;
-  leyInt += eyAcc*lkiAcc;
-  lezInt += ezAcc*lkiAcc;
+  lexInt += exAcc;
+  leyInt += eyAcc;
+  lezInt += ezAcc;
 
   // adjusted gyroscope measurements
-  gx = gx + lkpAcc*exAcc + lexInt;
-  gy = gy + lkpAcc*eyAcc + leyInt;
-  gz = gz + lkpAcc*ezAcc + ezMag*lkpMag + lezInt;
+  gx = gx + exAcc + lexInt;
+  gy = gy + eyAcc + leyInt;
+  gz = gz + ezAcc + ezMag*lkpMag + lezInt;
 
 
   // integrate quaternion rate and normalise
@@ -134,11 +132,8 @@ void initializeHeadingFusion()
   lq2 = cos(0.0)*sin(0.0)*cos(yawAngle/2) + sin(0.0)*cos(0.0)*sin(yawAngle/2);
   lq3 = cos(0.0)*cos(0.0)*sin(yawAngle/2) - sin(0.0)*sin(0.0)*cos(yawAngle/2);
 
-  lkpAcc = 0.2;
-  lkiAcc = 0.0005;
-
-  lkpMag = 0.2;//2.0;
-  lkiMag = 0.0005;//0.005;
+  lkpMag = 2.0;//2.0;
+  lkiMag = 0.005;//0.005;
 }