From 51d918dda7d378e0e23b3c1e7ee5a1c2a3bb721c Mon Sep 17 00:00:00 2001
From: jasonshort <jasonshort@f9c3cf11-9bcb-44bc-f272-b75c42450872>
Date: Sat, 30 Apr 2011 16:01:57 +0000
Subject: [PATCH] Added throttle_integrator Removed cos for nav_rate testing.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1934 f9c3cf11-9bcb-44bc-f272-b75c42450872
---
 ArduCopterMega/ArduCopterMega.pde | 5 +++++
 ArduCopterMega/Log.pde            | 8 +++++++-
 ArduCopterMega/navigation.pde     | 9 ++++-----
 3 files changed, 16 insertions(+), 6 deletions(-)

diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde
index 2be7194aec..b21a0a7f07 100644
--- a/ArduCopterMega/ArduCopterMega.pde
+++ b/ArduCopterMega/ArduCopterMega.pde
@@ -342,6 +342,7 @@ long	nav_lon;							// for error calcs
 int		nav_throttle;						// 0-1000 for throttle control
 int		nav_throttle_old;					// for filtering
 
+long 	throttle_integrator;					// used to control when we calculate nav_throttle
 bool 	invalid_throttle;					// used to control when we calculate nav_throttle
 bool 	invalid_nav;						// used to control when we calculate nav_throttle
 bool 	set_throttle_cruise_flag = false;	// used to track the throttle crouse value
@@ -521,6 +522,10 @@ void fast_loop()
 	// ------------------------------
 	set_servos_4();
 
+	// record throttle output
+	// ------------------------------
+	throttle_integrator += g.rc_3.servo_out;
+
 #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK
 	// HIL for a copter needs very fast update of the servo values
 	gcs.send_message(MSG_RADIO_OUT);
diff --git a/ArduCopterMega/Log.pde b/ArduCopterMega/Log.pde
index 84534edd6f..44a003aaea 100644
--- a/ArduCopterMega/Log.pde
+++ b/ArduCopterMega/Log.pde
@@ -496,18 +496,24 @@ void Log_Write_Current()
 	DataFlash.WriteByte(HEAD_BYTE1);
 	DataFlash.WriteByte(HEAD_BYTE2);
 	DataFlash.WriteByte(LOG_CURRENT_MSG);
+
 	DataFlash.WriteInt(g.rc_3.control_in);
+	DataFlash.WriteLong(throttle_integrator);
+
 	DataFlash.WriteInt((int)(current_voltage 	* 100.0));
 	DataFlash.WriteInt((int)(current_amps 		* 100.0));
 	DataFlash.WriteInt((int)current_total);
+
 	DataFlash.WriteByte(END_BYTE);
 }
 
 // Read a Current packet
 void Log_Read_Current()
 {
-	Serial.printf_P(PSTR("CURR: %d, %4.4f, %4.4f, %d\n"),
+	Serial.printf_P(PSTR("CURR: %d, %ld, %4.4f, %4.4f, %d\n"),
 			DataFlash.ReadInt(),
+			DataFlash.ReadLong(),
+
 			((float)DataFlash.ReadInt() / 100.f),
 			((float)DataFlash.ReadInt() / 100.f),
 			DataFlash.ReadInt());
diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde
index 0173cc5199..ebc0291f33 100644
--- a/ArduCopterMega/navigation.pde
+++ b/ArduCopterMega/navigation.pde
@@ -34,7 +34,6 @@ void navigate()
 	// nav_bearing will includes xtrac correction
 	// ------------------------------------------
 	nav_bearing = target_bearing;
-
 }
 
 bool check_missed_wp()
@@ -122,6 +121,7 @@ void calc_nav_output()
 	nav_roll 	=  (float)nav_lat * cos_nav_x;
 	nav_pitch 	= -(float)nav_lat * sin_nav_y;
 }
+
 #define WAYPOINT_SPEED 450
 
 #if NAV_TEST == 0
@@ -144,7 +144,6 @@ void calc_rate_nav()
 
 	// dampen our response
 	nav_lat -= constrain(dampening, 	-1800, 1800); // +- 20m max error
-
 }
 
 #else
@@ -161,7 +160,7 @@ void calc_rate_nav()
 	target_error 		= wrap_180(target_error);
 
 	// calc the cos of the error to tell how fast we are moving towards the target
-	error = (float)error * cos(radians((float)target_error/100));
+	//error = (float)error * cos(radians((float)target_error/100));
 
 	// Scale response by kP
 	long nav_lat 	= g.pid_nav_wp.kP() * error;
@@ -171,10 +170,10 @@ void calc_rate_nav()
 	last_ground_speed = g_gps->ground_speed;
 
 	// dampen our response
-	nav_lat -= constrain(dampening, 	-1800, 1800); // +- 20m max error
+	nav_lat -= constrain(dampening, 	-1800, 1800); // +- max error
 
 	// limit our output
-	nav_lat	= constrain(nav_lat, 	-3800, 3800); // +- 20m max error
+	nav_lat	= constrain(nav_lat, 	-2500, 2500); // +- max error
 }
 
 #endif