From 48cad8bc25970963bc4292ff29743791582d31fd Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Fri, 24 Feb 2012 11:23:49 +1100
Subject: [PATCH] DCM: use rotation_matrix_from_euler() in matrix reset

this makes the code a bit easier to read
---
 libraries/AP_DCM/AP_DCM.cpp | 28 ++--------------------------
 1 file changed, 2 insertions(+), 26 deletions(-)

diff --git a/libraries/AP_DCM/AP_DCM.cpp b/libraries/AP_DCM/AP_DCM.cpp
index 443b23365e..9fd4f06c78 100644
--- a/libraries/AP_DCM/AP_DCM.cpp
+++ b/libraries/AP_DCM/AP_DCM.cpp
@@ -215,34 +215,10 @@ AP_DCM::matrix_reset(bool recover_eulers)
 	// attitude then calculate the dcm matrix from the current
 	// roll/pitch/yaw values
 	if (recover_eulers && !isnan(roll) && !isnan(pitch) && !isnan(yaw)) {
-		float cp = cos(pitch);
-		float sp = sin(pitch);
-		float sr = sin(roll);
-		float cr = cos(roll);
-		float sy = sin(yaw);
-		float cy = cos(yaw);
-		//Serial.printf("setting DCM matrix to %f %f %f\n", ToDeg(roll), ToDeg(pitch), ToDeg(yaw));
-		_dcm_matrix.a.x = cp * cy;
-		_dcm_matrix.a.y = (sr * sp * cy) - (cr * sy);
-		_dcm_matrix.a.z = (cr * sp * cy) + (sr * sy);
-		_dcm_matrix.b.x = cp * sy;
-		_dcm_matrix.b.y = (sr * sp * sy) + (cr * cy);
-		_dcm_matrix.b.z = (cr * sp * sy) - (sr * cy);
-		_dcm_matrix.c.x = -sp;
-		_dcm_matrix.c.y = sr * cp;
-		_dcm_matrix.c.z = cr * cp;
+		rotation_matrix_from_euler(_dcm_matrix, roll, pitch, yaw);
 	} else {
 		// otherwise make it flat
-		//Serial.printf("zeroing DCM matrix\n");
-		_dcm_matrix.a.x = 1.0f;
-		_dcm_matrix.a.y = 0.0f;
-		_dcm_matrix.a.z = 0.0f;
-		_dcm_matrix.b.x = 0.0f;
-		_dcm_matrix.b.y = 1.0f;
-		_dcm_matrix.b.z = 0.0f;
-		_dcm_matrix.c.x = 0.0f;
-		_dcm_matrix.c.y = 0.0f;
-		_dcm_matrix.c.z = 1.0f;
+		rotation_matrix_from_euler(_dcm_matrix, 0, 0, 0);
 	}
 
 	if (_compass != NULL) {