Fixed Throttle integrator bug. Time was not being reset causing a big drop in throttle control.

ArduCopter/ArduCopter.pde

@@ -341,7 +341,6 @@ static float cos_yaw_x 		= 1;
 static float sin_pitch_y, sin_yaw_y, sin_roll_y;
 static long initial_simple_bearing;				// used for Simple mode
 static float simple_sin_y, simple_cos_x;
-static float boost; 							// used to give a little extra to maintain altitude
 
 // Acro
 #if CH7_OPTION == CH7_FLIP
@@ -420,7 +419,7 @@ static long		nav_lat;							// for error calcs
 static long		nav_lon;							// for error calcs
 static int		nav_throttle;						// 0-1000 for throttle control
 
-static long 	throttle_integrator;				// used to integrate throttle output to predict battery life
+static unsigned long throttle_integrator;				// used to integrate throttle output to predict battery life
 static bool 	invalid_throttle;					// used to control when we calculate nav_throttle
 //static bool 	set_throttle_cruise_flag = false;	// used to track the throttle crouse value
 
@@ -693,10 +692,6 @@ static void medium_loop()
 			// ----------------------------------
 			invalid_throttle = true;
 
-			// calc boost
-			// ----------
-			boost = get_angle_boost();
-
 			break;
 
 		// This case deals with sending high rate telemetry
@@ -965,7 +960,7 @@ static void update_GPS(void)
 			// so that the altitude is more accurate
 			// -------------------------------------
 			if (current_loc.lat == 0) {
-                SendDebugln("!! bad loc");
+				//SendDebugln("!! bad loc");
 				ground_start_count = 5;
 
 			}else{
@@ -1096,7 +1091,7 @@ void update_throttle_mode(void)
 
 		case THROTTLE_MANUAL:
 			if (g.rc_3.control_in > 0){
-				g.rc_3.servo_out = g.rc_3.control_in + boost;
+				g.rc_3.servo_out = g.rc_3.control_in + get_angle_boost();
 			}else{
 				g.rc_3.servo_out = 0;
 			}
@@ -1116,13 +1111,14 @@ void update_throttle_mode(void)
 
 				// get the AP throttle
 				nav_throttle = get_nav_throttle(altitude_error, 200); //150 =  target speed of 1.5m/s
+				//Serial.printf("in:%d, cr:%d, NT:%d, I:%1.4f\n", g.rc_3.control_in,altitude_error,  nav_throttle, g.pi_throttle.get_integrator());
 
 				// clear the new data flag
 				invalid_throttle = false;
 			}
 
 			// apply throttle control at 200 hz
-			g.rc_3.servo_out = g.throttle_cruise + nav_throttle + boost;
+			g.rc_3.servo_out = g.throttle_cruise + nav_throttle + get_angle_boost();
 			break;
 	}
 }

ArduCopter/Attitude.pde

@@ -104,7 +104,7 @@ get_nav_throttle(long z_error, int target_speed)
 	float delta_throttle	= (float)(timer - throttle_timer)/1000.0;
 	throttle_timer    		= timer;
 
-	return g.pi_throttle.get_pi(rate_error, delta_throttle);
+	return (int)g.pi_throttle.get_pi(rate_error, delta_throttle);
 }
 
 static int
@@ -162,6 +162,9 @@ static void reset_nav(void)
 	nav_throttle 		= 0;
 	invalid_throttle 	= true;
 
+	// clear the throttle timer
+	throttle_timer = millis();
+
 	g.pi_nav_lat.reset_I();
 	g.pi_nav_lon.reset_I();
 
@@ -174,14 +177,6 @@ static void reset_nav(void)
 throttle control
 ****************************************************************/
 
-// user input:
-// -----------
-//static int get_throttle(int throttle_input)
-//{
-//	throttle_input = (float)throttle_input * angle_boost();
-//	return  max(throttle_input, 0);
-//}
-
 static long
 get_nav_yaw_offset(int yaw_input, int reset)
 {
@@ -217,10 +212,10 @@ static int alt_hold_velocity()
 }
 */
 
-static float get_angle_boost()
+static int get_angle_boost()
 {
 	float temp = cos_pitch_x * cos_roll_x;
 	temp = 2.0 - constrain(temp, .5, 1.0);
-	return temp;
+	return (int)(temp * 50.0);
 }
 

ArduCopter/config.h

@@ -393,7 +393,7 @@
 #endif
 
 #ifndef RTL_THR
-# define RTL_THR			THROTTLE_AUTO
+# define RTL_THR			THROTTLE_HOLD
 #endif
 
 
@@ -513,7 +513,7 @@
 # define THROTTLE_P		0.6			//
 #endif
 #ifndef THROTTLE_I
-# define THROTTLE_I		0.10		// with 4m error, 12.5s windup
+# define THROTTLE_I		0.03		// with 4m error, 12.5s windup
 #endif
 #ifndef THROTTLE_IMAX
 # define THROTTLE_IMAX		300
@@ -670,7 +670,7 @@
 #endif
 
 #ifndef ALT_HOLD_HOME
-# define ALT_HOLD_HOME 8
+# define ALT_HOLD_HOME -1
 #endif
 
 #ifndef USE_CURRENT_ALT