Lots of little things. Yaw control is updated based on Jani's logs. Thanks!

Takeoff now works well.
better logging.
tuned down kD for Yaw - was causing some bad behavior.
Auto mission now resets when entering mode.

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

ArduCopterMega/APM_Config.h

@@ -6,18 +6,32 @@
 
 #define MAG_ORIENTATION		AP_COMPASS_COMPONENTS_DOWN_PINS_BACK
 
-#define GPS_PROTOCOL GPS_PROTOCOL_MTK
+//#define GPS_PROTOCOL GPS_PROTOCOL_MTK
 
-#define GCS_PROTOCOL        GCS_PROTOCOL_NONE
+//#define GCS_PROTOCOL        GCS_PROTOCOL_NONE
 //#define GCS_PORT 0
 
 #define SERIAL0_BAUD			38400
 
-# define STABILIZE_ROLL_P 		0.75
-# define STABILIZE_PITCH_P		0.75
-//# define STABILIZE_DAMPENER		0.1
+#define STABILIZE_ROLL_P 		0.75
+#define STABILIZE_PITCH_P		0.75
+//#define STABILIZE_DAMPENER		0.1
 
 
+#define CHANNEL_6_TUNING CH6_NONE
+//#define CHANNEL_6_TUNING CH6_STABLIZE_KP
+
+	/*
+	CH6_NONE
+	CH6_STABLIZE_KP
+	CH6_STABLIZE_KD
+	CH6_BARO_KP
+	CH6_BARO_KD
+	CH6_SONAR_KP
+	CH6_SONAR_KD
+	CH6_Y6_SCALING
+	*/
+
 //#define ACRO_RATE_TRIGGER 4200
 // if you want full ACRO mode, set value to 0
 // if you want mostly stabilize with flips, set value to 4200

ArduCopterMega/ArduCopterMega.pde

@@ -632,17 +632,9 @@ void medium_loop()
 				Log_Write_Nav_Tuning();
 
 			if (g.log_bitmask & MASK_LOG_GPS){
-				if(home_is_set){
-					Log_Write_GPS(g_gps->time,
-						current_loc.lat,
-						current_loc.lng,
-						g_gps->altitude,
-						current_loc.alt,
-						(long)g_gps->ground_speed,
-						g_gps->ground_course,
-						g_gps->fix,
-						g_gps->num_sats);
-				}
+				//if(home_is_set){
+					Log_Write_GPS();
+				//}
 			}
 
 			// XXX this should be a "GCS medium loop" interface
@@ -845,7 +837,7 @@ void update_GPS(void)
 				init_home();
 
 				// init altitude
-				current_loc.alt = g_gps->altitude;
+				current_loc.alt = home.alt;
 				ground_start_count = 0;
 			}
 		}
@@ -1135,15 +1127,14 @@ void update_alt()
 		if(altitude_sensor == BARO){
 			current_loc.alt = baro_alt + home.alt;
 		}else{
-			sonar_alt		= min(sonar_alt, 600);
 			current_loc.alt = sonar_alt + home.alt;
 		}
 
 	}else{
-
 		// no sonar altitude
 		current_loc.alt = baro_alt + home.alt;
 	}
+
 	//Serial.printf("b_alt: %ld, home: %ld ", baro_alt, home.alt);
 #endif
 

ArduCopterMega/Attitude.pde

@@ -45,7 +45,7 @@ output_stabilize_roll()
 	error 		= constrain(error, -2500, 2500);
 
 	// only buildup I if we are trying to roll hard
