From 0475dbf8ae01bab4d2f6617813b9e986abc5aa21 Mon Sep 17 00:00:00 2001
From: Jason Short <jasonshort@mac.com>
Date: Fri, 20 Jan 2012 10:11:18 -0800
Subject: [PATCH] relocated reset_I term functions to central location

---
 ArduCopter/control_modes.pde |  3 +--
 ArduCopter/navigation.pde    | 10 ++++----
 ArduCopter/system.pde        | 45 ++++++++++--------------------------
 3 files changed, 18 insertions(+), 40 deletions(-)

diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde
index 76f5c7f7a4..7beab99fbe 100644
--- a/ArduCopter/control_modes.pde
+++ b/ArduCopter/control_modes.pde
@@ -212,8 +212,7 @@ A_off: 201.95, -24.00, -88.56
 
 static void trim_accel()
 {
-	g.pi_stabilize_roll.reset_I();
-	g.pi_stabilize_pitch.reset_I();
+	reset_stability_I();
 
 	float trim_roll  = (float)g.rc_1.control_in / 30000.0;
 	float trim_pitch = (float)g.rc_2.control_in / 30000.0;
diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde
index 76760a0619..32140818aa 100644
--- a/ArduCopter/navigation.pde
+++ b/ArduCopter/navigation.pde
@@ -143,7 +143,7 @@ static void calc_location_error(struct Location *next_loc)
 */
 
 #define NAV_ERR_MAX 800
-static void calc_loiter(int x_error, int y_error)
+static void calc_loiter1(int x_error, int y_error)
 {
 	int16_t lon_PI 	= g.pi_loiter_lon.get_pi(x_error, dTnav);
 	int16_t lon_D 	= 3 * x_actual_speed ; // this controls the small bumps
@@ -160,7 +160,7 @@ static void calc_loiter(int x_error, int y_error)
 }
 
 
-static void calc_loiter1(int x_error, int y_error)
+static void calc_loiter(int x_error, int y_error)
 {
 	// East/West
 	x_error 				= constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX);	//800
@@ -389,15 +389,15 @@ static void set_new_altitude(int32_t _new_alt)
 	if(target_altitude > original_altitude){
 		// we are below, going up
 		alt_change_flag = ASCENDING;
-		Serial.printf("go up\n");
+		//Serial.printf("go up\n");
 	}else if(target_altitude < original_altitude){
 		// we are above, going down
 		alt_change_flag = DESCENDING;
-		Serial.printf("go down\n");
+		//Serial.printf("go down\n");
 	}else{
 		// No Change
 		alt_change_flag = REACHED_ALT;
-		Serial.printf("reached alt\n");
+		//Serial.printf("reached alt\n");
 	}
 
 	//Serial.printf("new alt: %d Org alt: %d\n", target_altitude, original_altitude);
diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde
index d783c5b5d7..bc599db619 100644
--- a/ArduCopter/system.pde
+++ b/ArduCopter/system.pde
@@ -361,29 +361,6 @@ static void init_ardupilot()
 	SendDebug("\nReady to FLY ");
 }
 
-/*
-  reset all I integrators
- */
-static void reset_I_all(void)
-{
-	g.pi_rate_roll.reset_I();
-	g.pi_rate_pitch.reset_I();
-	g.pi_rate_yaw.reset_I();
-	g.pi_stabilize_roll.reset_I();
-	g.pi_stabilize_pitch.reset_I();
-	g.pi_stabilize_yaw.reset_I();
-	g.pi_loiter_lat.reset_I();
-	g.pi_loiter_lon.reset_I();
-	g.pi_nav_lat.reset_I();
-	g.pi_nav_lon.reset_I();
-	g.pi_alt_hold.reset_I();
-	g.pi_throttle.reset_I();
-	g.pi_acro_roll.reset_I();
-	g.pi_acro_pitch.reset_I();
-	g.pi_optflow_roll.reset_I();
-	g.pi_optflow_pitch.reset_I();
-}
-
 
 //********************************************************************************
 //This function does all the calibrations, etc. that we need during a ground start
@@ -405,8 +382,8 @@ static void startup_ground(void)
 	// ---------------------------
 	clear_leds();
 
-    // when we re-calibrate the gyros, all previous I values now
-    // make no sense
+    // when we re-calibrate the gyros,
+    // all previous I values are invalid
     reset_I_all();
 }
 
@@ -473,7 +450,6 @@ static void set_mode(byte mode)
 			yaw_mode 		= YAW_ACRO;
 			roll_pitch_mode = ROLL_PITCH_ACRO;
 			throttle_mode 	= THROTTLE_MANUAL;
-			reset_rate_I();
 			break;
 
 		case STABILIZE:
@@ -561,14 +537,19 @@ static void set_mode(byte mode)
 		update_throttle_cruise();
 	}else {
 		// an automatic throttle
-		// todo: replace with a throttle cruise estimator
 		init_throttle_cruise();
 	}
 
 	if(roll_pitch_mode <= ROLL_PITCH_ACRO){
 		// We are under manual attitude control
-		// removes the navigation from roll and pitch commands, but leaves the wind compensation
-		reset_nav();
+		// remove the navigation from roll and pitch command
+		reset_nav_params();
+		// remove the wind compenstaion
+		reset_wind_I();
+		// Clears the WP navigation speed compensation
+		reset_nav_I();
+		// Clears the alt hold compensation
+		reset_throttle_I();
 	}
 
 	Log_Write_Mode(control_mode);
@@ -608,8 +589,7 @@ 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();
+		reset_throttle_I();
 	}
 }
 
@@ -619,8 +599,7 @@ init_throttle_cruise()
 #if AUTO_THROTTLE_HOLD == 0
 	// are we moving from manual throttle to auto_throttle?
 	if((old_control_mode <= STABILIZE) && (g.rc_3.control_in > MINIMUM_THROTTLE)){
-		g.pi_throttle.reset_I();
-		g.pi_alt_hold.reset_I();
+		reset_throttle_I();
 		g.throttle_cruise.set_and_save(g.rc_3.control_in);
 	}
 #endif