Merge branch 'master' of https://code.google.com/p/ardupilot-mega

ArduCopter/ArduCopter.pde

@@ -2007,7 +2007,7 @@ static void update_nav_wp()
 {
 	if(wp_control == LOITER_MODE){
 
-		// calc a pitch to the target
+		// calc error to target
 		calc_location_error(&next_WP);
 
 		// use error as the desired rate towards the target
@@ -2064,6 +2064,9 @@ static void update_nav_wp()
 		//Serial.printf("CIRCLE: angle:%d, dist:%d, X:%d, Y:%d, P:%d, R:%d  \n", angleTest, (int)wp_distance , (int)long_error, (int)lat_error, npitch, nroll);
 
 	}else if(wp_control == WP_MODE){
+		// calc error to target
+		calc_location_error(&next_WP);
+
 		int16_t speed = calc_desired_speed(g.waypoint_speed_max);
 		// use error as the desired rate towards the target
 		calc_nav_rate(speed);

ArduCopter/Attitude.pde

@@ -321,9 +321,9 @@ static int get_angle_boost(int value)
 {
 	float temp = cos_pitch_x * cos_roll_x;
 	temp = 1.0 - constrain(temp, .5, 1.0);
-//	int16_t output = temp * value;
-//	return constrain(output, 0, 100);
-	return (int)(temp * value);
+	int16_t output = temp * value;
+	return constrain(output, 0, 100);
+//	return (int)(temp * value);
 }
 
 #define NUM_G_SAMPLES 40

ArduCopter/navigation.pde

@@ -261,7 +261,7 @@ static void calc_loiter_pitch_roll()
 static int16_t calc_desired_speed(int16_t max_speed)
 {
 	/*
-			   |< WP Radius
+	|< WP Radius
 	0  1   2   3   4   5   6   7   8m
 	...|...|...|...|...|...|...|...|
 		  100  |  200	  300	  400cm/s
@@ -274,15 +274,15 @@ static int16_t calc_desired_speed(int16_t max_speed)
 	// wp_distance is always in m/s and not cm/s - I know it's stupid that way
 	// for example 4m from target = 200cm/s speed
 	// we choose the lowest speed based on disance
-	max_speed 		= min(max_speed, (wp_distance * 50));
+	max_speed 		= min(max_speed, (wp_distance * 100));
 
 	// limit the ramp up of the speed
 	// waypoint_speed_gov is reset to 0 at each new WP command
 	if(waypoint_speed_gov < max_speed){
-		waypoint_speed_gov += (int)(50.0 * dTnav); // increase at .5/ms
+		waypoint_speed_gov += (int)(100.0 * dTnav); // increase at .5/ms
 
-		// go at least 50cm/s
-		max_speed 		= max(50, waypoint_speed_gov);
+		// go at least 100cm/s
+		max_speed 		= max(100, waypoint_speed_gov);
 		// limit with governer
 		max_speed 		= min(max_speed, waypoint_speed_gov);
 	}
@@ -305,7 +305,6 @@ static void calc_nav_rate(int max_speed)
 	nav_lon				= nav_lon_p + x_iterm;
 	nav_lon				= constrain(nav_lon, -3000, 3000);
 
-
 	y_rate_error 		= (sin(temp) * max_speed) - y_actual_speed; // 413
 	y_rate_error 		= constrain(y_rate_error, -1000, 1000);	// added a rate error limit to keep pitching down to a minimum
 	int16_t y_iterm 	= g.pi_loiter_lat.get_i(y_rate_error, dTnav);