-	if(abs(g.rc_1.servo_out) < 1500){
+	if(abs(g.rc_1.servo_out) < 1000){
 		// smoother alternative to reset?
 		//if(g.pid_stabilize_roll.kI() != 0){
 		//	g.pid_stabilize_roll.kI(g.pid_stabilize_roll.kI() * .8);
@@ -54,7 +54,7 @@ output_stabilize_roll()
 	}
 
 	// write out angles back to servo out - this will be converted to PWM by RC_Channel
-	g.rc_1.servo_out 	= g.pid_stabilize_roll.get_pid(error,  	delta_ms_fast_loop, 1.0);
+	g.rc_1.servo_out 	= g.pid_stabilize_roll.get_pid(error,  	delta_ms_fast_loop, 1.0);		// 2500 * .7 = 1750
 
 	// We adjust the output by the rate of rotation:
 	// Rate control through bias corrected gyro rates
@@ -99,9 +99,11 @@ void
 clear_yaw_control()
 {
 	//Serial.print("Clear ");
-	rate_yaw_flag  	= false;			// exit rate_yaw_flag
-	nav_yaw 		= dcm.yaw_sensor;	// save our Yaw
-	yaw_error 		= 0;
+	rate_yaw_flag  		= false;			// exit rate_yaw_flag
+	nav_yaw 			= dcm.yaw_sensor;	// save our Yaw
+	g.rc_4.servo_out 	= 0;				// reset our output. It can stick when we are at 0 throttle
+	yaw_error 			= 0;
+	yaw_debug 			= YAW_HOLD; 		//0
 }
 
 void
@@ -138,40 +140,38 @@ output_yaw_with_hold(boolean hold)
 
 	if(hold){
 		// try and hold the current nav_yaw setting
-		yaw_error			= nav_yaw - dcm.yaw_sensor; 									// +- 60°
-		yaw_error 			= wrap_180(yaw_error);
+		yaw_error				= nav_yaw - dcm.yaw_sensor; 									// +- 60°
+		yaw_error 				= wrap_180(yaw_error);
 
 		// limit the error we're feeding to the PID
-		yaw_error			= constrain(yaw_error,	 -6000, 6000);						// limit error to 60 degees
+		yaw_error				= constrain(yaw_error,	 -6000, 6000);						// limit error to 60 degees
 
 		// Apply PID and save the new angle back to RC_Channel
-		g.rc_4.servo_out 	= g.pid_yaw.get_pid(yaw_error, delta_ms_fast_loop, 1.0); 		// .5 * 6000 = 3000
+		g.rc_4.servo_out 		= g.pid_yaw.get_pid(yaw_error, delta_ms_fast_loop, 1.0); 		// .5 * 6000 = 3000
 
 		// add in yaw dampener
-		g.rc_4.servo_out	-= constrain(dampener, -1800, 1800);
-		yaw_debug = YAW_HOLD; //0
+		g.rc_4.servo_out		-= constrain(dampener, -2400, 2400);
+		yaw_debug 				= YAW_HOLD; //0
 
 	}else{
 
 		if(g.rc_4.control_in == 0){
-			// we are breaking;
-			//g.rc_4.servo_out = (omega.z > 0) ? -600 : 600;
+
 			// adaptive braking
-			g.rc_4.servo_out 	= (int)((-1800.0 * omega.z) / 1);
-								//	-1800 * 0.925 / 6 = -277
+			g.rc_4.servo_out 	= (int)((-3000.0 * omega.z) / 1);
 
-			yaw_debug = YAW_BRAKE;  // 1
+			yaw_debug 			= YAW_BRAKE;  // 1
 
 		}else{
 			// RATE control
 			long error			= ((long)g.rc_4.control_in * 6) - rate;							// control is += 6000 * 6 = 36000
 			g.rc_4.servo_out 	= g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 1.0);	// kP .07 * 36000 = 2520
-			yaw_debug = YAW_RATE;  // 2
+			yaw_debug 			= YAW_RATE;  // 2
 		}
 	}
 
 	// Limit Output
-	g.rc_4.servo_out 	= constrain(g.rc_4.servo_out, -1800, 1800);								// limit to 24°
+	g.rc_4.servo_out 	= constrain(g.rc_4.servo_out, -2400, 2400);								// limit to 24°
 
 	//Serial.printf("%d\n",g.rc_4.servo_out);
 }
