Prosper Van / DCM_AHRS_GY80

AHRS based on MatrixPilot DCM algorithm; ported from Pololu MinIMU-9 example code in turn based on ArduPilot 1.5 built for sensor gy_80

Dependents:   quadCommand2

Fork of DCM_AHRS by Michael Shimniok

Diff: DCM.cpp

Revision:
11:8d46121bd255
Parent:
8:195d53710158
Child:
12:e85008094132
diff -r 11a323f190ba -r 8d46121bd255 DCM.cpp
--- a/DCM.cpp	Wed Jul 31 22:17:30 2013 +0000
+++ b/DCM.cpp	Thu Sep 26 05:19:13 2013 +0000
@@ -12,7 +12,7 @@
 //
 //void DCM::pv(float a[3])
 //{
-//    p.printf("%g,\t%g,\t%g \n\r",a[0],a[1],a[2]);
+//    p.printf("%3.2g,\t%3.2g,\t%3.2g \n\r",a[0],a[1],a[2]);
 //}
 //
 //void DCM::pm(float a[3][3])
@@ -169,8 +169,11 @@
 
 void DCM::Euler_angles(void)
 {
-    pitch = RAD2DEG(-asin(dcm[2][0]));
-    roll = RAD2DEG(atan2(dcm[2][1],dcm[2][2]));
+    float newpitch = RAD2DEG(-asin(dcm[2][0]));
+    float newroll = RAD2DEG(atan2(dcm[2][1],dcm[2][2]));
+    
+    pitch = newpitch*0.85 + pitch*0.15;
+    roll = newroll*0.85 + roll*0.15;
     yaw = RAD2DEG(atan2(dcm[1][0],dcm[0][0]));
 }