From 7877a872d357c5d7b3d80a9796133da029da0a64 Mon Sep 17 00:00:00 2001
From: Jason Short <jasonshort@mac.com>
Date: Fri, 13 Jan 2012 16:48:05 -0800
Subject: [PATCH] added update throttle cruise function

---
 ArduCopter/system.pde | 14 +++++++++++---
 1 file changed, 11 insertions(+), 3 deletions(-)

diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde
index e1f3537781..f5ffa7241a 100644
--- a/ArduCopter/system.pde
+++ b/ArduCopter/system.pde
@@ -558,9 +558,7 @@ static void set_mode(byte mode)
 
 	if(throttle_mode == THROTTLE_MANUAL){
 		// reset all of the throttle iterms
-		g.pi_alt_hold.reset_I();
-		g.pi_throttle.reset_I();
-
+		update_throttle_cruise();
 	}else {
 		// an automatic throttle
 		// todo: replace with a throttle cruise estimator
@@ -605,6 +603,16 @@ init_simple_bearing()
 	initial_simple_bearing = dcm.yaw_sensor;
 }
 
+static void update_throttle_cruise()
+{
+	int16_t tmp = g.pi_alt_hold.get_integrator();
+	if(tmp != 0){
+		g.throttle_cruise += tmp;
+		g.pi_alt_hold.reset_I();
+		g.pi_throttle.reset_I();
+	}
+}
+
 static void
 init_throttle_cruise()
 {