@@ -243,10 +243,10 @@ void calc_nav_throttle()
 	}
 
 	if(altitude_sensor == BARO){
-		nav_throttle = g.pid_baro_throttle.get_pid(error, delta_ms_fast_loop, scaler);
+		nav_throttle = g.pid_baro_throttle.get_pid(error, delta_ms_fast_loop, scaler);	// .25
 		nav_throttle = g.throttle_cruise + constrain(nav_throttle, -30, 80);
 	}else{
-		nav_throttle = g.pid_sonar_throttle.get_pid(error, delta_ms_fast_loop, scaler);
+		nav_throttle = g.pid_sonar_throttle.get_pid(error, delta_ms_fast_loop, scaler);	// .5
 		nav_throttle = g.throttle_cruise + constrain(nav_throttle, -60, 100);
 	}
 

ArduCopterMega/Log.pde

@@ -362,61 +362,54 @@ void Log_Write_Cmd(byte num, struct Location *wp)
 	DataFlash.WriteByte(num);
 	DataFlash.WriteByte(wp->id);
 	DataFlash.WriteByte(wp->p1);
+	DataFlash.WriteByte(wp->options);
 	DataFlash.WriteLong(wp->alt);
 	DataFlash.WriteLong(wp->lat);
 	DataFlash.WriteLong(wp->lng);
 	DataFlash.WriteByte(END_BYTE);
 }
 
-// Write a control tuning packet. Total length : 22 bytes
-#if HIL_MODE != HIL_MODE_ATTITUDE
-void Log_Write_Control_Tuning()
+void Log_Write_Nav_Tuning()
 {
 	DataFlash.WriteByte(HEAD_BYTE1);
 	DataFlash.WriteByte(HEAD_BYTE2);
-	DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG);
-
-	// Control
-	DataFlash.WriteInt((int)(g.rc_3.control_in));
-	DataFlash.WriteInt((int)(g.rc_3.servo_out));
-	DataFlash.WriteInt((int)(g.rc_4.control_in));
-	DataFlash.WriteInt((int)(g.rc_4.servo_out));
-	DataFlash.WriteInt((int)yaw_error);
-
+	DataFlash.WriteByte(LOG_NAV_TUNING_MSG);
 
-	// Yaw mode
-	DataFlash.WriteByte(yaw_debug);
+	DataFlash.WriteInt((int)(dcm.yaw_sensor/100));		// 1
+	DataFlash.WriteInt((int)wp_distance);				// 2
+	DataFlash.WriteInt((int)(target_bearing/100));		// 3
+	DataFlash.WriteInt((int)(nav_bearing/100));			// 4
 
-	// Gyro Rates
-	DataFlash.WriteInt((int)(omega.x * 1000));
-	DataFlash.WriteInt((int)(omega.y * 1000));
-	DataFlash.WriteInt((int)(omega.z * 1000));
+	DataFlash.WriteByte(altitude_sensor);				// 5
+	DataFlash.WriteInt((int)sonar_alt);					// 6
+	DataFlash.WriteInt((int)baro_alt);					// 7
 
-	// Position
-	DataFlash.WriteInt((int)dcm.pitch_sensor);
-	DataFlash.WriteInt((int)dcm.roll_sensor);
-	DataFlash.WriteInt((int)(dcm.yaw_sensor/10));
+	DataFlash.WriteInt(home.alt);						// 8
+	DataFlash.WriteInt((int)next_WP.alt);				// 9
+	DataFlash.WriteInt((int)altitude_error);			// 10
 
 	DataFlash.WriteByte(END_BYTE);
 }
-#endif
 
-// Write a navigation tuning packet. Total length : 18 bytes
-void Log_Write_Nav_Tuning()
+void Log_Read_Nav_Tuning()
 {
-	DataFlash.WriteByte(HEAD_BYTE1);
-	DataFlash.WriteByte(HEAD_BYTE2);
-	DataFlash.WriteByte(LOG_NAV_TUNING_MSG);
-	DataFlash.WriteInt((uint16_t)dcm.yaw_sensor);
-	DataFlash.WriteInt((int)wp_distance);
-	DataFlash.WriteInt((uint16_t)target_bearing);
-	DataFlash.WriteInt((uint16_t)nav_bearing);
-	DataFlash.WriteInt(altitude_error);
-	DataFlash.WriteInt((int)airspeed);
-	DataFlash.WriteInt((int)(nav_gain_scaler*1000));
-	DataFlash.WriteByte(END_BYTE);
+							 //	1	2	3	4	5	6	7	8	9	10
+	Serial.printf_P(PSTR("NTUN, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n"),
+				DataFlash.ReadInt(),	//yaw sensor
+				DataFlash.ReadInt(),	//distance
+				DataFlash.ReadInt(),	//target bearing
+				DataFlash.ReadInt(),	//nav bearing
+
+				DataFlash.ReadByte(),	//Alt sensor
+				DataFlash.ReadInt(),	//Sonar
+				DataFlash.ReadInt(),	//Baro
+
+				DataFlash.ReadInt(),	//home.alt
+				DataFlash.ReadInt(),	//Next_alt
+				DataFlash.ReadInt());	//Alt Error
 }
 
