diff --git a/ArduCopterMega/APM_Config.h b/ArduCopterMega/APM_Config.h
index 885dcfaae2..ccbf2562ab 100644
--- a/ArduCopterMega/APM_Config.h
+++ b/ArduCopterMega/APM_Config.h
@@ -4,13 +4,17 @@
 
 // GPS is auto-selected
 
+#define MAG_ORIENTATION		AP_COMPASS_COMPONENTS_DOWN_PINS_BACK
+
+#define GPS_PROTOCOL GPS_PROTOCOL_MTK
+
 #define GCS_PROTOCOL        GCS_PROTOCOL_NONE
 //#define GCS_PORT 0
 
 #define SERIAL0_BAUD			38400
 
-//# define STABILIZE_ROLL_P 		0.4
-//# define STABILIZE_PITCH_P		0.4
+# define STABILIZE_ROLL_P 		0.75
+# define STABILIZE_PITCH_P		0.75
 //# define STABILIZE_DAMPENER		0.1
 
 
@@ -23,4 +27,14 @@
 // Logging
 //#define LOG_CURRENT ENABLED
 
+# define LOG_ATTITUDE_FAST		DISABLED
+# define LOG_ATTITUDE_MED 		DISABLED
+# define LOG_GPS 				DISABLED
+# define LOG_PM 				DISABLED
+# define LOG_CTUN				ENABLED
+# define LOG_NTUN				DISABLED
+# define LOG_MODE				DISABLED
+# define LOG_RAW				DISABLED
+# define LOG_CMD				DISABLED
+# define LOG_CURRENT			DISABLED
 
diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde
index b684771aad..a069f0cb26 100644
--- a/ArduCopterMega/ArduCopterMega.pde
+++ b/ArduCopterMega/ArduCopterMega.pde
@@ -231,6 +231,7 @@ int 	max_stabilize_dampener;				//
 int 	max_yaw_dampener;					//
 boolean rate_yaw_flag;						// used to transition yaw control from Rate control to Yaw hold
 byte 	yaw_debug;
+bool 	did_clear_yaw_control;
 
 // LED output
 // ----------
