From a85ba80246f2b01597aba2cfccbc9397fb53a0c5 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sat, 3 Mar 2012 18:32:50 +1100
Subject: [PATCH] AP_Quaternion: added DCM compatibility interfaces

useful for getting the code going quickly
---
 libraries/AP_Quaternion/AP_Quaternion.cpp | 25 +++++++++++++++++++++--
 libraries/AP_Quaternion/AP_Quaternion.h   | 18 ++++++++++++++++
 2 files changed, 41 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_Quaternion/AP_Quaternion.cpp b/libraries/AP_Quaternion/AP_Quaternion.cpp
index aedf1cd78b..7a809b3f39 100644
--- a/libraries/AP_Quaternion/AP_Quaternion.cpp
+++ b/libraries/AP_Quaternion/AP_Quaternion.cpp
@@ -249,19 +249,36 @@ void AP_Quaternion::update(void)
 
 	_imu->update();
 
-	delta_t = _imu->get_delta_time();
+	deltat = _imu->get_delta_time();
 
 	// get current IMU state
 	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
 	gyro.x = -gyro.x;
 	gyro.y = -gyro.y;
 
-	accel = _imu->get_accel();
+
+	if (_centripetal && _gps && _gps->status() == GPS::GPS_OK) {
+		// compensate for centripetal acceleration
+		float veloc;
+		veloc = _gps->ground_speed / 100;
+		accel.y -= _gyro_smoothed.z * veloc;
+		accel.z += _gyro_smoothed.y * veloc;
+	}
+
+	// Quaternion code uses opposite z accel
 	accel.z = -accel.z;
 
 	if (_compass == NULL) {
 		update_IMU(deltat, gyro, accel);
 	} else {
+		// mag.z is reversed in quaternion code
 		Vector3f mag = Vector3f(_compass->mag_x, _compass->mag_y, - _compass->mag_z);
 		update_MARG(deltat, gyro, accel, mag);
 	}
@@ -289,6 +306,10 @@ void AP_Quaternion::update(void)
 			    1 - 2.0*(SEq_3*SEq_3 + SEq_4*SEq_4));
 	}
 
+	if (_compass != NULL) {
+		yaw += _compass->get_declination();
+	}
+
 	// and integer Eulers
 	roll_sensor  = 100 * ToDeg(roll);
 	pitch_sensor = 100 * ToDeg(pitch);
diff --git a/libraries/AP_Quaternion/AP_Quaternion.h b/libraries/AP_Quaternion/AP_Quaternion.h
index 41c76904c3..7d0f11d783 100644
--- a/libraries/AP_Quaternion/AP_Quaternion.h
+++ b/libraries/AP_Quaternion/AP_Quaternion.h
@@ -56,6 +56,21 @@ public:
 	int32_t		yaw_sensor;
 
 
+	// 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 Vector3f(0,0,0); }
+	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;
+
 private:
 	void 		update_IMU(float deltat, Vector3f &gyro, Vector3f &accel);
 	void 		update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag);
@@ -93,6 +108,9 @@ private:
 
 	// estimate gyroscope biases error
 	Vector3f gyro_bias;
+
+	// smoothed gyro estimate
+	Vector3f _gyro_smoothed;
 };
 
 #endif