+
 // Write a mode packet. Total length : 5 bytes
 void Log_Write_Mode(byte mode)
 {
@@ -428,24 +421,48 @@ void Log_Write_Mode(byte mode)
 }
 
 // Write an GPS packet. Total length : 30 bytes
-void Log_Write_GPS(	long log_Time, long log_Lattitude, long log_Longitude, long log_mix_alt, long log_gps_alt,
-					long log_Ground_Speed, long log_Ground_Course, byte log_Fix, byte log_NumSats)
+void Log_Write_GPS()
 {
 	DataFlash.WriteByte(HEAD_BYTE1);
 	DataFlash.WriteByte(HEAD_BYTE2);
 	DataFlash.WriteByte(LOG_GPS_MSG);
-	DataFlash.WriteLong(log_Time);
-	DataFlash.WriteByte(log_Fix);
-	DataFlash.WriteByte(log_NumSats);
-	DataFlash.WriteLong(log_Lattitude);
-	DataFlash.WriteLong(log_Longitude);
-	DataFlash.WriteLong(log_mix_alt);
-	DataFlash.WriteLong(log_gps_alt);
-	DataFlash.WriteLong(log_Ground_Speed);
-	DataFlash.WriteLong(log_Ground_Course);
+
+	DataFlash.WriteLong(g_gps->time);						// 1
+	DataFlash.WriteByte(g_gps->fix);						// 2
+	DataFlash.WriteByte(g_gps->num_sats);					// 3
+
+	DataFlash.WriteLong(current_loc.lat);					// 4
+	DataFlash.WriteLong(current_loc.lng);					// 5
+	DataFlash.WriteLong(g_gps->altitude);					// 6
+	DataFlash.WriteLong(current_loc.alt);					// 7
+
+	DataFlash.WriteInt(g_gps->ground_speed);				// 8
+	DataFlash.WriteInt((int)(g_gps->ground_course/100));	// 9
+
 	DataFlash.WriteByte(END_BYTE);
 }
 
+// Read a GPS packet
+void Log_Read_GPS()
+{						//	1	2	3 4				5			6		 7			8        9
+					//	GPS, t, 1, 8, 37.7659070, -122.4329400, 57.0500, 58.1400, 658.8400, -11636846.0000
+							//	1   2   3   4      5		6	 7		 8		9
+	Serial.printf_P(PSTR("GPS, %ld, %d, %d, %4.7f, %4.7f, %4.4f, %4.4f, %4.4f, %4.4f\n"),
+
+			DataFlash.ReadLong(),					// 1 time
+			(int)DataFlash.ReadByte(),				// 2 fix
+			(int)DataFlash.ReadByte(),				// 3 sats
+
+			(float)DataFlash.ReadLong() / t7,		// 4 lat
+			(float)DataFlash.ReadLong() / t7,		// 5 lon
+			(float)DataFlash.ReadLong() / 100.0,	// 6 gps alt
+			(float)DataFlash.ReadLong() / 100.0,	// 7 sensor alt
+
+			(float)DataFlash.ReadInt() / 100.0,		// 8 ground speed
+			(float)DataFlash.ReadInt());			// 9 ground course
+}
+
+
 // Write an raw accel/gyro data packet. Total length : 28 bytes
 #if HIL_MODE != HIL_MODE_ATTITUDE
 void Log_Write_Raw()
