Fixed bug in alt hold caused by false Sonar readings.

Reverted to prior Rangefinderlib in case that was causing bad sonar readings.
reduced crosstrack gains and max angle to avoid overshoots and oscillations.
added additional D gain to Loiter hold to prevent overshoots
added D gain to WP_nav to avoid speed oscillations
upped navigation speed to 6m/s from 4.5m/s


git-svn-id: https://arducopter.googlecode.com/svn/trunk@2710 f9c3cf11-9bcb-44bc-f272-b75c42450872

ArduCopterMega/ArduCopterMega.pde

@@ -1336,10 +1336,15 @@ void update_alt()
 		sonar_alt = (float)sonar_alt * temp;
 		*/
 
-		scale = (sonar_alt - 300) / 300;
-		scale = constrain(scale, 0, 1);
+		if(baro_alt < 800){
+			scale = (sonar_alt - 300) / 300;
+			scale = constrain(scale, 0, 1);
 
-		current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale) + home.alt;
+			current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale) + home.alt;
+
+		}else{
+			current_loc.alt = baro_alt + home.alt;
+		}
 
 	}else{
 		baro_alt 		= read_barometer();

ArduCopterMega/config.h

@@ -388,6 +388,8 @@
 	# define YAW_IMAX				1			// degrees * 100
 	#endif
 #endif
+
+
 //////////////////////////////////////////////////////////////////////////////
 // Autopilot control limits
 //
@@ -407,7 +409,7 @@
 # define NAV_LOITER_I			0.08		// upped a bit to deal with wind faster
 #endif
 #ifndef NAV_LOITER_D
-# define NAV_LOITER_D			0.03		// Added some D 2.20, untested
+# define NAV_LOITER_D			0.15		//
 #endif
 #ifndef NAV_LOITER_IMAX
 # define NAV_LOITER_IMAX		20			// 20°
@@ -422,7 +424,7 @@
 # define NAV_WP_I				0.5			// this is a fast ramp up
 #endif
 #ifndef NAV_WP_D
-# define NAV_WP_D				0			// slight dampening of a few degrees at most
+# define NAV_WP_D				.3			//
 #endif
 #ifndef NAV_WP_IMAX
 # define NAV_WP_IMAX			40			// degrees
@@ -430,7 +432,7 @@
 
 
 #ifndef WAYPOINT_SPEED
-# define WAYPOINT_SPEED				450			// for 4.5 ms error = 13.5 pitch
+# define WAYPOINT_SPEED			600			// for 6m/s error = 13mph
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
@@ -468,10 +470,10 @@
 // Crosstrack compensation
 //
 #ifndef XTRACK_GAIN
-# define XTRACK_GAIN          5 // deg/m
+# define XTRACK_GAIN          2 // deg/m
 #endif
 #ifndef XTRACK_ENTRY_ANGLE
-# define XTRACK_ENTRY_ANGLE   40 // deg
+# define XTRACK_ENTRY_ANGLE   8 // deg
 #endif
 
 

ArduCopterMega/system.pde

@@ -40,7 +40,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = {
 };
 
 // Create the top-level menu object.
-MENU(main_menu, "AC 2.0.30 Beta", main_menu_commands);
+MENU(main_menu, "AC 2.0.31 Beta", main_menu_commands);
 
 void init_ardupilot()
 {