Big update from Field testing.

Includes updated motor tests. These need to be verified.
Nav loiter D was added (.03) - verify this helps.
Nav_lat is now filtered for Rate control. This is to keep jerky pitching to a minimum.


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

ArduCopterMega/APM_Config.h

@@ -7,8 +7,11 @@
 //#define MAG_ORIENTATION		AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
 
 #define NAV_TEST 1	// 0 = traditional, 1 = rate controlled
-#define YAW_OPTION 1	// 0 = hybrid rate approach, 1 = offset Yaw approach
+#define YAW_OPTION 0	// 0 = hybrid rate approach, 1 = offset Yaw approach
-#define AUTO_RESET_LOITER 0 // enables Loiter to reset it's current location based on stick input.
+#define AUTO_RESET_LOITER 1 // enables Loiter to reset it's current location based on stick input.
+
+// do we want to have camera stabilization?
+#define CAMERA_STABILIZER ENABLED
 
 #define FRAME_CONFIG QUAD_FRAME
 	/*

ArduCopterMega/ArduCopterMega.pde

@@ -594,6 +594,9 @@ void medium_loop()
 				// control mode specific updates to nav_bearing
 				// --------------------------------------------
 				update_navigation();
+
+				if (g.log_bitmask & MASK_LOG_NTUN)
+					Log_Write_Nav_Tuning();
 			}
 
 			break;
@@ -632,19 +635,13 @@ void medium_loop()
 			medium_loopCounter++;
 
 			#if HIL_MODE != HIL_MODE_ATTITUDE
-				if (g.log_bitmask & MASK_LOG_ATTITUDE_MED && !(g.log_bitmask & MASK_LOG_ATTITUDE_FAST))
+				if (g.log_bitmask & MASK_LOG_ATTITUDE_MED)
 					Log_Write_Attitude();
 
 				if (g.log_bitmask & MASK_LOG_CTUN)
 					Log_Write_Control_Tuning();
 			#endif
 
-			if (g.log_bitmask & MASK_LOG_NTUN)
-				Log_Write_Nav_Tuning();
-
-			if (GPS_enabled && g.log_bitmask & MASK_LOG_GPS){
-				Log_Write_GPS();
-			}
 
 			// XXX this should be a "GCS medium loop" interface
 			#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
@@ -711,9 +708,9 @@ void fifty_hz_loop()
 			Log_Write_Raw();
 	#endif
 
-	//#if ENABLE_CAM
+	#if CAMERA_STABILIZER == ENABLED
 		camera_stabilization();
-	//#endif
+	#endif
 
 
 	// XXX is it appropriate to be doing the comms below on the fast loop?
@@ -904,6 +901,10 @@ void update_GPS(void)
 
 		current_loc.lng = g_gps->longitude;	// Lon * 10 * *7
 		current_loc.lat = g_gps->latitude;		// Lat * 10 * *7
+
+		if (g.log_bitmask & MASK_LOG_GPS){
+			Log_Write_GPS();
+		}
 	}
 }
 
@@ -978,15 +979,12 @@ void update_current_flight_mode(void)
 				nav_pitch 		= 0;
 				nav_roll 		= 0;
 
-				// Output Pitch, Roll, Yaw and Throttle
-				// ------------------------------------
-				// are we at rest? reset nav_yaw
 				if(g.rc_3.control_in == 0){
-					nav_yaw 	= dcm.yaw_sensor;
+					clear_yaw_control();
-				}
+				}else{
-
 					// Yaw control
 					output_manual_yaw();
+				}
 
 				// apply throttle control
 				output_manual_throttle();
@@ -1033,13 +1031,11 @@ void update_current_flight_mode(void)
 
 				// are we at rest? reset nav_yaw
 				if(g.rc_3.control_in == 0){
-					nav_yaw 	= dcm.yaw_sensor;
+					clear_yaw_control();
-				}
+				}else{
-
-				// Output Pitch, Roll, Yaw and Throttle
-				// ------------------------------------
 					// Yaw control
 					output_manual_yaw();
+				}
 
 				// apply throttle control
 				output_manual_throttle();
@@ -1165,8 +1161,8 @@ void update_navigation()
 			update_nav_yaw();
 		case LOITER:
 			// are we Traversing or Loitering?
-			//wp_control = (wp_distance < 10) ? LOITER_MODE : WP_MODE;
+			wp_control = (wp_distance < 50) ? LOITER_MODE : WP_MODE;
-			wp_control = LOITER_MODE;
+			//wp_control = LOITER_MODE;
 
 			// calculates the desired Roll and Pitch
 			update_nav_wp();
@@ -1241,8 +1237,8 @@ void update_alt()
 		//sonar_alt = sonar.read();
 
 		// decide if we're using sonar
-		if ((baro_alt < 1200) || sonar_alt < 300){
+		//if ((baro_alt < 1200) || sonar_alt < 300){
-
+		if (baro_alt < 700){
 			// correct alt for angle of the sonar
 			float temp = cos_pitch_x * cos_roll_x;
 			temp = max(temp, 0.707);
@@ -1280,15 +1276,14 @@ adjust_altitude()
 		flight_timer = 0;
 
 		if(g.rc_3.control_in <= 200){
-			next_WP.alt -= 3;												// 1 meter per second
+			next_WP.alt -= 1;												// 1 meter per second
+			next_WP.alt = max(next_WP.alt, (current_loc.alt - 100));		// don't go more than 4 meters below current location
 			next_WP.alt = max(next_WP.alt, 100); 							// no lower than 1 meter?
-			next_WP.alt = max((current_loc.alt - 400), next_WP.alt);		// don't go more than 4 meters below current location
+
 		}else if (g.rc_3.control_in > 700){
-			next_WP.alt += 3;												// 1 meter per second
+			next_WP.alt += 2;												// 1 meter per second
 			//next_WP.alt = min((current_loc.alt + 400), next_WP.alt);		// don't go more than 4 meters below current location
-			if(next_WP.alt > (current_loc.alt + 400)){
+			next_WP.alt = min(next_WP.alt, (current_loc.alt + 200));		// don't go more than 4 meters below current location
-				next_WP.alt = current_loc.alt + 400;
-			}
 		}
 	}
 }

ArduCopterMega/Attitude.pde

@@ -144,6 +144,9 @@ void calc_nav_throttle()
 	}
 
 	// simple filtering
+	if(nav_throttle_old == 0)
+		nav_throttle_old = nav_throttle;
+
 	nav_throttle 		= (nav_throttle + nav_throttle_old) >> 1;
 	nav_throttle_old 	= nav_throttle;
 
@@ -166,24 +169,12 @@ yaw control
 
 void output_manual_yaw()
 {
-	if(g.rc_3.control_in == 0){
-		// we want to only call this once
-		if(did_clear_yaw_control == false){
-			clear_yaw_control();
-			did_clear_yaw_control = true;
-		}
-
-	}else{ // motors running
-
 	// Yaw control
 	if(g.rc_4.control_in == 0){
 		output_yaw_with_hold(true); // hold yaw
 	}else{
 		output_yaw_with_hold(false); // rate control yaw
 	}
-
-		did_clear_yaw_control = false;
-	}
 }
 
 void auto_yaw()

ArduCopterMega/Camera.pde

@@ -1,5 +1,7 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
+#if CAMERA_STABILIZER == ENABLED
+
 void init_camera()
 {
 	g.rc_camera_pitch.set_angle(4500);
@@ -38,3 +40,4 @@ camera_stabilization()
 	//rc_camera_roll.calc_pwm();
 }
 
+#endif

ArduCopterMega/GCS_Mavlink.pde

@@ -637,6 +637,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 			switch (tell_command.id) {					// Switch to map APM command fields inot MAVLink command fields
 				case MAV_CMD_NAV_LOITER_TURNS:
 				case MAV_CMD_DO_SET_HOME:
+				case MAV_CMD_DO_SET_ROI:
 					tell_command.p1 = packet.param1;
 					break;
 

ArduCopterMega/Log.pde

@@ -97,6 +97,10 @@ dump_log(uint8_t argc, const Menu::arg *argv)
 	// check that the requested log number can be read
 	dump_log = argv[1].i;
 
+	if(dump_log == 99){
+		Log_Read(1, 4096);
+	}
+
 	if (/*(argc != 2) || */(dump_log < 1) || (dump_log > last_log_num)) {
 		Serial.printf_P(PSTR("bad log # %d\n"), dump_log);
 		return(-1);
@@ -175,9 +179,11 @@ select_logs(uint8_t argc, const Menu::arg *argv)
 
 	if (!strcasecmp_P(argv[0].str, PSTR("enable"))) {
 		g.log_bitmask.set_and_save(g.log_bitmask | bits);
+
 	}else{
 		g.log_bitmask.set_and_save(g.log_bitmask & ~bits);
 	}
+
 	return(0);
 }
 

