From 2f5a4cdc4a36973f2185917d144d11e9354b86ab Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sat, 3 Mar 2012 23:32:31 +1100
Subject: [PATCH] Quaternion: added in reporting of gyro drift and rp/yaw
 errors

---
 libraries/AP_Quaternion/AP_Quaternion.cpp | 32 +++++++++++++++++++++++
 libraries/AP_Quaternion/AP_Quaternion.h   | 12 ++++++---
 2 files changed, 41 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_Quaternion/AP_Quaternion.cpp b/libraries/AP_Quaternion/AP_Quaternion.cpp
index 1e7f2ffcdb..cb6cf426c5 100644
--- a/libraries/AP_Quaternion/AP_Quaternion.cpp
+++ b/libraries/AP_Quaternion/AP_Quaternion.cpp
@@ -199,6 +199,12 @@ void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, V
     w_err.y = twoSEq_1 * SEqHatDot_3 + twoSEq_2 * SEqHatDot_4 - twoSEq_3 * SEqHatDot_1 - twoSEq_4 * SEqHatDot_2;
     w_err.z = twoSEq_1 * SEqHatDot_4 - twoSEq_2 * SEqHatDot_3 + twoSEq_3 * SEqHatDot_2 - twoSEq_4 * SEqHatDot_1;
 
+    // keep track of the error rates
+    _error_rp_sum += 0.5*(fabs(w_err.x) + fabs(w_err.y));
+    _error_yaw_sum += fabs(w_err.z);
+    _error_rp_count++;
+    _error_yaw_count++;
+
     // compute and remove the gyroscope baises
     gyro_bias += w_err * (deltat * zeta);
     gyro -= gyro_bias;
@@ -319,3 +325,29 @@ void AP_Quaternion::update(void)
 		yaw_sensor += 36000;
 	}
 }
+
+// average error in roll/pitch since last call
+float AP_Quaternion::get_error_rp(void)
+{
+	float ret;
+	if (_error_rp_count == 0) {
+		return 0;
+	}
+	ret = _error_rp_sum / _error_rp_count;
+	_error_rp_sum = 0;
+	_error_rp_count = 0;
+	return ret;
+}
+
+// average error in yaw since last call
+float AP_Quaternion::get_error_yaw(void)
+{
+	float ret;
+	if (_error_yaw_count == 0) {
+		return 0;
+	}
+	ret = _error_yaw_sum / _error_yaw_count;
+	_error_yaw_sum = 0;
+	_error_yaw_count = 0;
+	return ret;
+}
diff --git a/libraries/AP_Quaternion/AP_Quaternion.h b/libraries/AP_Quaternion/AP_Quaternion.h
index 359fe088cd..4550a62dd2 100644
--- a/libraries/AP_Quaternion/AP_Quaternion.h
+++ b/libraries/AP_Quaternion/AP_Quaternion.h
@@ -60,16 +60,16 @@ public:
 	void update_DCM(void) { update(); }
 	void update_DCM_fast(void) { update(); }
 	Vector3f get_gyro(void) { return _gyro_smoothed; }
-	Vector3f get_integrator(void) { return Vector3f(0,0,0); }
+	Vector3f get_integrator(void) { return gyro_bias; }
 	float get_accel_weight(void) { return 0; }
 	float get_renorm_val(void) { return 0; }
-	float get_error_rp(void) { return 0; }
-	float get_error_yaw(void) { return 0; }
 	float get_health(void) { return 0; }
 	void matrix_reset(void) { }
 	uint8_t gyro_sat_count;
 	uint8_t renorm_range_count;
 	uint8_t renorm_blowup_count;
+	float get_error_rp(void);
+	float get_error_yaw(void);
 
 private:
 	void 		update_IMU(float deltat, Vector3f &gyro, Vector3f &accel);
@@ -111,6 +111,12 @@ private:
 
 	// smoothed gyro estimate
 	Vector3f _gyro_smoothed;
+
+	// estimate of error
+	float		_error_rp_sum;
+	uint16_t	_error_rp_count;
+	float		_error_yaw_sum;
+	uint16_t	_error_yaw_count;
 };
 
 #endif