@@ -491,10 +508,47 @@ void Log_Read_Current()
 			DataFlash.ReadInt());
 }
 
+
+// Write a control tuning packet. Total length : 22 bytes
+#if HIL_MODE != HIL_MODE_ATTITUDE
+void Log_Write_Control_Tuning()
+{
+	DataFlash.WriteByte(HEAD_BYTE1);
+	DataFlash.WriteByte(HEAD_BYTE2);
+	DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG);
+
+	// Control
+	DataFlash.WriteInt((int)(g.rc_3.control_in));
+	DataFlash.WriteInt((int)(g.rc_3.servo_out));
+	DataFlash.WriteInt((int)(g.rc_4.control_in));
+	DataFlash.WriteInt((int)(g.rc_4.servo_out));
+	DataFlash.WriteInt((int)yaw_error);
+
+	// Yaw mode
+	DataFlash.WriteByte(yaw_debug);
+
+	// Gyro Rates
+	DataFlash.WriteInt((int)(omega.x * 1000));
+	DataFlash.WriteInt((int)(omega.y * 1000));
+	DataFlash.WriteInt((int)(omega.z * 1000));
+
+	DataFlash.WriteInt((int)g.throttle_cruise);
+	DataFlash.WriteInt((int)g.pid_baro_throttle.get_integrator());
+	DataFlash.WriteInt((int)g.pid_sonar_throttle.get_integrator());
+
+	// Position
+	//DataFlash.WriteInt((int)dcm.pitch_sensor);
+	//DataFlash.WriteInt((int)dcm.roll_sensor);
+	//DataFlash.WriteInt((int)(dcm.yaw_sensor/10));
+
+	DataFlash.WriteByte(END_BYTE);
+}
+#endif
+
 // Read an control tuning packet
 void Log_Read_Control_Tuning()
 {
-	Serial.printf_P(PSTR("CTUN, %d, %d, %d, %d, %d, %d, %1.4f, %1.4f, %1.4f, %d, %d, %ld\n"),
+	Serial.printf_P(PSTR("CTUN, %d, %d, %d, %d, %d, %d, %1.4f, %1.4f, %1.4f, %d, %d, %d\n"),
 				// Control
 				DataFlash.ReadInt(),
 				DataFlash.ReadInt(),
@@ -512,24 +566,17 @@ void Log_Read_Control_Tuning()
 				(float)DataFlash.ReadInt() / 1000.0,
 
 				// Position
+				//DataFlash.ReadInt(),
+				//DataFlash.ReadInt(),
+				//(long)DataFlash.ReadInt() * 10);
+
+				// Alt Hold
 				DataFlash.ReadInt(),
 				DataFlash.ReadInt(),
-				(long)DataFlash.ReadInt() * 10);
+				DataFlash.ReadInt());
 
 }
 
-// Read a nav tuning packet
-void Log_Read_Nav_Tuning()
-{
-	Serial.printf_P(PSTR("NTUN: %4.4f, %d, %4.4f, %4.4f, %4.4f, %4.4f, %4.4f,\n"),
-				(float)((uint16_t)DataFlash.ReadInt())/100.0,
-				DataFlash.ReadInt(),
-				(float)((uint16_t)DataFlash.ReadInt())/100.0,
-				(float)((uint16_t)DataFlash.ReadInt())/100.0,
-				(float)DataFlash.ReadInt()/100.0,
-				(float)DataFlash.ReadInt()/100.0,
-				(float)DataFlash.ReadInt()/1000.0);
-}
 
 // Read a performance packet
 void Log_Read_Performance()
@@ -560,12 +607,12 @@ void Log_Read_Cmd()
 	long logvarl;
 
 	Serial.printf_P(PSTR("CMD:"));