ArduCopterMega/Parameters.h

@@ -237,7 +237,7 @@ public:
         throttle_fs_value 		(THROTTLE_FS_VALUE,         k_param_throttle_fs_value, 				PSTR("THR_FS_VALUE")),
         throttle_cruise         (100,						k_param_throttle_cruise,    			PSTR("TRIM_THROTTLE")),
 
-        flight_modes            (k_param_flight_modes,                                     			PSTR("FLIGHT_MODES")),
+        flight_modes            (k_param_flight_modes,                                     			PSTR("FLTMODE")),
 
         pitch_max         		(PITCH_MAX * 100,			k_param_pitch_max,		       			PSTR("PITCH_MAX")),
 

ArduCopterMega/command description.txt

@@ -71,4 +71,4 @@ Command ID		Name						Parameter 1			Parameter 2			Parameter 3			Parameter 4
 				Note:  Max cycle time = 60 sec, A repeat relay or repeat servo command will cancel any current repeating event
 
 // need to add command
-0xB7 / 185		MAV_CMD_DO_SET_ROI			Yaw_Mode			altitude			lat					lon
+0xB7 / 201		MAV_CMD_DO_SET_ROI			Yaw_Mode			altitude			lat					lon

ArduCopterMega/commands.pde

@@ -51,7 +51,7 @@ struct Location get_command_with_index(int i)
 		return temp;
 
 	}else{
-		Serial.println("LD");
+		//Serial.println("LD");
 		// we can load a command, we don't process it yet
 		// read WP position
 		long mem = (WP_START_BYTE) + (i * WP_SIZE);
@@ -78,7 +78,7 @@ struct Location get_command_with_index(int i)
 	if((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) && temp.options & WP_OPTION_ALT_RELATIVE){
 		//temp.alt += home.alt;
 	}
-	Serial.println("ADD ALT");
+	//Serial.println("ADD ALT");
 
 	if(temp.options & WP_OPTION_RELATIVE){
 		// If were relative, just offset from home

ArduCopterMega/commands_logic.pde

@@ -5,6 +5,9 @@
 /********************************************************************************/
 void handle_process_must()
 {
+	// clear nav_lat, this is how we pitch towards the target based on speed
+	nav_lat = 0;
+
 	switch(next_command.id){
 
 		case MAV_CMD_NAV_TAKEOFF:
@@ -38,6 +41,7 @@ void handle_process_must()
 		default:
 			break;
 	}
+
 }
 
 void handle_process_may()
@@ -494,9 +498,9 @@ void do_yaw()
 	command_yaw_speed		= next_command.lat * 100; // ms * 100
 
 
-	// if unspecified go 60° a second
+	// if unspecified go 30° a second
 	if(command_yaw_speed == 0)
-		command_yaw_speed = 6000;
+		command_yaw_speed = 3000;
 
 	// if unspecified go counterclockwise
 	if(command_yaw_dir == 0)

ArduCopterMega/commands_process.pde

@@ -16,62 +16,10 @@ void change_command(uint8_t index)
 	}
 }
 
