diff --git a/ArduCopterMega/APM_Config.h b/ArduCopterMega/APM_Config.h
index b38ffe28f5..885dcfaae2 100644
--- a/ArduCopterMega/APM_Config.h
+++ b/ArduCopterMega/APM_Config.h
@@ -4,7 +4,7 @@
 
 // GPS is auto-selected
 
-//#define GCS_PROTOCOL        GCS_PROTOCOL_NONE
+#define GCS_PROTOCOL        GCS_PROTOCOL_NONE
 //#define GCS_PORT 0
 
 #define SERIAL0_BAUD			38400
diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde
index f9c84fbbcd..ce6a222165 100644
--- a/ArduCopterMega/ArduCopterMega.pde
+++ b/ArduCopterMega/ArduCopterMega.pde
@@ -42,6 +42,7 @@ version 2.1 of the License, or (at your option) any later version.
 
 #define MAVLINK_COMM_NUM_BUFFERS 2
 #include <GCS_MAVLink.h>    // MAVLink GCS definitions
+
 //#include <GCS_SIMPLE.h>
 
 
@@ -767,7 +768,7 @@ void slow_loop()
 // 1Hz loop
 void super_slow_loop()
 {
-	if (g.log_bitmask & MASK_LOG_CUR)
+	if (g.log_bitmask & MASK_LOG_CURRENT)
 		Log_Write_Current();
 
     gcs.send_message(MSG_HEARTBEAT); // XXX This is running at 3 1/3 Hz instead of 1 Hz
@@ -1162,7 +1163,10 @@ adjust_altitude()
 			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 = min((current_loc.alt + 400), next_WP.alt);		// don't go more than 4 meters below current location
+			//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 = current_loc.alt + 400;
+			}
 		}
 	}
 }
\ No newline at end of file
diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde
index 1e741cdeea..40f46142bf 100644
--- a/ArduCopterMega/Attitude.pde
+++ b/ArduCopterMega/Attitude.pde
@@ -41,6 +41,10 @@ output_stabilize_roll()
 
 	// only buildup I if we are trying to roll hard
 	if(abs(g.rc_1.servo_out) < 1500){
+		// smoother alternative to reset?
+		//if(g.pid_stabilize_roll.kI() != 0){
+		//	g.pid_stabilize_roll.kI(g.pid_stabilize_roll.kI() * .8);
+		//}
 		g.pid_stabilize_roll.reset_I();
 	}
 
@@ -126,18 +130,18 @@ 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
 
 		// We adjust the output by the rate of rotation
-		long rate		= degrees(omega.z) * 100.0; 									// 3rad = 17188 , 6rad = 34377
-		int dampener 	= ((float)rate * g.hold_yaw_dampener);						// 18000 * .17 = 3000
+		long rate			= degrees(omega.z) * 100.0; 									// 3rad = 17188 , 6rad = 34377
+		int dampener 		= (float)rate * g.hold_yaw_dampener;						// 18000 * .17 = 3000
 
 		// Limit dampening to be equal to propotional term for symmetry
 		g.rc_4.servo_out	-= constrain(dampener, -max_yaw_dampener, max_yaw_dampener); 	// -3000
@@ -155,15 +159,26 @@ output_yaw_with_hold(boolean hold)
 			// we are breaking;
 			g.rc_4.servo_out = (omega.z > 0) ? -1800 : 1800;
 
+			//g.rc_4.servo_out = (int)((float)g.rc_4.servo_out * (omega.z / 6.0));
+
 			//switch comments to get old behavior.
 			//g.rc_4.servo_out = g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 1.0);// kP .07 * 36000 = 2520
 
 		}else{
-			g.rc_4.servo_out = g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 1.0);// kP .07 * 36000 = 2520
+			g.rc_4.servo_out 	= g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 1.0); // kP .07 * 36000 = 2520
+
+			// We adjust the output by the rate of rotation
+			//long rate			= degrees(omega.z) * 100.0; 									// 3rad = 17188 , 6rad = 34377
+			//int dampener 		= (float)rate * g.hold_yaw_dampener;							// 18000 * .17 = 3000
+
+			// Limit dampening to be equal to propotional term for symmetry
+			//g.rc_4.servo_out	-= constrain(dampener, -max_yaw_dampener, max_yaw_dampener); 	// -3000
+			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, -1800, 1800);						// limit to 24°
 	}
