Attitude

Made the Acro mode more NG like. Should be much more nimble!
Tweaked the Accel hold with sim tests. not flight tested or enabled by default.
added option to set scalar in angle boost

ArduCopter/Attitude.pde

@@ -98,28 +98,41 @@ get_nav_throttle(long z_error)
 	return (int)g.pi_throttle.get_pi(rate_error, .1);
 }
 
+#define ALT_ERROR_MAX2 300
+static int
+get_nav_throttle2(long z_error)
+{
+	if (z_error > ALT_ERROR_MAX2){
+		return g.pi_throttle.kP() * 80;
+
+	}else if (z_error < -ALT_ERROR_MAX2){
+		return g.pi_throttle.kP() * -60;
+
+	} else{
+		// limit error to prevent I term run up
+		z_error 		= constrain(z_error, -ALT_ERROR_MAX2, ALT_ERROR_MAX2);
+		int rate_error 	= g.pi_alt_hold.get_pi(z_error, .1); //_p = .85
+
+		rate_error 		= rate_error - altitude_rate;
+
+		// limit the rate
+		rate_error 		= constrain(rate_error, -100, 120);
+		return (int)g.pi_throttle.get_pi(rate_error, .1) + alt_hold_velocity();
+	}
+}
+
 static int
 get_rate_roll(long target_rate)
 {
-	long error;
+	long error	= (target_rate * 3.5) - (long)(degrees(omega.x) * 100.0);
-	target_rate 	= constrain(target_rate, -2500, 2500);
+	return g.pi_acro_roll.get_pi(error, G_Dt);
-	error			= (target_rate * 4.5) - (long)(degrees(omega.x) * 100.0);
-	target_rate 	= g.pi_rate_roll.get_pi(error, G_Dt);
-
-	// output control:
-	return (int)constrain(target_rate, -2500, 2500);
 }
 
 static int
 get_rate_pitch(long target_rate)
 {
-	long error;
+	long error	= (target_rate * 3.5) - (long)(degrees(omega.y) * 100.0);
-	target_rate 	= constrain(target_rate, -2500, 2500);
+	return  g.pi_acro_pitch.get_pi(error, G_Dt);
-	error			= (target_rate * 4.5) - (long)(degrees(omega.y) * 100.0);
-	target_rate 	= g.pi_rate_pitch.get_pi(error, G_Dt);
-
-	// output control:
-	return (int)constrain(target_rate, -2500, 2500);
 }
 
 static int
@@ -139,7 +152,7 @@ get_rate_yaw(long target_rate)
 static void reset_hold_I(void)
 {
 	g.pi_loiter_lat.reset_I();
-	g.pi_loiter_lat.reset_I();
+	g.pi_loiter_lon.reset_I();
 	g.pi_crosstrack.reset_I();
 }
 
@@ -189,19 +202,21 @@ get_nav_yaw_offset(int yaw_input, int reset)
 static int alt_hold_velocity()
 {
 	#if ACCEL_ALT_HOLD == 1
+		Vector3f accel_filt;
+		float error;
+
 		// subtract filtered Accel
-		float error	= abs(next_WP.alt - current_loc.alt);
+		error		= abs(next_WP.alt - current_loc.alt) - 25;
-
+		error		= min(error, 50.0);
-		error -= 100;
-		error = min(error, 200.0);
 		error		= max(error, 0.0);
-		error = 1 - (error/ 200.0);
+		error		= 1 - (error/ 50.0);
-		float sum = accels_rot_sum / (float)accels_rot_count;
 
-		accels_rot_sum = 0;
+		accel_filt	= imu.get_accel_filtered();
-		accels_rot_count = 0;
+		accels_rot 	= dcm.get_dcm_matrix() * imu.get_accel_filtered();
+		int output = (accels_rot.z + 9.81) * alt_hold_gain * error;   // alt_hold_gain  = 12
 
-		int output = (sum + 9.81) * alt_hold_gain * error;
+		//Serial.printf("s: %1.4f, g:%1.4f, e:%1.4f, o:%d\n",sum, alt_hold_gain, error, output);
+		return constrain(output, -70, 70);
 
 // fast rise
 //s: -17.6241, g:0.0000, e:1.0000, o:0
@@ -209,17 +224,15 @@ static int alt_hold_velocity()
 //s: -19.3193, g:0.0000, e:1.0000, o:0
 //s: -13.1310, g:47.8700, e:1.0000, o:-158
 
-		//Serial.printf("s: %1.4f, g:%1.4f, e:%1.4f, o:%d\n",sum, alt_hold_gain, error, output);
-		return output;
 	#else
 		return 0;
 	#endif
 }
 
-static int get_angle_boost()
+static int get_angle_boost(int value)
 {
 	float temp = cos_pitch_x * cos_roll_x;
 	temp = 1.0 - constrain(temp, .5, 1.0);
-	return (int)(temp * g.throttle_cruise);
+	return (int)(temp * value);
 }