@@ -583,6 +584,7 @@ void medium_loop()
 			// we call these regardless of GPS because of the rapid nature of the yaw sensor
 			// -----------------------------------------------------------------------------
 			if(wp_distance < 800){ // 8 meters
+			//if(g.rc_6.control_in > 500){ // 8 meters
 				calc_loiter_nav();
 			}else{
 				calc_waypoint_nav();
@@ -795,6 +797,11 @@ void update_GPS(void)
 	g_gps->update();
 	update_GPS_light();
 
+	//current_loc.lng =   377697000;		// Lon * 10 * *7
+	//current_loc.lat = -1224318000;		// Lat * 10 * *7
+	//current_loc.alt = 100;				// alt * 10 * *7
+	//return;
+
     if (g_gps->new_data && g_gps->fix) {
 
 		// XXX We should be sending GPS data off one of the regular loops so that we send
@@ -822,9 +829,6 @@ void update_GPS(void)
 			}else{
 				//Serial.printf("init Home!");
 
-				if (g.log_bitmask & MASK_LOG_CMD)
-					Log_Write_Startup(TYPE_GROUNDSTART_MSG);
-
 				// reset our nav loop timer
 				//nav_loopTimer = millis();
 				init_home();
@@ -980,14 +984,7 @@ void update_current_flight_mode(void)
 				nav_pitch 		= 0;
 				nav_roll 		= 0;
 
-				//if(g.rc_3.control_in)
-				// get desired height from the throttle
-				//next_WP.alt 	= home.alt + (g.rc_3.control_in); // 0 - 1000 (40 meters)
-				//next_WP.alt		= max(next_WP.alt, 30);
-
 				adjust_altitude();
-				// !!! testing
-				//next_WP.alt 	-= 500;
 
 				// Yaw control
 				// -----------
diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde
index ff543370c5..4193b4ed99 100644
--- a/ArduCopterMega/Attitude.pde
+++ b/ArduCopterMega/Attitude.pde
@@ -13,6 +13,11 @@ init_pids()
 void
 control_nav_mixer()
 {
+	// limit the nav pitch and roll of the copter
+	long pmax = g.pitch_max.get();
+	nav_roll 	= constrain(nav_roll,  -pmax, pmax);
+	nav_pitch 	= constrain(nav_pitch, -pmax, pmax);
+
 	// control +- 45° is mixed with the navigation request by the Autopilot
 	// output is in degrees = target pitch and roll of copter
 	g.rc_1.servo_out = g.rc_1.control_mix(nav_roll);
@@ -149,7 +154,9 @@ output_yaw_with_hold(boolean hold)
 		if(g.rc_4.control_in == 0){
 			// we are breaking;
 			//g.rc_4.servo_out = (omega.z > 0) ? -600 : 600;
-			g.rc_4.servo_out 	= (int)((float)g.rc_4.servo_out * (omega.z / 6.0));
+			// adaptive braking
+			g.rc_4.servo_out 	= (int)((1800.0 * omega.z) / 6.0);
+
 			yaw_debug = YAW_BRAKE;
 
 		}else{
@@ -160,8 +167,8 @@ output_yaw_with_hold(boolean hold)
 	}
 
 	// Limit dampening to be equal to propotional term for symmetry
-	//dampener 			= rate * g.hold_yaw_dampener;											// 34377 * .175 = 6000
-	//g.rc_4.servo_out	-= constrain(dampener, -1800, 1800);
+	dampener 			= rate * g.hold_yaw_dampener;											// 34377 * .175 = 6000
+	g.rc_4.servo_out	-= constrain(dampener, -1800, 1800);
 
 	// Limit Output
 	g.rc_4.servo_out 	= constrain(g.rc_4.servo_out, -1800, 1800);								// limit to 24°
@@ -263,7 +270,11 @@ yaw control
 void output_manual_yaw()
 {
 	if(g.rc_3.control_in == 0){
-		clear_yaw_control();
+		// we want to only call this once
+		if(did_clear_yaw_control == false){
+			clear_yaw_control();
+			did_clear_yaw_control = true;
+		}
 	}else{
 		// Yaw control
 		if(g.rc_4.control_in == 0){
@@ -271,6 +282,8 @@ void output_manual_yaw()
 		}else{
 			output_yaw_with_hold(false); // rate control yaw
 		}
+
+		did_clear_yaw_control = false;
 	}
 }
 
diff --git a/ArduCopterMega/Log.pde b/ArduCopterMega/Log.pde
index 66e040d4f8..5f673f2d04 100644
--- a/ArduCopterMega/Log.pde
+++ b/ArduCopterMega/Log.pde
@@ -77,9 +77,8 @@ print_log_menu(void)
 	if (last_log_num == 0) {
 		Serial.printf_P(PSTR("\nNo logs\n"));
 	}else{
-
 		Serial.printf_P(PSTR("\n%d logs\n"), last_log_num);
-		for(int i=1;i<last_log_num+1;i++) {
+		for(int i = 1; i < last_log_num + 1; i++) {
 			get_log_boundaries(last_log_num, i, log_start, log_end);
 			Serial.printf_P(PSTR("Log # %d,    start %d,   end %d\n"),
 							i, log_start, log_end);
@@ -112,7 +111,7 @@ dump_log(uint8_t argc, const Menu::arg *argv)
 				  dump_log_end);
 
 	Log_Read(dump_log_start, dump_log_end);
-	Serial.printf_P(PSTR("Complete\n"));
+	Serial.printf_P(PSTR("Done\n"));
 }
 
 static int8_t
@@ -227,31 +226,36 @@ void start_new_log(byte num_existing_logs)
 	int end_pages[50]	= {0,0,0};
 	byte data;
 
-	if(num_existing_logs > 0) {
+	if (num_existing_logs > 0) {
 		for(int i=0;i<num_existing_logs;i++) {
 			get_log_boundaries(num_existing_logs, i+1,start_pages[i],end_pages[i]);
 		}
 		end_pages[num_existing_logs - 1] = find_last_log_page(start_pages[num_existing_logs - 1]);
 	}
 
-	if(end_pages[num_existing_logs - 1] < 4095 && num_existing_logs < MAX_NUM_LOGS) {
+	if (end_pages[num_existing_logs - 1] < 4095 && num_existing_logs < MAX_NUM_LOGS) {
 		if(num_existing_logs > 0)
 			start_pages[num_existing_logs] = end_pages[num_existing_logs - 1] + 1;
 		else
 			start_pages[0] = 2;
+
 		num_existing_logs++;
+
 		DataFlash.StartWrite(1);
 		DataFlash.WriteByte(HEAD_BYTE1);
 		DataFlash.WriteByte(HEAD_BYTE2);
 		DataFlash.WriteByte(LOG_INDEX_MSG);
 		DataFlash.WriteByte(num_existing_logs);
+
 		for(int i=0;i<MAX_NUM_LOGS;i++) {
 			DataFlash.WriteInt(start_pages[i]);
 			DataFlash.WriteInt(end_pages[i]);
 		}
+
 		DataFlash.WriteByte(END_BYTE);
 		DataFlash.FinishWrite();
 		DataFlash.StartWrite(start_pages[num_existing_logs-1]);
+
 	}else{
 		gcs.send_text_P(SEVERITY_LOW,PSTR("<start_new_log> Logs full"));
 	}
@@ -364,26 +368,6 @@ void Log_Write_Cmd(byte num, struct Location *wp)
 	DataFlash.WriteByte(END_BYTE);
 }
 
-void Log_Write_Startup(byte type)
-{
-	DataFlash.WriteByte(HEAD_BYTE1);
-	DataFlash.WriteByte(HEAD_BYTE2);
-	DataFlash.WriteByte(LOG_STARTUP_MSG);
-	DataFlash.WriteByte(type);
-	DataFlash.WriteByte(g.waypoint_total);
-	DataFlash.WriteByte(END_BYTE);
-
-	// create a location struct to hold the temp Waypoints for printing
-	struct Location cmd = get_wp_with_index(0);
-		Log_Write_Cmd(0, &cmd);
-
-	for (int i = 1; i <= g.waypoint_total; i++){
-		cmd = get_wp_with_index(i);
-		Log_Write_Cmd(i, &cmd);
-	}
-}
-
-
 // Write a control tuning packet. Total length : 22 bytes
 #if HIL_MODE != HIL_MODE_ATTITUDE
 void Log_Write_Control_Tuning()
@@ -585,20 +569,6 @@ void Log_Read_Cmd()
 	Serial.println(" ");
 }
 
-void Log_Read_Startup()
-{
-	byte logbyte = DataFlash.ReadByte();
-
-	if (logbyte == TYPE_AIRSTART_MSG)
-		Serial.printf_P(PSTR("AIR START - "));
-	else if (logbyte == TYPE_GROUNDSTART_MSG)
-		Serial.printf_P(PSTR("GROUND START - "));
-	else
-		Serial.printf_P(PSTR("UNKNOWN STARTUP - "));
-
-	Serial.printf_P(PSTR(" %d commands in memory\n"),(int)DataFlash.ReadByte());
-}
-
 // Read an attitude packet
 void Log_Read_Attitude()
 {
@@ -648,26 +618,30 @@ void Log_Read_Raw()
 void Log_Read(int start_page, int end_page)
 {
 	byte data;
-	byte log_step = 0;
-	int packet_count = 0;
-	int page = start_page;
-
+	byte log_step 		= 0;
+	int packet_count 	= 0;
+	int page 			= start_page;
 
 	DataFlash.StartRead(start_page);
+
 	while (page < end_page && page != -1){
+
 		data = DataFlash.ReadByte();
-		switch(log_step)		 // This is a state machine to read the packets
-			{
+
+		// This is a state machine to read the packets
+		switch(log_step){
 			case 0:
 				if(data == HEAD_BYTE1)	// Head byte 1
 					log_step++;
 				break;
+
 			case 1:
 				if(data == HEAD_BYTE2)	// Head byte 2
 					log_step++;
 				else
 					log_step = 0;
 				break;
+
 			case 2:
 				if(data == LOG_ATTITUDE_MSG){
 					Log_Read_Attitude();
@@ -702,30 +676,32 @@ void Log_Read(int start_page, int end_page)
 					log_step++;
 
 				}else if(data == LOG_STARTUP_MSG){
-					Log_Read_Startup();
+					// not implemented
 					log_step++;
-				}else {
-					if(data == LOG_GPS_MSG){
-						Log_Read_GPS();
-						log_step++;
-					}else{
-						Serial.printf_P(PSTR("Error Reading Packet: %d\n"),packet_count);
-						log_step = 0;	 // Restart, we have a problem...
-					}
+
+				}else if(data == LOG_GPS_MSG){
+					Log_Read_GPS();
+					log_step++;
+
+				}else{
+					Serial.printf_P(PSTR("Error P: %d\n"),packet_count);
+					log_step = 0;	 // Restart, we have a problem...
 				}
 				break;
+
 			case 3:
 				if(data == END_BYTE){
 					 packet_count++;
 				}else{
-					Serial.printf_P(PSTR("Error Reading END_BYTE: %d\n"),data);
+					Serial.printf_P(PSTR("Error EB: %d\n"),data);
 				}
 				log_step = 0;			// Restart sequence: new packet...
 				break;
-			}
-		page = DataFlash.GetPage();
 		}
-	Serial.printf_P(PSTR("# of packets read: %d\n"), packet_count);
+
+		page = DataFlash.GetPage();
+	}
+	//Serial.printf_P(PSTR("# of packets read: %d\n"), packet_count);
 }
 
 
diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h
index 8607700301..eda88d3feb 100644
--- a/ArduCopterMega/config.h
+++ b/ArduCopterMega/config.h
@@ -255,6 +255,14 @@
 //////////////////////////////////////////////////////////////////////////////
 //////////////////////////////////////////////////////////////////////////////
 
+
+//////////////////////////////////////////////////////////////////////////////
+// Y6 Support
+
+#ifndef Y6_MOTOR_SCALER
+# define Y6_MOTOR_SCALER	0.92
+#endif
+
 //////////////////////////////////////////////////////////////////////////////
 // ACRO Rate Control
 
@@ -312,7 +320,7 @@
 # define STABILIZE_ROLL_I 		0.1		//
 #endif
 #ifndef STABILIZE_ROLL_D
-# define STABILIZE_ROLL_D 		0.11
+# define STABILIZE_ROLL_D 		0.13
 #endif
 #ifndef STABILIZE_ROLL_IMAX
 # define STABILIZE_ROLL_IMAX 	10		// 10 degrees
@@ -325,13 +333,12 @@
 # define STABILIZE_PITCH_I		0.1
 #endif
 #ifndef STABILIZE_PITCH_D
-# define STABILIZE_PITCH_D		0.11
+# define STABILIZE_PITCH_D		0.13
 #endif
 #ifndef STABILIZE_PITCH_IMAX
 # define STABILIZE_PITCH_IMAX	10
 #endif
 
-
 //////////////////////////////////////////////////////////////////////////////
 // YAW Control
 //
diff --git a/ArduCopterMega/motors.pde b/ArduCopterMega/motors.pde
index c062a24cea..6e3ab4c055 100644
--- a/ArduCopterMega/motors.pde
+++ b/ArduCopterMega/motors.pde
@@ -59,7 +59,7 @@ set_servos_4()
 		g.rc_3.servo_out 	= constrain(g.rc_3.servo_out, 0, 1000);
 
 		if(g.rc_3.servo_out > 0)
-			out_min = g.rc_3.radio_min + 60;
+			out_min = g.rc_3.radio_min + 90;
 
 		//Serial.printf("out: %d %d %d %d\t\t", g.rc_1.servo_out, g.rc_2.servo_out, g.rc_3.servo_out, g.rc_4.servo_out);
 
@@ -71,7 +71,8 @@ set_servos_4()
 
 		// limit Yaw control so we don't clip and loose altitude
 		// this is only a partial solution.
-		//g.rc_4.pwm_out = min(g.rc_4.pwm_out, (g.rc_3.radio_out - out_min));
+
+		// g.rc_4.pwm_out = min(g.rc_4.pwm_out, (g.rc_3.radio_out - out_min));
 
 		//Serial.printf("out: %d %d %d %d\n", g.rc_1.radio_out, g.rc_2.radio_out, g.rc_3.radio_out, g.rc_4.radio_out);
 		//Serial.printf("yaw: %d ", g.rc_4.radio_out);
@@ -163,15 +164,15 @@ set_servos_4()
 			int pitch_out 		=  g.rc_2.pwm_out / 2;
 
 			//left
-			motor_out[CH_2]		= ((g.rc_3.radio_out * 0.92) + roll_out + pitch_out);  // CCW TOP
+			motor_out[CH_2]		= ((g.rc_3.radio_out * Y6_MOTOR_SCALER) + roll_out + pitch_out);  // CCW TOP
 			motor_out[CH_3]		=  g.rc_3.radio_out + roll_out + pitch_out;			// CW
 
 			//right
-			motor_out[CH_7]		= ((g.rc_3.radio_out * 0.92) - roll_out + pitch_out);	// CCW TOP
+			motor_out[CH_7]		= ((g.rc_3.radio_out * Y6_MOTOR_SCALER) - roll_out + pitch_out);	// CCW TOP
 			motor_out[CH_1]		=  g.rc_3.radio_out - roll_out + pitch_out;			// CW
 
 			//back
-			motor_out[CH_8]     = ((g.rc_3.radio_out * 0.92) - g.rc_2.pwm_out);	// CCW TOP
+			motor_out[CH_8]     = ((g.rc_3.radio_out * Y6_MOTOR_SCALER) - g.rc_2.pwm_out);	// CCW TOP
 			motor_out[CH_4] 	=  g.rc_3.radio_out - g.rc_2.pwm_out;			// CW
 
 			//yaw
@@ -223,6 +224,13 @@ set_servos_4()
 			//g.pid_baro_throttle.kD((float)g.rc_6.control_in / 1000); //  0 to 1
 			//g.pid_baro_throttle.kP((float)g.rc_6.control_in / 4000); //  0 to .25
 
+
+			// uncomment me out to try in flight dampening, 0 = unflyable, .2 = unfun, .13 worked for me.
+			// use test,radio to get the value to use in your config.
+			//g.stabilize_dampener.set((float)g.rc_6.control_in / 1000.0);
+
+
+
 			/*
 			// ROLL and PITCH
 			// make sure you init_pids() after changing the kP
@@ -239,16 +247,41 @@ set_servos_4()
 			init_pids();
 			//*/
 
+			/*
+			Serial.printf("yaw: %d, lat_e: %ld, lng_e: %ld, \tnlat: %ld, nlng: %ld,\tnrll: %ld, nptc: %ld, \tcx: %.2f, sy: %.2f, \ttber: %ld, \tnber: %ld\n",
+					(int)(dcm.yaw_sensor / 100),
+					lat_error,
+					long_error,
+					nav_lat,
+					nav_lon,
+					nav_roll,
+					nav_pitch,
+					cos_yaw_x,
+					sin_yaw_y,
+					target_bearing,
+					nav_bearing);
+			//*/
+
 			/*
 
 			gcs_simple.write_byte(control_mode);
+			//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)(dcm.yaw_sensor 	/ 100));
+
 			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);
 
+			//gcs_simple.write_int((int)(cos_yaw_x * 100));
+			//gcs_simple.write_int((int)(sin_yaw_y * 100));
+
 			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
@@ -257,10 +290,9 @@ 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));
+			gcs_simple.write_int((int)(target_bearing 	/ 100));
+			gcs_simple.write_int((int)(nav_bearing 		/ 100));
+			gcs_simple.write_int((int)(nav_yaw 			/ 100));
 
 			if(altitude_sensor == BARO){
 				gcs_simple.write_int((int)g.pid_baro_throttle.get_integrator());
@@ -269,17 +301,15 @@ set_servos_4()
 			}
 
 			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));
-
+			gcs_simple.write_int(g.throttle_cruise);
 
 			//24
-
 			gcs_simple.flush(10); // Message ID
+
 			//*/
+			//Serial.printf("\n tb  %d\n", (int)(target_bearing 	/ 100));
+			//Serial.printf("\n nb  %d\n", (int)(nav_bearing 	/ 100));
+			//Serial.printf("\n dcm %d\n", (int)(dcm.yaw_sensor 	/ 100));
 
 			/*Serial.printf("a %ld, e %ld, i %d, t %d, b %4.2f\n",
 					current_loc.alt,
diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde
index f01e1798e2..e570980ba1 100644
--- a/ArduCopterMega/navigation.pde
+++ b/ArduCopterMega/navigation.pde
@@ -108,10 +108,6 @@ void calc_loiter_nav()
 					//EAST   -1000 * -1			+ 1000 *  0 	=  1000	// pitch back
 					//SOUTH  -1000 *  0			+ 1000 * -1 	= -1000	// pitch forward
 
-	long pmax = g.pitch_max.get();
-
-	nav_roll 	= constrain(nav_roll,  -pmax, pmax);
-	nav_pitch 	= constrain(nav_pitch, -pmax, pmax);
 }
 
 void calc_waypoint_nav()
@@ -129,9 +125,6 @@ void calc_waypoint_nav()
 	nav_roll 	=  (float)nav_lat * cos_nav_x;
 	nav_pitch 	= -(float)nav_lat * sin_nav_y;
 
-	long pmax = g.pitch_max.get();
-	nav_roll 	= constrain(nav_roll,  -pmax, pmax);
-	nav_pitch 	= constrain(nav_pitch, -pmax, pmax);
 }
 
 void calc_bearing_error()
diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde
index 4c1719d30a..023d653a94 100644
--- a/ArduCopterMega/system.pde
+++ b/ArduCopterMega/system.pde
@@ -41,10 +41,6 @@ MENU(main_menu, "ACM", main_menu_commands);
 
 void init_ardupilot()
 {
-	byte last_log_num;
-	int last_log_start;
-	int last_log_end;
-
 	// Console serial port
 	//
 	// The console port buffers are defined to be sufficiently large to support
@@ -67,9 +63,9 @@ void init_ardupilot()
 	// XXX the 128 byte receive buffer may be too small for NMEA, depending
 	// on the message set configured.
 	//
-#if GPS_PROTOCOL != GPS_PROTOCOL_IMU
+	#if GPS_PROTOCOL != GPS_PROTOCOL_IMU
 	Serial1.begin(38400, 128, 16);
-#endif
+	#endif
 
 	// Telemetry port.
 	//
@@ -81,8 +77,8 @@ void init_ardupilot()
 	//
 	Serial3.begin(SERIAL3_BAUD, 128, 128);
 
-	Serial.printf_P(PSTR("\n\nInit ArduCopterMega"
-						 "\n\nFree RAM: %lu\n"),
+	Serial.printf_P(PSTR("\n\nInit ACM"
+						 "\n\nRAM: %lu\n"),
 						 freeRAM());
 
 
@@ -131,21 +127,22 @@ void init_ardupilot()
 	  //  Serial.printf_P(PSTR("using %u bytes of memory\n"), AP_Var::get_memory_use());
 	}
 
-#ifdef RADIO_OVERRIDE_DEFAULTS
+
+	#ifdef RADIO_OVERRIDE_DEFAULTS
 	{
 		int16_t rc_override[8] = RADIO_OVERRIDE_DEFAULTS;
 		APM_RC.setHIL(rc_override);
 	}
-#endif
+	#endif
 
 	init_rc_in();		// sets up rc channels from radio
 	init_rc_out();		// sets up the timer libs
 	init_camera();
-#if HIL_MODE != HIL_MODE_ATTITUDE
+
+	#if HIL_MODE != HIL_MODE_ATTITUDE
 	adc.Init();	 		// APM ADC library initialization
 	barometer.Init();	// APM Abs Pressure sensor initialization
-#endif
-	DataFlash.Init(); 	// DataFlash log initialization
+	#endif
 
 	// Do GPS init
 	//g_gps = &GPS;
@@ -159,18 +156,14 @@ void init_ardupilot()
 		gcs.init(&Serial);
 	#endif
 
-
-	if (g.log_bitmask & MASK_LOG_MODE)
-		Log_Write_Mode(control_mode);
-
 	if(g.compass_enabled)
 		init_compass();
 
-#if HIL_MODE != HIL_MODE_ATTITUDE
+	#if HIL_MODE != HIL_MODE_ATTITUDE
 	if(g.sonar_enabled){
 		sonar.init(SONAR_PIN, &adc);
 	}
-#endif
+	#endif
 
 	pinMode(C_LED_PIN, OUTPUT);			// GPS status LED
 	pinMode(A_LED_PIN, OUTPUT);			// GPS status LED
@@ -198,33 +191,30 @@ void init_ardupilot()
 		}
 	}
 
-	if(g.log_bitmask > 0){
-		//	TODO - Here we will check  on the length of the last log
-		//  We don't want to create a bunch of little logs due to powering on and off
-		last_log_num = get_num_logs();
-		start_new_log(last_log_num);
-	}
-
-	// read in the flight switches
-	update_servo_switches();
-
-	// read in the flight switches
-	//update_servo_switches();
-
-	//imu.init_gyro(IMU::WARM_START);
-
+	// All of the Gyro calibrations
+	// ----------------------------
 	startup_ground();
 
-	if (g.log_bitmask & MASK_LOG_CMD)
-		Log_Write_Startup(TYPE_GROUNDSTART_MSG);
-
-	if (g.log_bitmask & MASK_LOG_SET_DEFAULTS) {
-		default_log_bitmask();
-	}
-
 	// set the correct flight mode
 	// ---------------------------
 	reset_control_switch();
+
+	// Logging:
+	// --------
+	DataFlash.Init(); 	// DataFlash log initialization
+	// setup the log bitmask
+	if (g.log_bitmask & MASK_LOG_SET_DEFAULTS)
+		default_log_bitmask();
+
+	if(g.log_bitmask != 0){
+		//	TODO - Here we will check  on the length of the last log
+		//  We don't want to create a bunch of little logs due to powering on and off
+		byte last_log_num = get_num_logs();
+		start_new_log(last_log_num);
+	}
+
+	if (g.log_bitmask & MASK_LOG_MODE)
+		Log_Write_Mode(control_mode);
 }
 
 //********************************************************************************
@@ -257,14 +247,6 @@ void startup_ground(void)
 	report_imu();
 #endif
 
-	// read the radio to set trims
-	// ---------------------------
-	//trim_radio();
-
-	if (g.log_bitmask & MASK_LOG_CMD)
-		Log_Write_Startup(TYPE_GROUNDSTART_MSG);
-
-
 #if HIL_MODE != HIL_MODE_ATTITUDE
 	// read Baro pressure at ground
 	//-----------------------------