From d5b619218cbcd599e955f64c75b65013881acf6b Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Fri, 24 Feb 2012 11:30:09 +1100
Subject: [PATCH] DCM: use rotation_matrix_from_euler() to calculate initial
 yaw

When we first get a compass reading or we first start motion we need
to setup the DCM matrix with the right yaw. This uses
rotation_matrix_from_euler() to get a DCM matrix corresponding to our
current roll/pitch, but with the correct yaw
---
 libraries/AP_DCM/AP_DCM.cpp | 47 ++++++++++++++++++-------------------
 1 file changed, 23 insertions(+), 24 deletions(-)

diff --git a/libraries/AP_DCM/AP_DCM.cpp b/libraries/AP_DCM/AP_DCM.cpp
index 9fd4f06c78..ee5c87552b 100644
--- a/libraries/AP_DCM/AP_DCM.cpp
+++ b/libraries/AP_DCM/AP_DCM.cpp
@@ -361,7 +361,6 @@ AP_DCM::drift_correction(void)
 	//static float scaled_omega_P[3];
 	//static float scaled_omega_I[3];
 	static bool in_motion = false;
-	Matrix3f rot_mat;
 
 	//*****Roll and Pitch***************
 
@@ -391,10 +390,25 @@ AP_DCM::drift_correction(void)
 	//*****YAW***************
 
 	if (_compass && _compass->healthy) {
-		// We make the gyro YAW drift correction based on compass magnetic heading
-		error_course = (_dcm_matrix.a.x * _compass->heading_y) - (_dcm_matrix.b.x * _compass->heading_x);	// Equation 23, Calculating YAW error
+		if (in_motion) {
+			// Equation 23, Calculating YAW error
+			// We make the gyro YAW drift correction based
+			// on compass magnetic heading
+			error_course = (_dcm_matrix.a.x * _compass->heading_y) - (_dcm_matrix.b.x * _compass->heading_x);
+		} else {
+			// this is our first estimate of the yaw,
+			// construct a DCM matrix based on the current
+			// roll/pitch and the compass heading, but
 
-	} else if (_gps) {
+			// first ensure the compass heading has been
+			// calculated
+			_compass->calculate(_dcm_matrix);
+
+			// now construct a new DCM matrix
+			rotation_matrix_from_euler(_dcm_matrix, roll, pitch, _compass->heading);
+			in_motion =  true;
+		}
+	} else if (_gps && _gps->status() == GPS::GPS_OK) {
 
 		// Use GPS Ground course to correct yaw gyro drift
 		if (_gps->ground_speed >= SPEEDFILT) {
@@ -404,26 +418,11 @@ AP_DCM::drift_correction(void)
 			if(in_motion) {
 				error_course = (_dcm_matrix.a.x * _course_over_ground_y) - (_dcm_matrix.b.x * _course_over_ground_x);	// Equation 23, Calculating YAW error
 			} else  {
-				float cos_psi_err, sin_psi_err;
-				// This is the case for when we first start moving and reset the DCM so that yaw matches the gps ground course
-				// This is just to get a reasonable estimate faster
-				yaw = atan2(_dcm_matrix.b.x, _dcm_matrix.a.x);
-				cos_psi_err = cos(ToRad(_gps->ground_course/100.0) - yaw);
-				sin_psi_err = sin(ToRad(_gps->ground_course/100.0) - yaw);
-				// Rxx = cos psi err, Rxy = - sin psi err, Rxz = 0
-				// Ryx = sin psi err, Ryy = cos psi err,   Ryz = 0
-				// Rzx = Rzy = 0, Rzz = 1
-				rot_mat.a.x = cos_psi_err;
-				rot_mat.a.y = -sin_psi_err;
-				rot_mat.b.x = sin_psi_err;
-				rot_mat.b.y = cos_psi_err;
-				rot_mat.a.z = 0;
-				rot_mat.b.z = 0;
-				rot_mat.c.x = 0;
-				rot_mat.c.y = 0;
-				rot_mat.c.z = 1.0;
-
-				_dcm_matrix = rot_mat * _dcm_matrix;
+				// when we first start moving, set the
+				// DCM matrix to the current
+				// roll/pitch values, but with yaw
+				// from the GPS
+				rotation_matrix_from_euler(_dcm_matrix, roll, pitch, ToRad(_gps->ground_course));
 				in_motion =  true;
 				error_course = 0;
 			}