Added Loiter Turns - It orbits the Current location

Added WP to Loiter_time so you can spec a specific location in the planner
added new Navigation alg. Should perform better (doesn't rely on 45° flow fields like last one)
Added "Jump" counter so missions don't get overwritten in flight.
removed I term by default for Rate pitch and roll

ArduCopter/APM_Config.h

@@ -44,4 +44,7 @@
 
 // lets use Manual throttle during Loiter
 //#define LOITER_THR			THROTTLE_MANUAL
-# define RTL_YAW 			YAW_HOLD
+# define RTL_YAW 			YAW_HOLD
+
+#define RATE_ROLL_I 	0.18
+#define RATE_PITCH_I	0.18

ArduCopter/ArduCopter.pde

@@ -1,6 +1,6 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-#define THISFIRMWARE "ArduCopter V2.0.44 Beta"
+#define THISFIRMWARE "ArduCopter V2.0.45 Beta"
 /*
 ArduCopter Version 2.0 Beta
 Authors:	Jason Short
@@ -319,21 +319,22 @@ static const float radius_of_earth 	= 6378100;		// meters
 static const float gravity 			= 9.81;			// meters/ sec^2
 static long		target_bearing;						// deg * 100 : 0 to 360 location of the plane to the target
 
-static int	climb_rate;							// m/s * 100  - For future implementation of controlled ascent/descent by rate
-static byte	wp_control;							// used to control - navgation or loiter
+static int	climb_rate;								// m/s * 100  - For future implementation of controlled ascent/descent by rate
+static byte	wp_control;								// used to control - navgation or loiter
 
-static byte	command_must_index;					// current command memory location
-static byte	command_may_index;					// current command memory location
-static byte	command_must_ID;					// current command ID
-static byte	command_may_ID;						// current command ID
-static byte wp_verify_byte;						// used for tracking state of navigating waypoints
+static byte	command_must_index;						// current command memory location
+static byte	command_may_index;						// current command memory location
+static byte	command_must_ID;						// current command ID
+static byte	command_may_ID;							// current command ID
+static byte wp_verify_byte;							// used for tracking state of navigating waypoints
 
 static float cos_roll_x 	= 1;
 static float cos_pitch_x 	= 1;
 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 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
 
 // Acro
 #if CH7_OPTION == CH7_FLIP
@@ -654,6 +655,9 @@ static void medium_loop()
 				dTnav 				= (float)(millis() - nav_loopTimer)/ 1000.0;
 				nav_loopTimer 		= millis();
 
+				// prevent runup from bad GPS
+				dTnav = min(dTnav, 1.0);
+
 				// calculate the copter's desired bearing and WP distance
 				// ------------------------------------------------------
 				if(navigate()){
@@ -666,7 +670,6 @@ static void medium_loop()
 						Log_Write_Nav_Tuning();
 				}
 			}
-
 			break;
 
 		// command processing
@@ -1221,10 +1224,10 @@ static void update_trig(void){
 	sin_yaw_y 		= yawvector.x;	// 1 y
 
 	//flat:
-	// 0 ° = cp: 1.00, sp: 0.00, cr: 1.00, sr:  0.00, cy:  0.00, sy:  1.00,
-	// 90° = cp: 1.00, sp: 0.00, cr: 1.00, sr:  0.00, cy:  1.00, sy:  0.00,
-	// 180 = cp: 1.00, sp: 0.10, cr: 1.00, sr: -0.01, cy:  0.00, sy: -1.00,
-	// 270 = cp: 1.00, sp: 0.10, cr: 1.00, sr: -0.01, cy: -1.00, sy:  0.00,
+	// 0 ° = cos_yaw:  0.00, sin_yaw:  1.00,
+	// 90° = cos_yaw:  1.00, sin_yaw:  0.00,
+	// 180 = cos_yaw:  0.00, sin_yaw: -1.00,
+	// 270 = cos_yaw: -1.00, sin_yaw:  0.00,
 
 
 	//Vector3f accel_filt	= imu.get_accel_filtered();
@@ -1366,6 +1369,9 @@ static void update_nav_wp()
 		// use error as the desired rate towards the target
 		calc_nav_rate(long_error, lat_error, g.waypoint_speed_max, 0);
 
+		// rotate pitch and roll to the copter frame of reference
+		calc_nav_pitch_roll();
+
 	}else if(wp_control == CIRCLE_MODE){
 
 		// check if we have missed the WP
@@ -1396,21 +1402,17 @@ static void update_nav_wp()
 		// nav_lon, nav_lat is calculated
 		calc_nav_rate(long_error, lat_error, 200, 0);
 
+		// rotate pitch and roll to the copter frame of reference
+		calc_nav_pitch_roll();
+
 	} else {
 		// for long journey's reset the wind resopnse
 		// it assumes we are standing still.
-		g.pi_loiter_lat.reset_I();
-		g.pi_loiter_lat.reset_I();
-
-		// calc the lat and long error to the target
-		calc_location_error(&next_WP);
-
 		// use error as the desired rate towards the target
-		calc_nav_rate(long_error, lat_error, g.waypoint_speed_max, 100);
-
+		calc_nav_rate2(g.waypoint_speed_max);
+		// rotate pitch and roll to the copter frame of reference
+		calc_nav_pitch_roll2();
 	}
-	// rotate pitch and roll to the copter frame of reference
-	calc_nav_pitch_roll();
 }
 
 static void update_auto_yaw()

ArduCopter/commands_logic.pde

@@ -24,7 +24,7 @@ static void handle_process_must()
 			break;
 
 		case MAV_CMD_NAV_LOITER_TURNS:	// Loiter N Times
-			//do_loiter_turns();
+			do_loiter_turns();
 			break;
 
 		case MAV_CMD_NAV_LOITER_TIME:  // 19
@@ -311,9 +311,15 @@ static void do_loiter_turns()
 
 static void do_loiter_time()
 {
-	wp_control 		= LOITER_MODE;
-	set_next_WP(&current_loc);
-	loiter_time 	= millis();
+	if(next_command.lat == 0){
+		wp_control 		= LOITER_MODE;
+		loiter_time 	= millis();
+		set_next_WP(&current_loc);
+	}else{
+		wp_control 		= WP_MODE;
+		set_next_WP(&next_command);
+	}
+
 	loiter_time_max = next_command.p1 * 1000; // units are (seconds)
 }
 
@@ -429,8 +435,16 @@ static bool verify_loiter_unlim()
 
 static bool verify_loiter_time()
 {
-	if ((millis() - loiter_time) > loiter_time_max) {
-		return true;
+	if(wp_control == LOITER_MODE){
+		if ((millis() - loiter_time) > loiter_time_max) {
+			return true;
+		}
+	}
+	if(wp_control == WP_MODE &&  wp_distance <= g.waypoint_radius){
+		// reset our loiter time
+		loiter_time = millis();
+		// switch to position hold
+		wp_control 	= LOITER_MODE;
 	}
 	return false;
 }
@@ -636,16 +650,24 @@ static void do_loiter_at_location()
 static void do_jump()
 {
 	struct Location temp;
-	if(next_command.lat > 0) {
+	if(jump == -10){
+		jump = next_command.lat;
+	}
 
+	if(jump > 0) {
+		jump--;
 		command_must_index 	= 0;
 		command_may_index 	= 0;
-		temp 				= get_command_with_index(g.waypoint_index);
-		temp.lat 			= next_command.lat - 1;					// Decrement repeat counter
 
-		set_command_with_index(temp, g.waypoint_index);
+		// set pointer to desired index
 		g.waypoint_index 	= next_command.p1 - 1;
-	} else if (next_command.lat == -1) {
+
+	} else if (jump == 0){
+		// we're done, move along
+		jump = -10;
+
+	} else if (jump == -1) {
+		// repeat forever
 	    g.waypoint_index 	= next_command.p1 - 1;
 	}
 }

ArduCopter/navigation.pde

@@ -116,6 +116,58 @@ static void calc_nav_rate(int x_error, int y_error, int max_speed, int min_speed
 	nav_lon		 	= constrain(g.pi_nav_lon.get_pi(x_rate_error, dTnav), -3500, 3500);
 }
 
+#define NAV2_ERR_MAX 600.0
+static void calc_nav_rate2(int max_speed)
+{
+	float scaler 	= (float)wp_distance / NAV2_ERR_MAX;
+	scaler			= constrain(scaler, 0.0, 1.0);
+	max_speed 		= (float)max_speed * scaler;
+
+	// XXX target_angle should be the original  desired target angle!
+	float temp		= radians((original_target_bearing - g_gps->ground_course)/100.0);
+
+	x_actual_speed 	= -sin(temp) * (float)g_gps->ground_speed;
+	x_rate_error 	= -x_actual_speed;
+	x_rate_error 	= constrain(x_rate_error, -800, 800);
+	nav_lon		 	= constrain(g.pi_nav_lon.get_pi(x_rate_error, dTnav), -3500, 3500);
+
+	y_actual_speed 	= cos(temp) * (float)g_gps->ground_speed;
+	y_rate_error 	= max_speed - y_actual_speed; // 413
+	y_rate_error 	= constrain(y_rate_error, -800, 800);	// added a rate error limit to keep pitching down to a minimum
+	nav_lat		 	= constrain(g.pi_nav_lat.get_pi(y_rate_error, dTnav), -3500, 3500);
+
+	/*Serial.printf("max_speed: %d, xspeed: %d, yspeed: %d, x_re: %d, y_re: %d, nav_lon: %ld, nav_lat: %ld  ",
+					max_speed,
+					x_actual_speed,
+					y_actual_speed,
+					x_rate_error,
+					y_rate_error,
+					nav_lon,
+					nav_lat);*/
+}
+
+// nav_roll, nav_pitch
+static void calc_nav_pitch_roll2()
+{
+	float temp  	 = radians((float)(9000 - (dcm.yaw_sensor - original_target_bearing))/100.0);
+	float _cos_yaw_x = cos(temp);
+	float _sin_yaw_y = sin(temp);
+
+	// rotate the vector
+	nav_roll 	=  (float)nav_lon * _sin_yaw_y - (float)nav_lat * _cos_yaw_x;
+	nav_pitch 	=  (float)nav_lon * _cos_yaw_x + (float)nav_lat * _sin_yaw_y;
+
+	// flip pitch because forward is negative
+	nav_pitch = -nav_pitch;
+
+	/*Serial.printf("_cos_yaw_x:%1.4f, _sin_yaw_y:%1.4f, nav_roll:%ld, nav_pitch:%ld\n",
+					_cos_yaw_x,
+					_sin_yaw_y,
+					nav_roll,
+					nav_pitch);*/
+}
+
+
 // nav_roll, nav_pitch
 static void calc_nav_pitch_roll()
 {