From 16c55c64d4a64bcf8b7fbe3179bdcec60ae40437 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Fri, 29 Jun 2012 14:53:06 +1000
Subject: [PATCH] AHRS: don't add the P terms in _omega

this can lead feedback via the _P_gain()
---
 libraries/AP_AHRS/AP_AHRS_DCM.cpp | 10 +++++++---
 1 file changed, 7 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
index 7683546fac..f5028db50f 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
@@ -76,10 +76,14 @@ AP_AHRS_DCM::update(void)
 void
 AP_AHRS_DCM::matrix_update(float _G_Dt)
 {
-	// Equation 16, adding proportional and integral correction terms
-	_omega = _gyro_vector + _omega_I + _omega_P + _omega_yaw_P;
+	// note that we do not include the P terms in _omega. This is
+	// because the spin_rate is calculated from _omega.length(),
+	// and including the P terms would give positive feedback into
+	// the _P_gain() calculation, which can lead to a very large P
+	// value
+	_omega = _gyro_vector + _omega_I;
 
-	_dcm_matrix.rotate(_omega * _G_Dt);
+	_dcm_matrix.rotate((_omega + _omega_P + _omega_yaw_P) * _G_Dt);
 }