From 9caa4aeb44d85ab6ce42566e90ebddebd8ae046b Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 23 Feb 2012 08:00:25 +1100
Subject: [PATCH] DCM: range check the matrix before calculating pitch

The asin() in the pitch calculation can only take values between -1
and 1. This change ensures that the value is in range, and if it isn't
then we force a normalization. If that fails we reset the matrix
---
 libraries/AP_DCM/AP_DCM.cpp | 30 ++++++++++++++++++++++++++++++
 libraries/AP_DCM/AP_DCM.h   |  1 +
 2 files changed, 31 insertions(+)

diff --git a/libraries/AP_DCM/AP_DCM.cpp b/libraries/AP_DCM/AP_DCM.cpp
index 4a1aae5360..e6b2e87f60 100644
--- a/libraries/AP_DCM/AP_DCM.cpp
+++ b/libraries/AP_DCM/AP_DCM.cpp
@@ -225,6 +225,33 @@ AP_DCM::matrix_reset(void)
 	}
 }
 
+/*
+  check the DCM matrix for pathological values
+ */
+void
+AP_DCM::check_matrix(void)
+{
+	// some DCM matrix values can lead to an out of range error in
+	// the pitch calculation via asin().  These NaN values can
+	// feed back into the rest of the DCM matrix via the
+	// error_course value.
+	if (_dcm_matrix.c.x > 1.0 ||
+	    _dcm_matrix.c.x < -1.0) {
+		// We have an invalid matrix. Force a normalisation.
+		renorm_range_count++;
+		normalize();
+		if (_dcm_matrix.c.x > 1.0 ||
+		    _dcm_matrix.c.x < -1.0) {
+			// normalisation didn't fix the problem! We're
+			// in real trouble. All we can do is reset
+			SITL_debug("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n",
+				   _dcm_matrix.c.x);
+			renorm_blowup_count++;
+			matrix_reset();
+		}
+	}
+}
+
 /*************************************************
 Direction Cosine Matrix IMU: Theory
 William Premerlani and Paul Bizard
@@ -410,6 +437,8 @@ AP_DCM::drift_correction(void)
 void
 AP_DCM::euler_angles(void)
 {
+	check_matrix();
+
 	#if (OUTPUTMODE == 2)				 // Only accelerometer info (debugging purposes)
 	roll 		= atan2(_accel_vector.y, -_accel_vector.z);		// atan2(acc_y, acc_z)
 	pitch 		= asin((_accel_vector.x) / (double)9.81); // asin(acc_x)
@@ -431,6 +460,7 @@ AP_DCM::euler_angles(void)
 void
 AP_DCM::euler_rp(void)
 {
+	check_matrix();
 	pitch 			= -asin(_dcm_matrix.c.x);
 	roll 			= atan2(_dcm_matrix.c.y, _dcm_matrix.c.z);
 	roll_sensor 	= roll * DEGX100;	//degrees(roll)  * 100;
diff --git a/libraries/AP_DCM/AP_DCM.h b/libraries/AP_DCM/AP_DCM.h
index 1523c80ea9..7ce6a5fd7b 100644
--- a/libraries/AP_DCM/AP_DCM.h
+++ b/libraries/AP_DCM/AP_DCM.h
@@ -101,6 +101,7 @@ private:
 	float 		read_adc(int select);
 	void 		matrix_update(float _G_Dt);
 	void 		normalize(void);
+	void		check_matrix(void);
 	Vector3f 	renorm(Vector3f const &a, int &problem);
 	void 		drift_correction(void);
 	void 		euler_angles(void);