-
-// called by 10 Hz Medium loop
-// ---------------------------
-void update_commands2(void)
-{
-	//Serial.println("Upd CMDs");
-
-	// This function loads commands into three buffers
-	// when a new command is loaded, it is processed with process_XXX()
-
-	// If we have a command in the queue already
-	if(next_command.id != NO_COMMAND){
-		return;
-	}
-
-	// fetch next command if the next command queue is empty
-	// -----------------------------------------------------
-	next_command = get_command_with_index(g.waypoint_index + 1);
-	Serial.printf("next CMD %d\n", next_command.id);
-
-	if(next_command.id == NO_COMMAND){
-		// if no commands were available from EEPROM
-		// And we have no nav commands
-		// --------------------------------------------
-		if (command_must_ID == NO_COMMAND){
-			gcs.send_text_P(SEVERITY_LOW,PSTR("out of commands!"));
-			handle_no_commands();
-		}
-
-	} else {
-		// A command was loaded from EEPROM
-		// --------------------------------------------
-		// Set our current mission index + 1;
-		increment_WP_index();
-
-		Serial.printf("loaded cmd %d\n" , g.waypoint_index.get());
-
-		// debug by outputing the Waypoint loaded
-		print_wp(&next_command, g.waypoint_index);
-
-
-		// act on our new command
-		if (process_next_command()){
-			// invalidate command queue so a new one is loaded
-			// -----------------------------------------------
-			clear_command_queue();
-		}
-	}
-}
-
 // called by 10 Hz Medium loop
 // ---------------------------
 void update_commands(void)
 {
-	//Serial.println("Upd CMDs");
-
 	// fill command queue with a new command if available
 	if(next_command.id == NO_COMMAND){
 

ArduCopterMega/config.h

@@ -371,7 +371,7 @@
 //
 // how much to we pitch towards the target
 #ifndef PITCH_MAX
-# define PITCH_MAX				18			// degrees
+# define PITCH_MAX				24			// degrees
 #endif
 
 
@@ -382,10 +382,10 @@
 # define NAV_LOITER_P			2.5			// upped to be a bit more aggressive
 #endif
 #ifndef NAV_LOITER_I
-# define NAV_LOITER_I			0.05		// upped a bit to deal with wind faster
+# define NAV_LOITER_I			0.08		// upped a bit to deal with wind faster
 #endif
 #ifndef NAV_LOITER_D
-# define NAV_LOITER_D			0.00
+# define NAV_LOITER_D			0.03		// Added some D 2.20, untested
 #endif
 #ifndef NAV_LOITER_IMAX
 # define NAV_LOITER_IMAX		20			// 20°
@@ -515,6 +515,12 @@
 # define LOG_CURRENT			DISABLED
 #endif
 
+// if we are using fast, Disable Medium
+#if LOG_ATTITUDE_FAST == ENABLED
+	#undef LOG_ATTITUDE_MED
+	#define LOG_ATTITUDE_MED 		DISABLED
+#endif
+
 #ifndef DEBUG_PORT
 # define DEBUG_PORT 0
 #endif

ArduCopterMega/motors_hexa.pde

@@ -111,6 +111,68 @@ void output_motors_disarmed()
 
 void output_motor_test()
 {
+	motor_out[CH_1] = g.rc_3.radio_min;
+	motor_out[CH_2] = g.rc_3.radio_min;
+	motor_out[CH_3] = g.rc_3.radio_min;
+	motor_out[CH_4] = g.rc_3.radio_min;
+	motor_out[CH_7] = g.rc_3.radio_min;
+	motor_out[CH_8] = g.rc_3.radio_min;
+
+
+	if(g.frame_orientation == X_FRAME){
+//  31
+//	24
+		if(g.rc_1.control_in > 3000){	// right
+			motor_out[CH_1] += 50;
+		}
+
+		if(g.rc_1.control_in < -3000){	// left
+			motor_out[CH_2] += 50;
+		}
+
+		if(g.rc_2.control_in > 3000){ 	// back
+			motor_out[CH_8] += 50;
+			motor_out[CH_4] += 50;
+		}
+
+		if(g.rc_2.control_in < -3000){	// front
+			motor_out[CH_7] += 50;
+			motor_out[CH_3] += 50;
+		}
+
+	}else{
+//  3
+// 2 1
+//	4
+		if(g.rc_1.control_in > 3000){	// right
+			motor_out[CH_4] += 50;
+			motor_out[CH_8] += 50;
+		}
+
+		if(g.rc_1.control_in < -3000){	// left
+			motor_out[CH_7] += 50;
+			motor_out[CH_3] += 50;
+		}
+
+		if(g.rc_2.control_in > 3000){ 	// back
+			motor_out[CH_2] += 50;
+		}
+
+		if(g.rc_2.control_in < -3000){	// front
+			motor_out[CH_1] += 50;
+		}
+
+	}
+
+	APM_RC.OutputCh(CH_1, motor_out[CH_1]);
+	APM_RC.OutputCh(CH_2, motor_out[CH_2]);
+	APM_RC.OutputCh(CH_3, motor_out[CH_3]);
+	APM_RC.OutputCh(CH_4, motor_out[CH_4]);
+	APM_RC.OutputCh(CH_7, motor_out[CH_7]);
+	APM_RC.OutputCh(CH_8, motor_out[CH_8]);
+}
+
+/*
 	APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
 	APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 50);
 	delay(1000);
@@ -134,6 +196,7 @@ void output_motor_test()
 	APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
 	APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 50);
 	delay(1000);
+*/
 }
 
 #endif

ArduCopterMega/motors_quad.pde

@@ -96,21 +96,56 @@ void output_motors_disarmed()
 
 void output_motor_test()
 {
-	APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
+	motor_out[CH_1] = g.rc_3.radio_min;
-	APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 50);
+	motor_out[CH_2] = g.rc_3.radio_min;
-	delay(1000);
+	motor_out[CH_3] = g.rc_3.radio_min;
+	motor_out[CH_4] = g.rc_3.radio_min;
 
-	APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
-	APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 50);
-	delay(1000);
 
