ACM -

Implemented automatic ranging of Alt Hold gains. Works well in simulator and testing.
- alt hold estimation moved to 50 hz
- simple fixed observer calc for smooth and accurate climb rates useful for derivative calcs
- auto-reset of the I term by moving I value into throttle value. This recalcs the gain every 20seconds for battery drainage compensation in long flights.
- remove filtering for Nav_throttle
- added a way to lower the gain on nav_throttle for descents by / climb_rate error by 2 - seems to work OK and keeps copter from dropping like a rock when the Baro drifts quickly lower.
- removed old throttle hold set point code
- made throttle override for alt hold +- 200 vs 250

ArduCopter/Attitude.pde

@@ -186,7 +186,7 @@ get_rate_yaw(int32_t target_rate)
 static int16_t
 get_nav_throttle(int32_t z_error)
 {
-	static int16_t old_output = 0;
+	//static int16_t old_output = 0;
 	int16_t rate_error = 0;
 	int16_t output = 0;
 
@@ -198,19 +198,23 @@ get_nav_throttle(int32_t z_error)
 	z_error 		= constrain(z_error, -400, 400);
 
 	// compensates throttle setpoint error for hovering
-	int16_t iterm = g.pi_alt_hold.get_i(z_error, .1);
+	int16_t iterm = g.pi_alt_hold.get_i(z_error, .02);
 
 	// calculate rate error
 	rate_error 		= rate_error - climb_rate;
 
+	// hack to see if we can smooth out oscillations
+	if(rate_error < 0)
+		rate_error = rate_error >> 1;
+
 	// limit the rate
-	output =  constrain(g.pid_throttle.get_pid(rate_error, .1), -160, 180);
+	output =  constrain(g.pid_throttle.get_pid(rate_error, .02), -80, 120);
 
 	// light filter of output
-	output = (old_output + output) / 2;
+	//output = (old_output + output) / 2;
 
 	// save our output
-	old_output  = output;
+	//old_output  = output;
 
 	// output control:
 	return output + iterm;
@@ -219,8 +223,6 @@ get_nav_throttle(int32_t z_error)
 // Keeps old data out of our calculation / logs
 static void reset_nav_params(void)
 {
-	// forces us to update nav throttle
-	invalid_throttle 		= true;
 	nav_throttle 			= 0;
 
 	// always start Circle mode at same angle

ArduCopter/config.h

@@ -741,7 +741,7 @@
 # define THROTTLE_P			0.45	//
 #endif
 #ifndef THROTTLE_I
-# define THROTTLE_I			0.0		//
+# define THROTTLE_I			0.0		// Don't edit
 #endif
 #ifndef THROTTLE_D
 # define THROTTLE_D			0.02	//

ArduCopter/control_modes.pde

@@ -186,9 +186,6 @@ static void auto_trim()
 
 			//Serial.println("Done");
 			auto_level_counter = 0;
-
-			// set TC
-			init_throttle_cruise();
 		}
 	}
 }

ArduCopter/system.pde

@@ -532,16 +532,8 @@ static void set_mode(byte mode)
 		motor_auto_armed = true;
 	}
 
-	if(throttle_mode == THROTTLE_MANUAL){
-		// reset all of the throttle iterms
-		update_throttle_cruise();
-
-		// reset auto_throttle
-		nav_throttle 			= 0;
-	}else {
-		// an automatic throttle
-		init_throttle_cruise();
-	}
+	// called to calculate gain for alt hold
+	update_throttle_cruise();
 
 	if(roll_pitch_mode <= ROLL_PITCH_ACRO){
 		// We are under manual attitude control
@@ -594,18 +586,10 @@ static void update_throttle_cruise()
 		g.throttle_cruise += tmp;
 		reset_throttle_I();
 	}
-}
 
-static void
-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)){
-		reset_throttle_I();
-		g.throttle_cruise.set_and_save(g.rc_3.control_in);
-	}
-#endif
+	// recalc kp
+	g.pid_throttle.kP((float)g.throttle_cruise.get() / 981.0);
+	//Serial.printf("kp:%1.4f\n",kp);
 }
 
 #if CLI_SLIDER_ENABLED == ENABLED && CLI_ENABLED == ENABLED