-	for(int i = 1; i < 4; i++) {
+	for(int i = 1; i <= 4; i++) {
 		logvarb = DataFlash.ReadByte();
 		Serial.print(logvarb, DEC);
 		Serial.print(comma);
 	}
-	for(int i = 1; i < 4; i++) {
+	for(int i = 1; i <= 3; i++) {
 		logvarl = DataFlash.ReadLong();
 		Serial.print(logvarl, DEC);
 		Serial.print(comma);
@@ -589,22 +636,6 @@ void Log_Read_Mode()
 	Serial.println(flight_mode_strings[DataFlash.ReadByte()]);
 }
 
-// Read a GPS packet
-void Log_Read_GPS()
-{
-	Serial.printf_P(PSTR("GPS: %ld, %d, %d, %4.7f, %4.7f, %4.4f, %4.4f, %4.4f, %4.4f\n"),
-			DataFlash.ReadLong(),
-			(int)DataFlash.ReadByte(),
-			(int)DataFlash.ReadByte(),
-			(float)DataFlash.ReadLong() / t7,
-			(float)DataFlash.ReadLong() / t7,
-			(float)DataFlash.ReadLong() / 100.0,
-			(float)DataFlash.ReadLong() / 100.0,
-			(float)DataFlash.ReadLong() / 100.0,
-			(float)DataFlash.ReadLong() / 100.0);
-
-}
-
 // Read a raw accel/gyro packet
 void Log_Read_Raw()
 {

ArduCopterMega/commands.pde

@@ -15,6 +15,10 @@ void init_auto()
 		Serial.println("ia_f");
 		do_RTL();
 	}
+
+	// initialize commands
+	// -------------------
+	init_commands();
 }
 
 // this is only used by an air-start
@@ -76,7 +80,6 @@ struct Location get_wp_with_index(int i)
 
 	// this is a hack for now, until we get GUI support
 	yaw_tracking = TRACK_NEXT_WP;
-
 	return temp;
 }
 
@@ -222,6 +225,8 @@ void init_home()
 	// -------------------
 	set_wp_with_index(home, 0);
 
+	print_wp(&home, 0);
+
 	// Save prev loc
 	// -------------
 	next_WP = prev_WP = home;

ArduCopterMega/commands_logic.pde

