Loiter: Made the "retro loiter" routines configurable. Add RETRO_LOITER_MODE ENABLED to APM_Config.h to enable the older loiter shtuff.

ArduCopter/ArduCopter.pde

@@ -922,9 +922,11 @@ void loop()
 
 		// check for new GPS messages
 		// --------------------------
-		//if(GPS_enabled){
-		//	update_GPS();
-		//}
+		#if RETRO_LOITER_MODE == DISABLED
+			if(GPS_enabled){
+				update_GPS();
+			}
+		#endif
 
 		// perform 10hz tasks
 		// ------------------
@@ -996,9 +998,11 @@ static void medium_loop()
 		case 0:
 			medium_loopCounter++;
 
+			#if RETRO_LOITER_MODE == ENABLED
 			if(GPS_enabled){
 				update_GPS();
 			}
+			#endif
 
 			#if HIL_MODE != HIL_MODE_ATTITUDE					// don't execute in HIL mode
 				if(g.compass_enabled){
@@ -1036,8 +1040,11 @@ static void medium_loop()
 					// this calculates the velocity for Loiter
 					// only called when there is new data
 					// ----------------------------------
-					//calc_XY_velocity();
+					#if RETRO_LOITER_MODE == ENABLED
 					calc_GPS_velocity();
+					#else
+					calc_XY_velocity();
+					#endif
 
 					// If we have optFlow enabled we can grab a more accurate speed
 					// here and override the speed from the GPS

ArduCopter/config.h

@@ -942,4 +942,9 @@
 # define QUATERNION_ENABLE DISABLED
 #endif
 
+// TEMPORARY FOR TESTING
+#ifndef RETRO_LOITER_MODE
+# define RETRO_LOITER_MODE DISABLED
+#endif
+
 #endif // __ARDUCOPTER_CONFIG_H__

ArduCopter/navigation.pde

@@ -162,8 +162,10 @@ static void calc_location_error(struct Location *next_loc)
 #define NAV_RATE_ERR_MAX 250
 static void calc_loiter(int x_error, int y_error)
 {
+	#if RETRO_LOITER_MODE == ENABLED
 	x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX);
 	y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX);
+	#endif
 
 	int32_t p,i,d;						// used to capture pid values for logging
 	int32_t output;
@@ -179,7 +181,10 @@ static void calc_loiter(int x_error, int y_error)
 	}
 #endif
 
-	x_rate_error	= constrain(x_target_speed - x_actual_speed, -NAV_RATE_ERR_MAX, NAV_RATE_ERR_MAX);			// calc the speed error
+	x_rate_error	= x_target_speed - x_actual_speed;			// calc the speed error
+	#if RETRO_LOITER_MODE == ENABLED
+	x_rate_error = constrain(x_rate_error, -NAV_RATE_ERR_MAX, NAV_RATE_ERR_MAX);
+	#endif
 	p				= g.pid_loiter_rate_lon.get_p(x_rate_error);
 	i				= g.pid_loiter_rate_lon.get_i(x_rate_error, dTnav);
 	d				= g.pid_loiter_rate_lon.get_d(x_rate_error, dTnav);
@@ -206,7 +211,10 @@ static void calc_loiter(int x_error, int y_error)
 	}
 #endif
 
-	y_rate_error	= constrain(y_target_speed - y_actual_speed, -NAV_RATE_ERR_MAX, NAV_RATE_ERR_MAX);
+	y_rate_error	= y_target_speed - y_actual_speed;
+	#if RETRO_LOITER_MODE == ENABLED
+	y_rate_error = constrain(y_rate_error, -NAV_RATE_ERR_MAX, NAV_RATE_ERR_MAX);
+	#endif
 	p				= g.pid_loiter_rate_lat.get_p(y_rate_error);
 	i				= g.pid_loiter_rate_lat.get_i(y_rate_error, dTnav);
 	d				= g.pid_loiter_rate_lat.get_d(y_rate_error, dTnav);