-	APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
+	if(g.frame_orientation == X_FRAME){
-	APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 50);
+//  31
-	delay(1000);
+//	24
+		if(g.rc_1.control_in > 3000){
+			motor_out[CH_1] += 50;
+			motor_out[CH_4] += 50;
+		}
 
-	APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
+		if(g.rc_1.control_in < -3000){
-	APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 50);
+			motor_out[CH_2] += 50;
-	delay(1000);
+			motor_out[CH_3] += 50;
+		}
+
+		if(g.rc_2.control_in > 3000){
+			motor_out[CH_2] += 50;
+			motor_out[CH_4] += 50;
+		}
+
+		if(g.rc_2.control_in < -3000){
+			motor_out[CH_1] += 50;
+			motor_out[CH_3] += 50;
+		}
+
+	}else{
+//  3
+// 2 1
+//	4
+		if(g.rc_1.control_in > 3000)
+			motor_out[CH_1] += 50;
+
+		if(g.rc_1.control_in < -3000)
+			motor_out[CH_2] += 50;
+
+		if(g.rc_2.control_in > 3000)
+			motor_out[CH_4] += 50;
+
+		if(g.rc_2.control_in < -3000)
+			motor_out[CH_3] += 50;
+	}
+
+	APM_RC.OutputCh(CH_1, motor_out[CH_1]);
+	APM_RC.OutputCh(CH_2, motor_out[CH_2]);
+	APM_RC.OutputCh(CH_3, motor_out[CH_3]);
+	APM_RC.OutputCh(CH_4, motor_out[CH_4]);
 }
 
 #endif