+
+	//Serial.printf("%d\n",g.rc_4.servo_out);
 }
 
 // slight left rudder gives right roll.
diff --git a/ArduCopterMega/Log.pde b/ArduCopterMega/Log.pde
index 472752be71..4adb106522 100644
--- a/ArduCopterMega/Log.pde
+++ b/ArduCopterMega/Log.pde
@@ -69,7 +69,7 @@ print_log_menu(void)
 		PLOG(MODE);
 		PLOG(RAW);
 		PLOG(CMD);
-		PLOG(CUR);
+		PLOG(CURRENT);
 		#undef PLOG
 	}
 	Serial.println();
@@ -166,7 +166,7 @@ select_logs(uint8_t argc, const Menu::arg *argv)
 		TARG(MODE);
 		TARG(RAW);
 		TARG(CMD);
-		TARG(CUR);
+		TARG(CURRENT);
 		#undef TARG
 	}
 
diff --git a/ArduCopterMega/commands_logic.pde b/ArduCopterMega/commands_logic.pde
index 5c6a4aa610..7b31bbdb19 100644
--- a/ArduCopterMega/commands_logic.pde
+++ b/ArduCopterMega/commands_logic.pde
@@ -212,8 +212,8 @@ void do_RTL(void)
 	// output control mode to the ground station
 	gcs.send_message(MSG_HEARTBEAT);
 
-	if (g.log_bitmask & MASK_LOG_MODE)
-		Log_Write_Mode(control_mode);
+	//if (g.log_bitmask & MASK_LOG_MODE)
+	//	Log_Write_Mode(control_mode);
 }
 
 void do_takeoff()
diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h
index f7e81c63d9..6c27610f96 100644
--- a/ArduCopterMega/config.h
+++ b/ArduCopterMega/config.h
@@ -270,7 +270,6 @@
 #ifndef ACRO_RATE_ROLL_IMAX
 # define ACRO_RATE_ROLL_IMAX	 20
 #endif
-# define ACRO_RATE_ROLL_IMAX_CENTIDEGREE ACRO_RATE_ROLL_IMAX * 100
 
 #ifndef ACRO_RATE_PITCH_P
 # define ACRO_RATE_PITCH_P         .190
@@ -284,10 +283,9 @@
 #ifndef ACRO_RATE_PITCH_IMAX
 # define ACRO_RATE_PITCH_IMAX   20
 #endif
-#define ACRO_RATE_PITCH_IMAX_CENTIDEGREE ACRO_RATE_PITCH_IMAX * 100
 
 #ifndef ACRO_RATE_YAW_P
-# define ACRO_RATE_YAW_P         .2			// used to control response in turning
+# define ACRO_RATE_YAW_P         .1			// used to control response in turning
 #endif
 #ifndef ACRO_RATE_YAW_I
 # define ACRO_RATE_YAW_I         0.0
@@ -298,7 +296,6 @@
 #ifndef ACRO_RATE_YAW_IMAX
 # define ACRO_RATE_YAW_IMAX   0
 #endif
-# define ACRO_RATE_YAW_IMAX_CENTIDEGREE ACRO_RATE_YAW_IMAX * 100
 
 #ifndef ACRO_RATE_TRIGGER
 # define ACRO_RATE_TRIGGER   0
@@ -315,7 +312,7 @@
 # define STABILIZE_ROLL_I 		0.1		//
 #endif
 #ifndef STABILIZE_ROLL_D
-# define STABILIZE_ROLL_D 		0.135
+# define STABILIZE_ROLL_D 		0.11
 #endif
 #ifndef STABILIZE_ROLL_IMAX
 # define STABILIZE_ROLL_IMAX 	10		// 10 degrees
@@ -328,7 +325,7 @@
 # define STABILIZE_PITCH_I		0.1
 #endif
 #ifndef STABILIZE_PITCH_D
-# define STABILIZE_PITCH_D		0.135
+# define STABILIZE_PITCH_D		0.11
 #endif
 #ifndef STABILIZE_PITCH_IMAX
 # define STABILIZE_PITCH_IMAX	10
@@ -384,10 +381,10 @@
 # define THROTTLE_BARO_P		0.25
 #endif
 #ifndef THROTTLE_BARO_I
