Add code to disable throttle if we are on the ground and in FBW_B or higher

Add code to disable throttle if we are on the ground and in FBW_B or higher.  We believe we are on the ground if speed < 5 and alt < 5.  Also check that we are not trying to perform a takeoff.

ArduPlane/Attitude.pde

@@ -302,6 +302,17 @@ static void set_servos(void)
 		#else
 			// convert 0 to 100% into PWM
 			g.channel_throttle.servo_out = constrain(g.channel_throttle.servo_out, g.throttle_min.get(), g.throttle_max.get());
+			if (g.airspeed_enabled == true) {
+			// We want to supress the throttle if we think we are on the ground and in an autopilot controlled throttle mode.
+				if((control_mode==Circle || control_mode>=FBW_B) && current_loc.alt-home.alt < 500 && airspeed< 500) {
+					if(!(control_mode==Auto && takeoff_complete == false)  g.channel_throttle.servo_out = 0;
+				}
+			} else {
+				if((control_mode==Circle || control_mode>=FBW_B) && current_loc.alt-home.alt < 500 && g_gps->ground_speed < 500) {
+					if(!(control_mode==Auto && takeoff_complete == false)  g.channel_throttle.servo_out = 0;
+				}
+			}
+			
 		#endif
 
 		g.channel_throttle.calc_pwm();

ArduPlane/commands_logic.pde

@@ -222,6 +222,7 @@ static void do_takeoff()
 	next_WP.lat 		= home.lat + 1000;	// so we don't have bad calcs
 	next_WP.lng 		= home.lng + 1000;	// so we don't have bad calcs
 	takeoff_complete 	= false;			// set flag to use gps ground course during TO.  IMU will be doing yaw drift correction
+											// Flag also used to override "on the ground" throttle disable
 }
 
 static void do_nav_wp()