From 78095b1dd7c100c3c9d9b0b511ac8762098f7c69 Mon Sep 17 00:00:00 2001
From: jasonshort <jasonshort@f9c3cf11-9bcb-44bc-f272-b75c42450872>
Date: Thu, 13 Jan 2011 01:31:05 +0000
Subject: [PATCH] Implemented new DCM based yaw independent heading controller.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1495 f9c3cf11-9bcb-44bc-f272-b75c42450872
---
 ArduCopterMega/navigation.pde | 23 ++++++++++++++++-------
 ArduCopterMega/test.pde       | 22 +++++++++++++++++++---
 2 files changed, 35 insertions(+), 10 deletions(-)

diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde
index ad5e2a8b8d..62797f3349 100644
--- a/ArduCopterMega/navigation.pde
+++ b/ArduCopterMega/navigation.pde
@@ -46,30 +46,39 @@ void navigate()
 
 }
 
+#define DIST_ERROR_MAX 3000
 void calc_nav()
 {
 	/*
 	Becuase we are using lat and lon to do our distance errors here's a quick chart:
 	100 	= 1m
 	1000 	= 11m
+	3000 	= 33m
 	10000 	= 111m
 	pitch_max = 22° (2200)
 	*/
 	
-	float cos_yaw = cos(dcm.yaw);
-	float sin_yaw = sin(dcm.yaw);
-		
+	Vector2f yawvector;
+	Matrix3f temp = dcm.get_dcm_matrix();
+
+	yawvector.x 	= temp.a.x;
+	yawvector.y 	= temp.b.x;
+	yawvector.normalize();
+	
 	// ROLL
 	nav_lon = pid_nav_lon.get_pid((long)((float)(next_WP.lng - GPS.longitude) * scaleLongDown), dTnav, 1.0);
-	nav_lon = constrain(nav_lon, -pitch_max, pitch_max); // Limit max command
+	nav_lon = constrain(nav_lon, -DIST_ERROR_MAX, DIST_ERROR_MAX); // Limit max command
 
 	// PITCH
 	nav_lat = pid_nav_lat.get_pid(next_WP.lat - GPS.latitude, dTnav, 1.0);
-	nav_lat = constrain(nav_lat, -pitch_max, pitch_max); // Limit max command
+	nav_lat = constrain(nav_lat, -DIST_ERROR_MAX, DIST_ERROR_MAX); // Limit max command
 
 	// rotate the vector
-	nav_roll 	= (float)nav_lon * cos_yaw - (float)nav_lat * sin_yaw;
-	nav_pitch 	= (float)nav_lon * sin_yaw + (float)nav_lat * cos_yaw;
+	nav_roll 	= (float)nav_lon * yawvector.x - (float)nav_lat * yawvector.y;
+	nav_pitch 	= (float)nav_lon * yawvector.y + (float)nav_lat * yawvector.x;
+
+	nav_roll 	= constrain(nav_roll,  -pitch_max, pitch_max);
+	nav_pitch 	= constrain(nav_pitch, -pitch_max, pitch_max);
 }
 
 /*
diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde
index 1295ee7862..0b1ad4f5fa 100644
--- a/ArduCopterMega/test.pde
+++ b/ArduCopterMega/test.pde
@@ -207,7 +207,7 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
 			ts_num++;
 			if (ts_num > 10){
 				ts_num = 0;
-				Serial.printf_P(PSTR("r: %d, p:%d, rc1:%d, rc2:%d, rc4 %d, ny:%ld, ys:%ld, ye:%ld,  R: %d,  L: %d  F: %d  B: %d\n"),
+				/*Serial.printf_P(PSTR("r: %d, p:%d, rc1:%d, rc2:%d, rc4 %d, ny:%ld, ys:%ld, ye:%ld,  R: %d,  L: %d  F: %d  B: %d\n"),
 					(int)(roll_sensor/100),
 					(int)(pitch_sensor/100),
 					rc_1.pwm_out,
@@ -219,6 +219,16 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
 					motor_out[RIGHT],
 					motor_out[LEFT],
 					motor_out[FRONT],
+					motor_out[BACK]);*/
+
+				Serial.printf_P(PSTR("r: %d, p:%d, rc1:%d, Int%4.4f,  R: %d,  L: %d  F: %d  B: %d\n"),
+					(int)(roll_sensor/100),
+					(int)(pitch_sensor/100),
+					rc_1.pwm_out,
+					pid_stabilize_roll.get_integrator(),
+					motor_out[RIGHT],
+					motor_out[LEFT],
+					motor_out[FRONT],
 					motor_out[BACK]);
 			}
 
@@ -390,9 +400,15 @@ test_imu(uint8_t argc, const Menu::arg *argv)
 			
 			// We are using the IMU
 			// ---------------------
-			Serial.printf_P(PSTR("A: %d,%d,%d\tG: %d,%d,%d\t"), (int)(accels.x*100), (int)(accels.y*100), (int)(accels.z*100),(int)(gyros.x*100), (int)(gyros.y*100), (int)(gyros.z*100));
+			Serial.printf_P(PSTR("A: %4.4f, %4.4f, %4.4f\t"
+								 "G: %4.4f, %4.4f, %4.4f\t"),
+								accels.x, accels.y, accels.z,
+								gyros.x,  gyros.y,  gyros.z);
 
-			Serial.printf_P(PSTR("r: %d\tp: %d\t y: %d\n"), ((int)roll_sensor/100), ((int)pitch_sensor/100), ((uint16_t)yaw_sensor/100));
+			Serial.printf_P(PSTR("r: %ld\tp: %ld\t y: %ld\n"),
+								roll_sensor,
+								pitch_sensor,
+								yaw_sensor);
 		}
 		
 		if(Serial.available() > 0){