From f70dfe440de69acef57410b1b439772445ae0863 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 5 Mar 2012 11:27:45 +1100
Subject: [PATCH] Quaternion: fix the gyro bias in centripetal and remove
 smoothing

the centripetal code needs to take account of the current gyro
bias.

It turned out that the accel and gyro smoothing was causing
significant control lag, and we're better off just letting the
quaternion code handle it via its own smoothing parameters
---
 libraries/AP_Quaternion/AP_Quaternion.cpp | 28 ++++++++++++++---------
 libraries/AP_Quaternion/AP_Quaternion.h   | 21 +++++++++++------
 2 files changed, 31 insertions(+), 18 deletions(-)

diff --git a/libraries/AP_Quaternion/AP_Quaternion.cpp b/libraries/AP_Quaternion/AP_Quaternion.cpp
index 1b4346657f..6c9455714e 100644
--- a/libraries/AP_Quaternion/AP_Quaternion.cpp
+++ b/libraries/AP_Quaternion/AP_Quaternion.cpp
@@ -269,27 +269,30 @@ void AP_Quaternion::update(void)
 	gyro = _imu->get_gyro();
 	accel = _imu->get_accel();
 
-	// keep a smoothed gyro vector for centripetal and reporting
-	// to mainline code
-	_gyro_smoothed = (_gyro_smoothed * 0.95) + (gyro * 0.05);
-
-	// Quaternion code uses opposite x and y gyro sense
+	// Quaternion code uses opposite x and y gyro sense from the
+	// rest of APM
 	gyro.x = -gyro.x;
 	gyro.y = -gyro.y;
 
+	// Quaternion code uses opposite z accel as well
+	accel.z = -accel.z;
 
 	if (_centripetal && _gps && _gps->status() == GPS::GPS_OK) {
+		// compensate for linear acceleration, limited to 1g
+		float acceleration = _gps->acceleration();
+		acceleration = constrain(acceleration, 0, 9.8);
+		accel.x -= acceleration;
+
 		// compensate for centripetal acceleration
 		float veloc;
 		veloc = _gps->ground_speed / 100;
-		accel.x -= _gps->acceleration();
-		accel.y -= _gyro_smoothed.z * veloc;
-		accel.z += _gyro_smoothed.y * veloc;
+		// be careful of the signs in this calculation. the
+		// quaternion system uses different signs than the
+		// rest of APM
+		accel.y -= (gyro.z - gyro_bias.z) * veloc;
+		accel.z += (gyro.y - gyro_bias.y) * veloc;
 	}
 
-	// Quaternion code uses opposite z accel
-	accel.z = -accel.z;
-
 	if (_compass == NULL) {
 		update_IMU(deltat, gyro, accel);
 	} else {
@@ -298,6 +301,9 @@ void AP_Quaternion::update(void)
 		update_MARG(deltat, gyro, accel, mag);
 	}
 
+	// keep the corrected gyro for reporting
+	_gyro_corrected = gyro;
+
 	// compute the Eulers
 	float test = (SEq_1*SEq_3 - SEq_4*SEq_2);
 	const float singularity = 0.499; // 86.3 degrees?
diff --git a/libraries/AP_Quaternion/AP_Quaternion.h b/libraries/AP_Quaternion/AP_Quaternion.h
index 4550a62dd2..543c37f579 100644
--- a/libraries/AP_Quaternion/AP_Quaternion.h
+++ b/libraries/AP_Quaternion/AP_Quaternion.h
@@ -59,8 +59,14 @@ public:
 	// compatibility methods with DCM
 	void update_DCM(void) { update(); }
 	void update_DCM_fast(void) { update(); }
-	Vector3f get_gyro(void) { return _gyro_smoothed; }
-	Vector3f get_integrator(void) { return gyro_bias; }
+	Vector3f get_gyro(void) {
+		// notice the sign reversals here
+		return Vector3f(-_gyro_corrected.x, -_gyro_corrected.y, _gyro_corrected.z);
+	}
+	Vector3f get_integrator(void) {
+		// notice the sign reversals here
+		return Vector3f(-gyro_bias.x, -gyro_bias.y, gyro_bias.z);
+        }
 	float get_accel_weight(void) { return 0; }
 	float get_renorm_val(void) { return 0; }
 	float get_health(void) { return 0; }
@@ -91,10 +97,11 @@ private:
 	// true if we are doing centripetal acceleration correction
 	bool		_centripetal;
 
-	// maximum gyroscope measurement error in rad/s (set to 5 degrees/second)
-	static const float gyroMeasError = 5.0 * (M_PI/180.0);
+	// maximum gyroscope measurement error in rad/s (set to 10 degrees/second)
+	static const float gyroMeasError = 10.0 * (M_PI/180.0);
 
-	// maximum gyroscope drift rate in radians/s/s (set to 0.02 degrees/s/s)
+	// maximum gyroscope drift rate in radians/s/s (set to 0.02
+	// degrees/s/s, which is 1.2 degrees/s/minute)
 	static const float gyroMeasDrift = 0.02 * (PI/180.0);
 
 	float beta;
@@ -109,8 +116,8 @@ private:
 	// estimate gyroscope biases error
 	Vector3f gyro_bias;
 
-	// smoothed gyro estimate
-	Vector3f _gyro_smoothed;
+	// the current corrected gyro vector
+	Vector3f _gyro_corrected;
 
 	// estimate of error
 	float		_error_rp_sum;