@@ -126,7 +126,7 @@ bool handle_no_commands()
 
 bool verify_must()
 {
-	Serial.printf("vmust ::%d", nav_throttle);
+	//Serial.printf("vmust: %d\n", command_must_ID);
 
 	switch(command_must_ID) {
 
@@ -219,11 +219,12 @@ void do_RTL(void)
 void do_takeoff()
 {
 	Location temp		= current_loc;
-	temp.alt			+= next_command.alt;
+	temp.alt			= next_command.alt;
+
 	takeoff_complete	= false;			// set flag to use g_gps ground course during TO.  IMU will be doing yaw drift correction
 
-	Serial.print("dt ");
-	Serial.println(temp.alt,DEC);
+	//Serial.print("dt ");
+	//Serial.println(temp.alt, DEC);
 
 	set_next_WP(&temp);
 }
@@ -294,7 +295,7 @@ void do_loiter_time()
 
 bool verify_takeoff()
 {
-	Serial.print("vt ");
+	//Serial.printf("vt c_alt:%ld, n_alt:%ld\n", current_loc.alt, next_WP.alt);
 
 	// wait until we are ready!
 	if(g.rc_3.control_in == 0)

ArduCopterMega/commands_process.pde

@@ -35,6 +35,8 @@ void load_next_command_from_EEPROM()
 	// -----------------------------------------------------
 	if(next_command.id == NO_COMMAND){
 		next_command = get_wp_with_index(g.waypoint_index + 1);
+		//Serial.println("AA");
+		print_wp(&next_command, g.waypoint_index + 1);
 	}
 
 	// If the preload failed, return or just Loiter
@@ -61,6 +63,7 @@ void process_next_command()
 			SendDebug("MSG <pnc> new c_must_id ");
 			SendDebug(next_command.id,DEC);
 			SendDebug(" index:");
+
 			SendDebugln(command_must_index,DEC);
 			if (g.log_bitmask & MASK_LOG_CMD)
 				Log_Write_Cmd(g.waypoint_index, &next_command);
@@ -104,7 +107,8 @@ void process_must()
 {
 	//gcs.send_text_P(SEVERITY_LOW,PSTR("New cmd: <process_must>"));
 	//gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
-	Serial.printf("pmst %d\n", (int)next_command.id);
+	//Serial.printf("pmst %d\n", (int)next_command.id);
+	print_wp(&next_command, g.waypoint_index);
 
 	// clear May indexes
 	command_may_index	= NO_COMMAND;

ArduCopterMega/config.h

@@ -349,7 +349,7 @@
 # define YAW_I					0.0			// Always 0
 #endif
 #ifndef  YAW_D
-# define YAW_D					0.175		//
+# define YAW_D					0.05		// Trying a lower value to prevent odd behavior
 #endif
 #ifndef  YAW_IMAX
 # define YAW_IMAX				0			//	Always 0
@@ -399,7 +399,7 @@
 
 
 #ifndef THROTTLE_SONAR_P
-# define THROTTLE_SONAR_P		.5
+# define THROTTLE_SONAR_P		.65		// upped a hair from .5
 #endif
 #ifndef THROTTLE_SONAR_I
 # define THROTTLE_SONAR_I		0.1

ArduCopterMega/radio.pde

@@ -14,7 +14,7 @@ void init_rc_in()
 	g.rc_2.set_angle(4500);
 	g.rc_3.set_range(0,1000);
 	g.rc_3.scale_output = .9;
-	g.rc_4.set_angle(6000);
+	g.rc_4.set_angle(4500);
 
 	// set rc dead zones
 	g.rc_1.dead_zone = 60;		// 60 = .6 degrees

ArduCopterMega/system.pde

@@ -17,9 +17,9 @@ extern int8_t	test_mode(uint8_t argc, const Menu::arg *argv);		// in test.cpp
 static int8_t	main_menu_help(uint8_t argc, const Menu::arg *argv)
 {
 	Serial.printf_P(PSTR("Commands:\n"
-						 "  logs        log readback/setup mode\n"
-						 "  setup       setup mode\n"
-						 "  test        test mode\n"
+						 "  logs\n"
+						 "  setup\n"
+						 "  test\n"
 						 "\n"
 						 "Move the slide switch and reset to FLY.\n"
 						 "\n"));
@@ -171,7 +171,7 @@ void init_ardupilot()
 	pinMode(SLIDE_SWITCH_PIN, INPUT);	// To enter interactive mode
 	pinMode(PUSHBUTTON_PIN, INPUT);		// unused
 	DDRL |= B00000100;					// Set Port L, pin 2 to output for the relay
-	
+
 	// Logging:
 	// --------
 	DataFlash.Init(); 	// DataFlash log initialization
@@ -224,7 +224,7 @@ void init_ardupilot()
 //********************************************************************************
 void startup_ground(void)
 {
-	gcs.send_text_P(SEVERITY_LOW,PSTR("<startup_ground> GROUND START"));
+	gcs.send_text_P(SEVERITY_LOW,PSTR("GROUND START"));
 
 	#if(GROUND_START_DELAY > 0)
 		//gcs.send_text_P(SEVERITY_LOW, PSTR(" With Delay"));
@@ -275,9 +275,10 @@ void startup_ground(void)
 			break;
 	    }
 	}
+
 	report_gps();
 	SendDebug("\nReady to FLY ");
-	gcs.send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY."));
+	//gcs.send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY."));
 }
 
 void set_mode(byte mode)

ArduCopterMega/test.pde

@@ -134,14 +134,12 @@ test_radio(uint8_t argc, const Menu::arg *argv)
 	while(1){
 		delay(20);
 		read_radio();
-		output_manual_throttle();
+		//output_manual_throttle();
+		//g.rc_1.calc_pwm();
+		//g.rc_2.calc_pwm();
+		//g.rc_4.calc_pwm();
 
-		g.rc_1.calc_pwm();
-		g.rc_2.calc_pwm();
-		g.rc_3.calc_pwm();
-		g.rc_4.calc_pwm();
-
-		Serial.printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\n"),
+		Serial.printf_P(PSTR("IN  1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\n"),
 							g.rc_1.control_in,
 							g.rc_2.control_in,
 							g.rc_3.control_in,