ArduCopterMega/motors_tri.pde

@@ -69,17 +69,26 @@ void output_motors_disarmed()
 
 void output_motor_test()
 {
-	APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
+	motor_out[CH_1] = g.rc_3.radio_min;
-	APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 50);
+	motor_out[CH_2] = g.rc_3.radio_min;
-	delay(1000);
+	motor_out[CH_4] = g.rc_3.radio_min;
 
-	APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
-	APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 50);
-	delay(1000);
 
-	APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
+	if(g.rc_1.control_in > 3000){	// right
-	APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 50);
+		motor_out[CH_1] += 50;
-	delay(1000);
+	}
+
+	if(g.rc_1.control_in < -3000){	// left
+		motor_out[CH_2] += 50;
+	}
+
+	if(g.rc_2.control_in > 3000){	// back
+		motor_out[CH_4] += 50;
+	}
+
+	APM_RC.OutputCh(CH_1, motor_out[CH_1]);
+	APM_RC.OutputCh(CH_2, motor_out[CH_2]);
+	APM_RC.OutputCh(CH_4, motor_out[CH_4]);
 }
 
 #endif

ArduCopterMega/motors_y6.pde

@@ -88,23 +88,35 @@ void output_motors_disarmed()
 
 void output_motor_test()
 {
-	APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
+	motor_out[CH_1] = g.rc_3.radio_min;
-	APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
+	motor_out[CH_2] = g.rc_3.radio_min;
-	APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 50);
+	motor_out[CH_3] = g.rc_3.radio_min;
-	APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 50);
+	motor_out[CH_4] = g.rc_3.radio_min;
-	delay(1000);
+	motor_out[CH_7] = g.rc_3.radio_min;
+	motor_out[CH_8] = g.rc_3.radio_min;
 
-	APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
-	APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
-	APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 50);
-	APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 50);
-	delay(1000);
 
