Implementing a limiter for the ramp up of speed to WPs.

ArduCopter/ArduCopter.pde

@@ -341,6 +341,7 @@ 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 byte jump = -10;								// used to track loops in jump command
+static int waypoint_speed_gov;
 
 // Acro
 #if CH7_OPTION == CH7_FLIP

ArduCopter/commands.pde

@@ -170,14 +170,19 @@ static void set_next_WP(struct Location *wp)
 	scaleLongUp 		= 1.0f/cos(rads);
 
 	// this is handy for the groundstation
+	// -----------------------------------
 	wp_totalDistance 	= get_distance(&current_loc, &next_WP);
 	wp_distance 		= wp_totalDistance;
 	target_bearing 		= get_bearing(&current_loc, &next_WP);
 
 	// to check if we have missed the WP
-	// ----------------------------
+	// ---------------------------------
 	original_target_bearing = target_bearing;
 
+	// reset speed governer
+	// --------------------
+	waypoint_speed_gov = 0;
+
 	gcs.print_current_waypoints();
 }
 

ArduCopter/commands_logic.pde

@@ -247,6 +247,7 @@ static void do_nav_wp()
 	}
 	set_next_WP(&next_command);
 
+
 	// this is our bitmask to verify we have met all conditions to move on
 	wp_verify_byte 	= 0;
 

ArduCopter/navigation.pde

@@ -111,9 +111,14 @@ static void calc_nav_rate(int max_speed)
 		  100	  200	  300	  400
 	                                     +|+
 	*/
-
 	max_speed 		= min(max_speed, (wp_distance * 50));
 
+	// limit the ramp up of the speed
+	if(waypoint_speed_gov < max_speed){
+		waypoint_speed_gov += 40;
+		max_speed 		= min(max_speed, waypoint_speed_gov);
+	}
+
 	// XXX target_angle should be the original  desired target angle!
 	float temp		= radians((original_target_bearing - g_gps->ground_course)/100.0);