From 0fa1f29cf3e10eb25f9ffe4733e80391cf333493 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sat, 23 Jun 2012 13:33:57 +1000
Subject: [PATCH] AHRS: implement spin rate limits

this follows the method that Bill developed in his fastRotations
paper. We've demonstrated that this is indeed needed in APM, as we
were able to produce the 'dizzy' effects in both the ArduPlane and
ArduCopter simulator
---
 libraries/AP_AHRS/AP_AHRS_DCM.cpp | 41 ++++++++++++++++++++++++++-----
 libraries/AP_AHRS/AP_AHRS_DCM.h   |  3 +++
 2 files changed, 38 insertions(+), 6 deletions(-)

diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
index 271ce4b78d..d5bd1b9c0d 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
@@ -23,6 +23,12 @@
 // from the GPS and wait for the ground speed to get above GPS_SPEED_MIN
 #define GPS_SPEED_RESET 100
 
+// the limit (in degrees/second) beyond which we stop integrating
+// omega_I. At larger spin rates the DCM PI controller can get 'dizzy'
+// which results in false gyro drift. See
+// http://gentlenav.googlecode.com/files/fastRotations.pdf
+#define SPIN_RATE_LIMIT 20
+
 // table of user settable parameters
 const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = {
 	// @Param: YAW_P
@@ -257,6 +263,18 @@ AP_AHRS_DCM::yaw_error_gps(void)
 }
 
 
+float
+AP_AHRS_DCM::_P_gain(float spin_rate)
+{
+	if (spin_rate < ToDeg(50)) {
+		return 1.0;
+	}
+	if (spin_rate > ToDeg(500)) {
+		return 10.0;
+	}
+	return spin_rate/ToDeg(50);
+}
+
 // yaw drift correction using the compass or GPS
 // this function prodoces the _omega_yaw_P vector, and also
 // contributes to the _omega_I.z long term yaw drift estimate
@@ -309,13 +327,19 @@ AP_AHRS_DCM::drift_correction_yaw(float deltat)
 	// convert the error vector to body frame
 	error = _dcm_matrix.mul_transpose(error);
 
+	// the spin rate changes the P gain, and disables the
+	// integration at higher rates
+	float spin_rate = _omega.length();
+
 	// update the proportional control to drag the
-	// yaw back to the right value
-	_omega_yaw_P = error * _kp_yaw.get();
+	// yaw back to the right value. We use a gain
+	// that depends on the spin rate. See the fastRotations.pdf
+	// paper from Bill Premerlani
+	_omega_yaw_P = error * _P_gain(spin_rate) * _kp_yaw.get();
 
 	// don't update the drift term if we lost the yaw reference
 	// for more than 2 seconds
-	if (yaw_deltat < 2.0) {
+	if (yaw_deltat < 2.0 && spin_rate < ToRad(SPIN_RATE_LIMIT)) {
 		// also add to the I term
 		_omega_I_sum.z += error.z * _ki_yaw * yaw_deltat;
 	}
@@ -426,14 +450,19 @@ AP_AHRS_DCM::drift_correction(float deltat)
     // convert the error term to body frame
     error = _dcm_matrix.mul_transpose(error);
 
+    // base the P gain on the spin rate
+    float spin_rate = _omega.length();
+
     // we now want to calculate _omega_P and _omega_I. The
     // _omega_P value is what drags us quickly to the
     // accelerometer reading.
-    _omega_P = error * _kp;
+    _omega_P = error * _P_gain(spin_rate) * _kp;
 
     // accumulate some integrator error
-    _omega_I_sum += error * _ki * _ra_deltat;
-    _omega_I_sum_time += _ra_deltat;
+    if (spin_rate < ToRad(SPIN_RATE_LIMIT)) {
+	    _omega_I_sum += error * _ki * _ra_deltat;
+	    _omega_I_sum_time += _ra_deltat;
+    }
 
     if (_omega_I_sum_time >= 5) {
 	    // limit the rate of change of omega_I to the hardware
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h
index 66f1df9850..91aedb527e 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.h
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.h
@@ -75,6 +75,9 @@ private:
 	float		_omega_I_sum_time;
 	Vector3f 	_omega;			// Corrected Gyro_Vector data
 
+	// P term gain based on spin rate
+	float 		_P_gain(float spin_rate);
+
 	// state to support status reporting
 	float		_renorm_val_sum;
 	uint16_t	_renorm_val_count;