-# define THROTTLE_BARO_I		0.01
+# define THROTTLE_BARO_I		0.04
 #endif
 #ifndef THROTTLE_BARO_D
-# define THROTTLE_BARO_D		0.2
+# define THROTTLE_BARO_D		0.0	// lowered to 0 to debug effects
 #endif
 #ifndef THROTTLE_BARO_IMAX
 # define THROTTLE_BARO_IMAX		50
@@ -398,7 +395,7 @@
 # define THROTTLE_SONAR_P		.3
 #endif
 #ifndef THROTTLE_SONAR_I
-# define THROTTLE_SONAR_I		0.01
+# define THROTTLE_SONAR_I		0.1
 #endif
 #ifndef THROTTLE_SONAR_D
 # define THROTTLE_SONAR_D		0.03
diff --git a/ArduCopterMega/control_modes.pde b/ArduCopterMega/control_modes.pde
index c8ec3efd94..14ef06f277 100644
--- a/ArduCopterMega/control_modes.pde
+++ b/ArduCopterMega/control_modes.pde
@@ -80,7 +80,7 @@ void read_trim_switch()
 				#endif
 			}else{
 				// set the throttle nominal
-				if(g.rc_3.control_in > 200){
+				if(g.rc_3.control_in > 150){
 					g.throttle_cruise.set_and_save(g.rc_3.control_in);
 					//Serial.printf("tnom %d\n", g.throttle_cruise.get());
 				}
diff --git a/ArduCopterMega/defines.h b/ArduCopterMega/defines.h
index 24cada92f7..515dbc52ba 100644
--- a/ArduCopterMega/defines.h
+++ b/ArduCopterMega/defines.h
@@ -212,7 +212,7 @@
 #define MASK_LOG_MODE			(1<<6)
 #define MASK_LOG_RAW			(1<<7)
 #define MASK_LOG_CMD			(1<<8)
-#define MASK_LOG_CUR			(1<<9)
+#define MASK_LOG_CURRENT		(1<<9)
 #define MASK_LOG_SET_DEFAULTS	(1<<15)
 
 // Waypoint Modes
diff --git a/ArduCopterMega/motors.pde b/ArduCopterMega/motors.pde
index 548e0cca23..90e82a9e88 100644
--- a/ArduCopterMega/motors.pde
+++ b/ArduCopterMega/motors.pde
@@ -9,21 +9,29 @@ void arm_motors()
 
 	// Arm motor output : Throttle down and full yaw right for more than 2 seconds
 	if (g.rc_3.control_in == 0){
-		if (g.rc_4.control_in > 2700) {
-			if (arming_counter > ARM_DELAY) {
-				motor_armed = true;
+		// full right
+		if (g.rc_4.control_in > 4000) {
+			if (arming_counter >= ARM_DELAY) {
+				motor_armed 	= true;
+				arming_counter 	= ARM_DELAY;
 			} else{
 				arming_counter++;
 			}
-		}else if (g.rc_4.control_in < -2700) {
-			if (arming_counter > DISARM_DELAY){
-				motor_armed = false;
+		// full left
+		}else if (g.rc_4.control_in < -4000) {
+			if (arming_counter >= DISARM_DELAY){
+				motor_armed 	= false;
+				arming_counter 	= DISARM_DELAY;
 			}else{
 				arming_counter++;
 			}
+		// centered
 		}else{
 			arming_counter = 0;
 		}
+
+	}else{
+		arming_counter = 0;
 	}
 }
 
@@ -186,7 +194,7 @@ set_servos_4()
 		}
 
 		num++;
-		if (num > 50){
+		if (num > 25){
 			num = 0;
 			/*
 			Serial.printf("t_alt:%ld, alt:%ld, thr: %d sen: ",
@@ -224,24 +232,15 @@ set_servos_4()
 			//*/
 
 			/*
-			gcs_simple.write_int(motor_out[CH_1]);
-			gcs_simple.write_int(motor_out[CH_2]);
-			gcs_simple.write_int(motor_out[CH_3]);
-			gcs_simple.write_int(motor_out[CH_4]);
-			gcs_simple.write_int(g.rc_3.servo_out);
 
-			gcs_simple.write_int((int)(cos_yaw_x * 100));
-			gcs_simple.write_int((int)(sin_yaw_y * 100));
-			gcs_simple.write_int((int)(dcm.yaw_sensor / 100));
-			gcs_simple.write_int((int)(nav_yaw / 100));
+			gcs_simple.write_byte(control_mode);
+			gcs_simple.write_int(g.rc_3.servo_out);
 
 			gcs_simple.write_int((int)nav_lat);
 			gcs_simple.write_int((int)nav_lon);
-
 			gcs_simple.write_int((int)nav_roll);
 			gcs_simple.write_int((int)nav_pitch);
 
-			//24
 			gcs_simple.write_long(current_loc.lat);	//28
 			gcs_simple.write_long(current_loc.lng);	//32
 			gcs_simple.write_int((int)current_loc.alt);	//34
@@ -250,6 +249,27 @@ set_servos_4()
 			gcs_simple.write_long(next_WP.lng);
 			gcs_simple.write_int((int)next_WP.alt);		//44
 
+			gcs_simple.write_int((int)(target_bearing / 100));
+			gcs_simple.write_int((int)(nav_bearing / 100));
+			gcs_simple.write_int((int)(nav_yaw / 100));
+			gcs_simple.write_int((int)(dcm.yaw_sensor / 100));
+
+			if(altitude_sensor == BARO){
+				gcs_simple.write_int((int)g.pid_baro_throttle.get_integrator());
+			}else{
+				gcs_simple.write_int((int)g.pid_sonar_throttle.get_integrator());
+			}
+
+			gcs_simple.write_int(g.throttle_cruise);
+
+
+			//gcs_simple.write_int((int)(cos_yaw_x * 100));
+			//gcs_simple.write_int((int)(sin_yaw_y * 100));
+			//gcs_simple.write_int((int)(nav_yaw / 100));
+
+
+			//24
+
 			gcs_simple.flush(10); // Message ID
 			//*/
 
diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde
index 7738044d14..fdc5bdee0c 100644
--- a/ArduCopterMega/setup.pde
+++ b/ArduCopterMega/setup.pde
@@ -107,8 +107,8 @@ setup_factory(uint8_t argc, const Menu::arg *argv)
 	Serial.printf_P(PSTR("\nFACTORY RESET complete - reboot APM"));
 
 	delay(1000);
-	default_log_bitmask();
-	default_gains();
+	//default_log_bitmask();
+	//default_gains();
 
 	for (;;) {
 	}
@@ -683,7 +683,7 @@ void default_log_bitmask()
 {
 
 	// convenience macro for testing LOG_* and setting LOGBIT_*
-	#define LOGBIT(_s)	(MASK_LOG_##_s ? MASK_LOG_##_s : 0)
+	#define LOGBIT(_s)	(LOG_##_s ? MASK_LOG_##_s : 0)
 
 	g.log_bitmask =
 		LOGBIT(ATTITUDE_FAST)	|
@@ -695,7 +695,7 @@ void default_log_bitmask()
 		LOGBIT(MODE)			|
 		LOGBIT(RAW)				|
 		LOGBIT(CMD)				|
-		LOGBIT(CUR);
+		LOGBIT(CURRENT);
 	#undef LOGBIT
 
 	g.log_bitmask.save();
diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde
index 54b37ca9b3..3ac5447dae 100644
--- a/ArduCopterMega/system.pde
+++ b/ArduCopterMega/system.pde
@@ -292,6 +292,10 @@ void set_mode(byte mode)
 	control_mode = mode;
 	control_mode = constrain(control_mode, 0, NUM_MODES - 1);
 
+	// XXX temporary
+	//if (g.log_bitmask & MASK_LOG_MODE)
+		Log_Write_Mode(control_mode);
+
 	// used to stop fly_aways
 	if(g.rc_1.control_in == 0){
 		// we are on the ground is this is true
@@ -481,8 +485,11 @@ init_simple_bearing()
 void
 init_throttle_cruise()
 {
-	if(set_throttle_cruise_flag == false && g.rc_3.control_in > 250){
-		set_throttle_cruise_flag = true;
-		g.throttle_cruise.set_and_save(g.rc_3.control_in);
+	if(set_throttle_cruise_flag == false){
+		if(g.rc_3.control_in > 200){
+			//set_throttle_cruise_flag = true;
+			g.throttle_cruise.set_and_save(g.rc_3.control_in);
+			//Serial.printf("throttle_cruise: %d\n", g.throttle_cruise.get());
+		}
 	}
 }
\ No newline at end of file