-	APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
+	if(g.rc_1.control_in > 3000){	// right
-	APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
+		motor_out[CH_1] += 50;
-	APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 50);
+		motor_out[CH_7] += 50;
-	APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 50);
+	}
-	delay(1000);
+
+	if(g.rc_1.control_in < -3000){	// left
+		motor_out[CH_2] += 50;
+		motor_out[CH_3] += 50;
+	}
+
+	if(g.rc_2.control_in > 3000){	// back
+		motor_out[CH_8] += 50;
+		motor_out[CH_4] += 50;
+	}
+
+	APM_RC.OutputCh(CH_1, motor_out[CH_1]);
+	APM_RC.OutputCh(CH_2, motor_out[CH_2]);
+	APM_RC.OutputCh(CH_3, motor_out[CH_4]);
+	APM_RC.OutputCh(CH_4, motor_out[CH_4]);
+	APM_RC.OutputCh(CH_7, motor_out[CH_7]);
+	APM_RC.OutputCh(CH_8, motor_out[CH_8]);
 }
 
 #endif

ArduCopterMega/navigation.pde

@@ -165,9 +165,10 @@ void calc_rate_nav()
 	// we want to be going 450cm/s
 	int error 			= constrain(WAYPOINT_SPEED - groundspeed, -1000, 1000);
 	// Scale response by kP
-	nav_lat 		= g.pid_nav_wp.get_pid(error, dTnav, 1.0);
+	nav_lat 		= nav_lat + g.pid_nav_wp.get_pid(error, dTnav, 1.0);
+	nav_lat 		>>= 1; // divide by two
 
-	Serial.printf("dTnav: %ld, gs: %d, err: %d, int: %d, pitch: %ld", dTnav,  groundspeed, error, (int)g.pid_nav_wp.get_integrator(), (long)nav_lat);
+	//Serial.printf("dTnav: %ld, gs: %d, err: %d, int: %d, pitch: %ld", dTnav,  groundspeed, error, (int)g.pid_nav_wp.get_integrator(), (long)nav_lat);
 
 	// limit our output
 	nav_lat	= constrain(nav_lat, 	-4000, 4000); // +- max error
@@ -219,7 +220,7 @@ void update_crosstrack(void)
 {
 	// Crosstrack Error
 	// ----------------
-	if (cross_track_test() < 5000) {	 // If we are too far off or too close we don't do track following
+	if (cross_track_test() < 9000) {	 // If we are too far off or too close we don't do track following
 		crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / 100)) * wp_distance;	 // Meters we are off track line
 		nav_bearing += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get());
 		nav_bearing = wrap_360(nav_bearing);

ArduCopterMega/setup.pde

@@ -247,6 +247,8 @@ static int8_t
 setup_motors(uint8_t argc, const Menu::arg *argv)
 {
 	while(1){
+		delay(20);
+		read_radio();
 		output_motor_test();
 		if(Serial.available() > 0){
 			g.esc_calibrate.set_and_save(0);

ArduCopterMega/system.pde

@@ -37,7 +37,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = {
 };
 
 // Create the top-level menu object.
-MENU(main_menu, "AC 2.0.19 Beta", main_menu_commands);
+MENU(main_menu, "AC 2.0.20 Beta", main_menu_commands);
 
 void init_ardupilot()
 {
@@ -137,7 +137,10 @@ void init_ardupilot()
 
 	init_rc_in();		// sets up rc channels from radio
 	init_rc_out();		// sets up the timer libs
+
+	#if CAMERA_STABILIZER == ENABLED
 		init_camera();
+	#endif
 
 	#if HIL_MODE != HIL_MODE_ATTITUDE
 	adc.Init();	 		// APM ADC library initialization
@@ -331,8 +334,11 @@ void set_mode(byte mode)
 		case ACRO:
 			break;
 
+		case SIMPLE:
 		case STABILIZE:
 			do_loiter_at_location();
+			g.pid_baro_throttle.reset_I();
+			g.pid_sonar_throttle.reset_I();
 			break;
 
 		case ALT_HOLD:
@@ -345,9 +351,6 @@ void set_mode(byte mode)
 			init_auto();
 			break;
 
-		case SIMPLE:
-			break;
-
 		case LOITER:
 			init_throttle_cruise();
 			do_loiter_at_location();