diff --git a/ArduCopterMega/APM_Config.h b/ArduCopterMega/APM_Config.h
index 7566b24a5f..a9e8b19fd7 100644
--- a/ArduCopterMega/APM_Config.h
+++ b/ArduCopterMega/APM_Config.h
@@ -6,11 +6,9 @@
 
 #define GCS_PROTOCOL        GCS_PROTOCOL_NONE
 
-//#define MAGORIENTATION	AP_COMPASS_COMPONENTS_UP_PINS_BACK
-#define MAGORIENTATION		AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
+//#define MAG_ORIENTATION	AP_COMPASS_COMPONENTS_UP_PINS_BACK
 
 # define SERIAL0_BAUD			38400
-//# define SERIAL0_BAUD			115200
 
 //# define STABILIZE_ROLL_P 		0.4
 //# define STABILIZE_PITCH_P		0.4
@@ -24,15 +22,6 @@
 
 
 
-// For future development, don't enable unless you know them
-// These are all experimental and underwork, jp 23-12-10
-//#define ENABLE_EXTRAS     ENABLED
-//#define ENABLE_EXTRAINIT  ENABLED
-//#define ENABLE_CAM        ENABLED
-//#define ENABLE_AM         ENABLED
-//#define ENABLE_xx         ENABLED
-
- 
 // Logging
 //#define LOG_CURRENT ENABLED
 
diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde
index 0c62688d9b..3aee45bbfe 100644
--- a/ArduCopterMega/ArduCopterMega.pde
+++ b/ArduCopterMega/ArduCopterMega.pde
@@ -91,43 +91,52 @@ GPS         *g_gps;
 
 #if HIL_MODE == HIL_MODE_NONE
 
-// real sensors
-AP_ADC_ADS7844          adc;
-APM_BMP085_Class        barometer;
-AP_Compass_HMC5843      compass(Parameters::k_param_compass);
+	// real sensors
+	AP_ADC_ADS7844          adc;
+	APM_BMP085_Class        barometer;
+	AP_Compass_HMC5843      compass(Parameters::k_param_compass);
 
-// real GPS selection
-#if   GPS_PROTOCOL == GPS_PROTOCOL_AUTO
-AP_GPS_Auto     g_gps_driver(&Serial1, &g_gps);
-#elif GPS_PROTOCOL == GPS_PROTOCOL_NMEA
-AP_GPS_NMEA     g_gps_driver(&Serial1);
-#elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF
-AP_GPS_SIRF     g_gps_driver(&Serial1);
-#elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX
-AP_GPS_UBLOX    g_gps_driver(&Serial1);
-#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK
-AP_GPS_MTK      g_gps_driver(&Serial1);
-#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK16
-AP_GPS_MTK16    g_gps_driver(&Serial1);
-#elif GPS_PROTOCOL == GPS_PROTOCOL_NONE
-AP_GPS_None     g_gps_driver(NULL);
-#else
- #error Unrecognised GPS_PROTOCOL setting.
-#endif // GPS PROTOCOL
+	// real GPS selection
+	#if   GPS_PROTOCOL == GPS_PROTOCOL_AUTO
+		AP_GPS_Auto     g_gps_driver(&Serial1, &g_gps);
+
+	#elif GPS_PROTOCOL == GPS_PROTOCOL_NMEA
+		AP_GPS_NMEA     g_gps_driver(&Serial1);
+
+	#elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF
+		AP_GPS_SIRF     g_gps_driver(&Serial1);
+
+	#elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX
+		AP_GPS_UBLOX    g_gps_driver(&Serial1);
+
+	#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK
+		AP_GPS_MTK      g_gps_driver(&Serial1);
+
+	#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK16
+		AP_GPS_MTK16    g_gps_driver(&Serial1);
+
+	#elif GPS_PROTOCOL == GPS_PROTOCOL_NONE
+		AP_GPS_None     g_gps_driver(NULL);
+
+	#else
+		#error Unrecognised GPS_PROTOCOL setting.
+	#endif // GPS PROTOCOL
 
 #elif HIL_MODE == HIL_MODE_SENSORS
-// sensor emulators
-AP_ADC_HIL              adc;
-APM_BMP085_HIL_Class    barometer;
-AP_Compass_HIL  compass;
-AP_GPS_HIL              g_gps_driver(NULL);
+	// sensor emulators
+	AP_ADC_HIL              adc;
+	APM_BMP085_HIL_Class    barometer;
+	AP_Compass_HIL          compass;
+	AP_GPS_HIL              g_gps_driver(NULL);
 
 #elif HIL_MODE == HIL_MODE_ATTITUDE
-AP_DCM_HIL dcm;
-AP_GPS_HIL              g_gps_driver(NULL);
+	AP_DCM_HIL              dcm;
+	AP_GPS_HIL              g_gps_driver(NULL);
+	AP_Compass_HIL          compass; // never used
+	AP_IMU_Shim             imu; // never used
 
 #else
- #error Unrecognised HIL_MODE setting.
+	#error Unrecognised HIL_MODE setting.
 #endif // HIL MODE
 
 #if HIL_MODE != HIL_MODE_DISABLED
@@ -152,10 +161,10 @@ AP_GPS_HIL              g_gps_driver(NULL);
 ////////////////////////////////////////////////////////////////////////////////
 //
 #if   GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
-GCS_MAVLINK         gcs;
+	GCS_MAVLINK         gcs;
 #else
-// If we are not using a GCS, we need a stub that does nothing.
-GCS_Class           gcs;
+	// If we are not using a GCS, we need a stub that does nothing.
+	GCS_Class           gcs;
 #endif
 
 AP_RangeFinder_MaxsonarXL sonar;
@@ -165,8 +174,6 @@ AP_RangeFinder_MaxsonarXL sonar;
 ////////////////////////////////////////////////////////////////////////////////
 
 byte 	control_mode		= STABILIZE;
-boolean failsafe			= false;		// did our throttle dip below the failsafe value?
-boolean ch3_failsafe		= false;
 byte 	oldSwitchPosition;					// for remembering the control mode switch
 
 const char *comma = ",";
@@ -177,6 +184,7 @@ const char* flight_mode_strings[] = {
 	"ALT_HOLD",
 	"FBW",
 	"AUTO",
+	"LOITER",
 	"POSITION_HOLD",
 	"RTL",
 	"TAKEOFF",
@@ -189,7 +197,7 @@ const char* flight_mode_strings[] = {
 			3	Throttle
 			4	Rudder (if we have ailerons)
 			5	Mode - 3 position switch
-			6 	Altitude for Hold, user assignable
+			6 	User assignable
 			7	trainer switch - sets throttle nominal (toggle switch), sets accels to Level (hold > 1 second)
 			8	TBD
 */
@@ -199,7 +207,15 @@ const char* flight_mode_strings[] = {
 int motor_out[4];
 Vector3f omega;
 
+// Failsafe
+// --------
+boolean 	failsafe;						// did our throttle dip below the failsafe value?
+boolean 	ch3_failsafe;
+boolean		motor_armed;
+boolean		motor_auto_safe;
+
 // PIDs
+// ----
 int 	max_stabilize_dampener;				//
 int 	max_yaw_dampener;					//
 boolean rate_yaw_flag;						// used to transition yaw control from Rate control to Yaw hold
@@ -209,8 +225,6 @@ boolean rate_yaw_flag;						// used to transition yaw control from Rate control
 boolean motor_light;						// status of the Motor safety
 boolean GPS_light;							// status of the GPS light
 
-float       nav_gain_scaler = 1;        // Gain scaling for headwind/tailwind TODO: why does this variable need to be initialized to 1?
-
 // GPS variables
 // -------------
 const 	float t7			= 10000000.0;	// used to scale GPS values for EEPROM storage
@@ -218,51 +232,41 @@ float 	scaleLongUp			= 1;			// used to reverse longtitude scaling
 float 	scaleLongDown 		= 1;			// used to reverse longtitude scaling
 byte 	ground_start_count	= 5;			// have we achieved first lock and set Home?
 
-// Magnetometer variables
-// ----------------------
-Vector3f	mag_offsets;
-
 // Location & Navigation
 // ---------------------
-const   float radius_of_earth 	= 6378100;	// meters
-const   float gravity 			= 9.81;		// meters/ sec^2
-//byte 	wp_radius				= 3;		// meters
+const	float radius_of_earth 	= 6378100;	// meters
+const	float gravity 			= 9.81;		// meters/ sec^2
 long	nav_bearing;						// deg * 100 : 0 to 360 current desired bearing to navigate
-long 	target_bearing;						// deg * 100 : 0 to 360 location of the plane to the target
-long 	crosstrack_bearing;					// deg * 100 : 0 to 360 desired angle of plane to target
-int 	climb_rate;							// m/s * 100  - For future implementation of controlled ascent/descent by rate
+long	target_bearing;						// deg * 100 : 0 to 360 location of the plane to the target
+long	crosstrack_bearing;					// deg * 100 : 0 to 360 desired angle of plane to target
+int		climb_rate;							// m/s * 100  - For future implementation of controlled ascent/descent by rate
+float	nav_gain_scaler 		= 1;		// Gain scaling for headwind/tailwind TODO: why does this variable need to be initialized to 1?
 
 byte	command_must_index;					// current command memory location
 byte	command_may_index;					// current command memory location
 byte	command_must_ID;					// current command ID
 byte	command_may_ID;						// current command ID
 
-float cos_roll_x, 	sin_roll_y;
-float cos_pitch_x,	sin_pitch_y;
-float sin_yaw_y,	cos_yaw_x;
+float cos_roll_x 	= 1;
+float cos_pitch_x 	= 1;
+float cos_yaw_x 	= 1;
+float sin_pitch_y, sin_yaw_y, sin_roll_y;
 
 // Airspeed
 // --------
-int 	airspeed;							// m/s * 100
-float 	airspeed_error;						// m / s * 100
-
-// Throttle Failsafe
-// ------------------
-boolean		motor_armed 		= false;
-boolean		motor_auto_safe 	= false;
+int		airspeed;							// m/s * 100
 
 // Location Errors
 // ---------------
-long 	bearing_error;						// deg * 100 : 0 to 36000
-long 	altitude_error;						// meters * 100 we are off in altitude
+long	bearing_error;						// deg * 100 : 0 to 36000
+long	altitude_error;						// meters * 100 we are off in altitude
 float	crosstrack_error;					// meters we are off trackline
 long 	distance_error;						// distance to the WP
 long 	yaw_error;							// how off are we pointed
+long	long_error, lat_error;				// temp for debugging
 
-long long_error, lat_error;					// temp fro debugging
-
-// Sensors
-// -------
+// Battery Sensors
+// ---------------
 float	battery_voltage		= LOW_VOLTAGE * 1.05;		// Battery Voltage of total battery, initialized above threshold for filter
 float 	battery_voltage1 	= LOW_VOLTAGE * 1.05;		// Battery Voltage of cell 1, initialized above threshold for filter
 float 	battery_voltage2 	= LOW_VOLTAGE * 1.05;		// Battery Voltage of cells 1 + 2, initialized above threshold for filter
@@ -272,19 +276,9 @@ float 	battery_voltage4 	= LOW_VOLTAGE * 1.05;		// Battery Voltage of cells 1 +
 float 	current_voltage 	= LOW_VOLTAGE * 1.05;		// Battery Voltage of cells 1 + 2+3 + 4, initialized above threshold for filter
 float	current_amps;
 float	current_total;
-//int 	milliamp_hours;
-//boolean	current_enabled		= false;
 
-// Magnetometer variables
-// ----------------------
-//int 	magnetom_x;
-//int 	magnetom_y;
-//int 	magnetom_z;
-
-//float 	mag_offset_x;
-//float 	mag_offset_y;
-//float 	mag_offset_z;
-//float 	mag_declination;
+// Airspeed Sensors
+// ----------------
 
 // Barometer Sensor variables
 // --------------------------
@@ -292,20 +286,18 @@ unsigned long 	abs_pressure;
 unsigned long 	ground_pressure;
 int 			ground_temperature;
 
-byte 	altitude_sensor = BARO;				// used to know which sensor is active, BARO or SONAR
-
-// Sonar Sensor variables
+// Altitude Sensor variables
 // ----------------------
 long	sonar_alt;
 long	baro_alt;
-
+byte 	altitude_sensor = BARO;				// used to know which sensor is active, BARO or SONAR
 
 // flight mode specific
 // --------------------
-boolean takeoff_complete	= false;		// Flag for using take-off controls
-boolean land_complete		= false;
-int 	takeoff_altitude;
-int 	landing_distance;					// meters;
+boolean	takeoff_complete;					// Flag for using take-off controls
+boolean	land_complete;
+int		takeoff_altitude;
+int		landing_distance;					// meters;
 long 	old_alt;							// used for managing altitude rates
 int		velocity_land;
 
@@ -320,13 +312,13 @@ int 	loiter_time_max;					// millis : how long to stay in LOITER mode
 
 // these are the values for navigation control functions
 // ----------------------------------------------------
-long 	nav_roll;							// deg * 100 : target roll angle
-long 	nav_pitch;							// deg * 100 : target pitch angle
-long 	nav_yaw;							// deg * 100 : target yaw angle
-long 	nav_lat;							// for error calcs
-long 	nav_lon;							// for error calcs
-int 	nav_throttle;						// 0-1000 for throttle control
-int 	nav_throttle_old;					// for filtering
+long	nav_roll;							// deg * 100 : target roll angle
+long	nav_pitch;							// deg * 100 : target pitch angle
+long	nav_yaw;							// deg * 100 : target yaw angle
+long	nav_lat;							// for error calcs
+long	nav_lon;							// for error calcs
+int		nav_throttle;						// 0-1000 for throttle control
+int		nav_throttle_old;					// for filtering
 
 long 	command_yaw_start;					// what angle were we to begin with
 long 	command_yaw_start_time;				// when did we start turning
@@ -338,12 +330,9 @@ byte	command_yaw_dir;
 
 // Waypoints
 // ---------
-//long 	GPS_wp_distance;					// meters - distance between plane and next waypoint
-long 	wp_distance;						// meters - distance between plane and next waypoint
-long 	wp_totalDistance;					// meters - distance between old and next waypoint
-//byte 	wp_total;							// # of Commands total including way
-//byte 	wp_index;							// Current active command index
-byte 	next_wp_index;						// Current active command index
+long	wp_distance;						// meters - distance between plane and next waypoint
+long	wp_totalDistance;					// meters - distance between old and next waypoint
+byte	next_wp_index;						// Current active command index
 
 // repeating event control
 // -----------------------
@@ -376,14 +365,14 @@ boolean	home_is_set; 						// Flag for if we have g_gps lock and have set the ho
 
 // IMU variables
 // -------------
-float G_Dt							= 0.02;		// Integration time for the gyros (DCM algorithm)
+float G_Dt						= 0.02;		// Integration time for the gyros (DCM algorithm)
 
 
 // Performance monitoring
 // ----------------------
 long 	perf_mon_timer;
-float 	imu_health; 							// Metric based on accel gain deweighting
-int 	G_Dt_max;								// Max main loop cycle time in milliseconds
+float 	imu_health; 						// Metric based on accel gain deweighting
+int 	G_Dt_max;							// Max main loop cycle time in milliseconds
 byte 	gyro_sat_count;
 byte 	adc_constraints;
 byte 	renorm_sqrt_count;
@@ -395,7 +384,7 @@ byte	gcs_messages_sent;
 // GCS
 // ---
 char GCS_buffer[53];
-char display_PID = -1;					// Flag used by DebugTerminal to indicate that the next PID calculation with this index should be displayed
+char display_PID = -1;						// Flag used by DebugTerminal to indicate that the next PID calculation with this index should be displayed
 
 // System Timers
 // --------------
@@ -415,12 +404,12 @@ byte			fbw_timer;					// for limiting the execution of FBW input
 unsigned long 	nav_loopTimer;				// used to track the elapsed ime for GPS nav
 unsigned long 	nav2_loopTimer;				// used to track the elapsed ime for GPS nav
 
-
 unsigned long 	dTnav;						// Delta Time in milliseconds for navigation computations
 unsigned long 	dTnav2;						// Delta Time in milliseconds for navigation computations
 unsigned long 	elapsedTime;				// for doing custom events
 float 			load;						// % MCU cycles used
 
+byte			counter_one_herz;
 
 ////////////////////////////////////////////////////////////////////////////////
 // Top-level logic
@@ -450,8 +439,14 @@ void loop()
 	if (millis() - medium_loopTimer > 19) {
 		delta_ms_medium_loop 	= millis() - medium_loopTimer;
 		medium_loopTimer		= millis();
+
 		medium_loop();
 
+		counter_one_herz++;
+		if(counter_one_herz == 50){
+			super_slow_loop();
+		}
+
 		if (millis() - perf_mon_timer > 20000) {
 			if (mainLoop_count != 0) {
 				gcs.send_message(MSG_PERF_REPORT);
@@ -541,15 +536,10 @@ void medium_loop()
 		case 2:
 			medium_loopCounter++;
 
-
 			// Read altitude from sensors
 			// ------------------
 			update_alt();
 
-			// altitude smoothing
-			// ------------------
-			calc_altitude_error();
-
 			// perform next command
 			// --------------------
 			update_commands();
@@ -643,9 +633,6 @@ void slow_loop()
 					}
 				#endif
 
-				if (g.log_bitmask & MASK_LOG_CUR)
-					Log_Write_Current();
-
 				superslow_loopCounter = 0;
             }
 			break;
@@ -690,6 +677,13 @@ void slow_loop()
 	}
 }
 
+void super_slow_loop()
+{
+	if (g.log_bitmask & MASK_LOG_CUR)
+		Log_Write_Current();
+}
+
+
 void update_GPS(void)
 {
 	g_gps->update();
@@ -737,17 +731,6 @@ void update_GPS(void)
 		current_loc.lng = g_gps->longitude;	// Lon * 10 * *7
 		current_loc.lat = g_gps->latitude;		// Lat * 10 * *7
 
-		/*Serial.printf_P(PSTR("Lat: %.7f, Lon: %.7f, Alt: %dm, GSP: %d COG: %d, dist: %d, #sats: %d\n"),
-					((float)g_gps->latitude /  T7),
-					((float)g_gps->longitude / T7),
-					(int)g_gps->altitude / 100,
-					(int)g_gps->ground_speed / 100,
-					(int)g_gps->ground_course / 100,
-					(int)wp_distance,
-					(int)g_gps->num_sats);
-		*/
-	}else{
-		//Serial.println("No fix");
 	}
 }
 
@@ -763,11 +746,6 @@ void update_current_flight_mode(void)
 			//	break;
 
 			default:
-
-				// based on altitude error
-				// -----------------------
-				calc_nav_throttle();
-
 				// Output Pitch, Roll, Yaw and Throttle
 				// ------------------------------------
 				auto_yaw();
@@ -906,10 +884,6 @@ void update_current_flight_mode(void)
 				// -----------
 				output_manual_yaw();
 
-				// based on altitude error
-				// -----------------------
-				calc_nav_throttle();
-
 				// Output Pitch, Roll, Yaw and Throttle
 				// ------------------------------------
 				// apply throttle control
@@ -924,10 +898,6 @@ void update_current_flight_mode(void)
 				break;
 
 			case RTL:
-				// based on altitude error
-				// -----------------------
-				calc_nav_throttle();
-
 				// Output Pitch, Roll, Yaw and Throttle
 				// ------------------------------------
 				auto_yaw();
@@ -949,11 +919,6 @@ void update_current_flight_mode(void)
 				// -----------
 				output_manual_yaw();
 
-				// based on altitude error
-				// -----------------------
-				calc_nav_throttle();
-
-
 				// Output Pitch, Roll, Yaw and Throttle
 				// ------------------------------------
 
@@ -1031,6 +996,7 @@ void update_alt()
 {
 	altitude_sensor = BARO;
 	baro_alt 		= read_barometer();
+	//Serial.printf("b_alt: %ld, home: %ld ", baro_alt, home.alt);
 
 	if(g.sonar_enabled){
 		// decide which sensor we're usings
@@ -1058,4 +1024,13 @@ void update_alt()
 		// no sonar altitude
 		current_loc.alt = baro_alt + home.alt;
 	}
+	//Serial.printf("b_alt: %ld, home: %ld ", baro_alt, home.alt);
+
+	// altitude smoothing
+	// ------------------
+	calc_altitude_error();
+
+	// Amount of throttle to apply for hovering
+	// ----------------------------------------
+	calc_nav_throttle();
 }
\ No newline at end of file
diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde
index 31996b55e3..44727c3419 100644
--- a/ArduCopterMega/Attitude.pde
+++ b/ArduCopterMega/Attitude.pde
@@ -129,15 +129,21 @@ output_yaw_with_hold(boolean hold)
 		g.rc_4.servo_out	-= constrain(dampener, -max_yaw_dampener, max_yaw_dampener); 	// -3000
 
 	}else{
+
 		// rate control
 		long rate		= degrees(omega.z) * 100; 											// 3rad = 17188 , 6rad = 34377
 		rate			= constrain(rate, -36000, 36000);									// limit to something fun!
+
+		if(abs(rate) < 1000 )	//experiment to limit yaw reversing
+			rate = 0;
+
 		long error		= ((long)g.rc_4.control_in * 6) - rate;								// control is += 6000 * 6 = 36000
 																							// -error = CCW, 	+error = CW
 		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 	= constrain(g.rc_4.servo_out, -2400, 2400);						// limit to 24°
 	}
 }
+// slight left rudder give right roll.
 
 void
 output_rate_roll()
@@ -201,32 +207,24 @@ void calc_nav_throttle()
 {
 	// limit error
 	long error = constrain(altitude_error, -400, 400);
+	float scaler = 1.0;
 
-	if(altitude_sensor == BARO) {
-		float t = g.pid_baro_throttle.kP();
+	if(error < 0){
+		scaler = (altitude_sensor == BARO) ? .5 : .9;
+	}
 
-		if(error > 0){ 					// go up
-			g.pid_baro_throttle.kP(t);
-		}else{							// go down
-			g.pid_baro_throttle.kP(t/4.0);
-		}
-
-		// limit output of throttle control
-		nav_throttle = g.pid_baro_throttle.get_pid(error, delta_ms_fast_loop, 1.0);
+	if(altitude_sensor == BARO){
+		nav_throttle = g.pid_baro_throttle.get_pid(error, delta_ms_fast_loop, scaler);
 		nav_throttle = g.throttle_cruise + constrain(nav_throttle, -30, 80);
-
-		g.pid_baro_throttle.kP(t);
-
 	}else{
-		// SONAR
-		nav_throttle = g.pid_sonar_throttle.get_pid(error, delta_ms_fast_loop, 1.0);
-
-		// limit output of throttle control
+		nav_throttle = g.pid_sonar_throttle.get_pid(error, delta_ms_fast_loop, scaler);
 		nav_throttle = g.throttle_cruise + constrain(nav_throttle, -60, 100);
 	}
 
 	nav_throttle = (nav_throttle + nav_throttle_old) >> 1;
 	nav_throttle_old = nav_throttle;
+
+	//Serial.printf("nav_thr %d, scaler %2.2f ", nav_throttle, scaler);
 }
 
 float angle_boost()
diff --git a/ArduCopterMega/Parameters.h b/ArduCopterMega/Parameters.h
index 4987ce2f1f..d32784da55 100644
--- a/ArduCopterMega/Parameters.h
+++ b/ArduCopterMega/Parameters.h
@@ -247,7 +247,7 @@ public:
         log_bitmask             (0,                         k_param_log_bitmask,           			PSTR("LOG_BITMASK")),
         ground_temperature      (0,                         k_param_ground_temperature,    			PSTR("GND_TEMP")),
         ground_pressure         (0,                         k_param_ground_pressure,       			PSTR("GND_ABS_PRESS")),
-        RTL_altitude            (ALT_HOLD_HOME_CM,          k_param_RTL_altitude,          			PSTR("ALT_HOLD_RTL")),
+        RTL_altitude            (ALT_HOLD_HOME * 100,		k_param_RTL_altitude,          			PSTR("ALT_HOLD_RTL")),
 
         // RC channel           group key                   name
         //----------------------------------------------------------------------
diff --git a/ArduCopterMega/commands.pde b/ArduCopterMega/commands.pde
index cc1654f5f8..a0c2a89578 100644
--- a/ArduCopterMega/commands.pde
+++ b/ArduCopterMega/commands.pde
@@ -95,8 +95,7 @@ void decrement_WP_index()
 
 long read_alt_to_hold()
 {
-	read_EEPROM_alt_RTL();
-	if(g.RTL_altitude == -1)
+	if(g.RTL_altitude < 0)
 		return current_loc.alt;
 	else
 		return g.RTL_altitude + home.alt;
diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h
index 26010b5250..265928dac3 100644
--- a/ArduCopterMega/config.h
+++ b/ArduCopterMega/config.h
@@ -535,7 +535,7 @@
 #ifndef ALT_HOLD_HOME
 # define ALT_HOLD_HOME 8
 #endif
-#define ALT_HOLD_HOME_CM ALT_HOLD_HOME*100
+#define ALT_HOLD_HOME_CM ALT_HOLD_HOME * 100
 
 #ifndef USE_CURRENT_ALT
 # define USE_CURRENT_ALT FALSE
diff --git a/ArduCopterMega/motors.pde b/ArduCopterMega/motors.pde
index c849c7ecb5..6637ff8f32 100644
--- a/ArduCopterMega/motors.pde
+++ b/ArduCopterMega/motors.pde
@@ -74,8 +74,8 @@ set_servos_4()
 
 		}else if(g.frame_type == X_FRAME){
 			//Serial.println("X_FRAME");
-			int roll_out 	 	= g.rc_1.pwm_out / 2;
-			int pitch_out 	 	= g.rc_2.pwm_out / 2;
+			int roll_out 	 	= g.rc_1.pwm_out * .707;
+			int pitch_out 	 	= g.rc_2.pwm_out * .707;
 
 			motor_out[CH_3]	 	= g.rc_3.radio_out + roll_out + pitch_out;
 			motor_out[CH_2]	 	= g.rc_3.radio_out + roll_out - pitch_out;
diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde
index 7ec6866723..026e35c6ec 100644
--- a/ArduCopterMega/navigation.pde
+++ b/ArduCopterMega/navigation.pde
@@ -86,8 +86,10 @@ void calc_nav()
 	nav_roll 	= (float)nav_lon * sin_yaw_y - (float)nav_lat * cos_yaw_x;
 	nav_pitch 	= -((float)nav_lon * cos_yaw_x + (float)nav_lat * sin_yaw_y);
 
-	nav_roll 	= constrain(nav_roll,  -g.pitch_max.get(), g.pitch_max.get());
-	nav_pitch 	= constrain(nav_pitch, -g.pitch_max.get(), g.pitch_max.get());
+	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/radio.pde b/ArduCopterMega/radio.pde
index e8b49fb1ad..17e119cf68 100644
--- a/ArduCopterMega/radio.pde
+++ b/ArduCopterMega/radio.pde
@@ -1,17 +1,28 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+
+//Function that will read the radio data, limit servos and trigger a failsafe
+// ----------------------------------------------------------------------------
+byte failsafeCounter = 0;		// we wait a second to take over the throttle and send the plane circling
+
+
 void init_rc_in()
 {
-	read_EEPROM_radio();		// read Radio limits
+	//read_EEPROM_radio();		// read Radio limits
+
+	// set rc channel ranges
 	g.rc_1.set_angle(4500);
 	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);
 
+	// set rc dead zones
 	g.rc_1.dead_zone = 60;		// 60 = .6 degrees
 	g.rc_2.dead_zone = 60;
 	g.rc_3.dead_zone = 20;
 	g.rc_4.dead_zone = 500;
 
+	//set auxiliary ranges
 	g.rc_5.set_range(0,1000);
 	g.rc_5.set_filter(false);
 	g.rc_6.set_range(0,1000);
@@ -56,10 +67,12 @@ void read_radio()
 	g.rc_6.set_pwm(APM_RC.InputCh(CH_6));
 	g.rc_7.set_pwm(APM_RC.InputCh(CH_7));
 	g.rc_8.set_pwm(APM_RC.InputCh(CH_8));
+
+	//throttle_failsafe(g.rc_3.radio_in);
+
 	//Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d \n"), g.rc_1.control_in, g.rc_2.control_in, g.rc_3.control_in, g.rc_4.control_in);
 }
 
-/*
 void throttle_failsafe(uint16_t pwm)
 {
 	if(g.throttle_fs_enabled == 0)
@@ -101,7 +114,6 @@ void throttle_failsafe(uint16_t pwm)
 		}
 	}
 }
-*/
 
 void trim_radio()
 {
diff --git a/ArduCopterMega/sensors.pde b/ArduCopterMega/sensors.pde
index 7dbe85a38a..c970b56e90 100644
--- a/ArduCopterMega/sensors.pde
+++ b/ArduCopterMega/sensors.pde
@@ -1,33 +1,75 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
 
+// Sensors are not available in HIL_MODE_ATTITUDE
+#if HIL_MODE != HIL_MODE_ATTITUDE
+
 void ReadSCP1000(void) {}
 
 void init_barometer(void)
 {
-	for(int i = 0; i < 300; i++){		// We take some readings...
-		delay(20);
-		barometer.Read(); 	// Get initial data from absolute pressure sensor
-		ground_pressure 	= (ground_pressure * 9l   + barometer.Press) / 10l;
-		ground_temperature 	= (ground_temperature * 9 + barometer.Temp) / 10;
-	}
-	abs_pressure  = barometer.Press;
-}
+	int flashcount;
 
-// Sensors are not available in HIL_MODE_ATTITUDE
-#if HIL_MODE != HIL_MODE_ATTITUDE
+	#if HIL_MODE == HIL_MODE_SENSORS
+		hil.update();					// look for inbound hil packets for initialization
+	#endif
+
+	ground_pressure = 0;
+
+	while(ground_pressure == 0){
+		barometer.Read(); 					// Get initial data from absolute pressure sensor
+		ground_pressure 	= barometer.Press;
+		ground_temperature 	= barometer.Temp;
+		delay(20);
+		Serial.printf("barometer.Press %ld\n", barometer.Press);
+	}
+
+	for(int i = 0; i < 30; i++){		// We take some readings...
+
+		#if HIL_MODE == HIL_MODE_SENSORS
+			hil.update(); 				// look for inbound hil packets
+		#endif
+
+		barometer.Read(); 				// Get initial data from absolute pressure sensor
+		ground_pressure	= (ground_pressure * 9l   + barometer.Press) / 10l;
+		ground_temperature	= (ground_temperature * 9 + barometer.Temp) / 10;
+
+		delay(20);
+		if(flashcount == 5) {
+			digitalWrite(C_LED_PIN, LOW);
+			digitalWrite(A_LED_PIN, HIGH);
+			digitalWrite(B_LED_PIN, LOW);
+		}
+
+		if(flashcount >= 10) {
+			flashcount = 0;
+			digitalWrite(C_LED_PIN, HIGH);
+			digitalWrite(A_LED_PIN, LOW);
+			digitalWrite(B_LED_PIN, HIGH);
+		}
+		flashcount++;
+	}
+
+	// makes the filtering work later
+	abs_pressure  = barometer.Press;
+
+	// save home pressure - will be overwritten by init_home, no big deal
+	ground_pressure = abs_pressure;
+
+	//Serial.printf("abs_pressure %ld\n", abs_pressure);
+	SendDebugln("barometer calibration complete.");
+}
 
 long read_barometer(void)
 {
  	float x, scaling, temp;
 
-	barometer.Read(); 	// Get new data from absolute pressure sensor
+	barometer.Read();		// Get new data from absolute pressure sensor
 
 	//abs_pressure 			= (abs_pressure + barometer.Press) >> 1;		// Small filtering
 	abs_pressure 			= ((float)abs_pressure * .7) + ((float)barometer.Press * .3);		// large filtering
 	scaling 				= (float)ground_pressure / (float)abs_pressure;
 	temp 					= ((float)ground_temperature / 10.0f) + 273.15f;
 	x 						= log(scaling) * temp * 29271.267f;
-
 	return 	(x / 10);
 }
 
@@ -37,10 +79,13 @@ void read_airspeed(void)
 
 }
 
+void zero_airspeed(void)
+{
+
+}
+
 #endif // HIL_MODE != HIL_MODE_ATTITUDE
 
-
-
 #if BATTERY_EVENT == 1
 void read_battery(void)
 {
diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde
index 719611c2d4..fcd2c7b1a6 100644
--- a/ArduCopterMega/setup.pde
+++ b/ArduCopterMega/setup.pde
@@ -33,8 +33,7 @@ const struct Menu::command setup_menu_commands[] PROGMEM = {
 	{"compass",			setup_compass},
 	{"mag_offset",		setup_mag_offset},
 	{"declination",		setup_declination},
-	{"show",			setup_show},
-	{"ap_show",			AP_Var_menu_show}
+	{"show",			setup_show}
 };
 
 // Create the setup menu object.
@@ -75,6 +74,8 @@ setup_show(uint8_t argc, const Menu::arg *argv)
 	report_flight_modes();
 	report_imu();
 	report_compass();
+
+	AP_Var_menu_show(argc, argv);
 	return(0);
 }
 
@@ -388,7 +389,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
 
 		// escape hatch
 		if(Serial.available() > 0){
-			save_EEPROM_flight_modes();
+			g.flight_modes.save();
 			print_done();
 			report_flight_modes();
 			return (0);
@@ -426,7 +427,7 @@ setup_compass(uint8_t argc, const Menu::arg *argv)
 		return 0;
 	}
 
-	save_EEPROM_mag();
+	g.compass_enabled.save();
 	report_compass();
 	return 0;
 }
@@ -451,8 +452,7 @@ setup_frame(uint8_t argc, const Menu::arg *argv)
 		report_frame();
 		return 0;
 	}
-
-	save_EEPROM_frame();
+	g.frame_type.save();
 	report_frame();
 	return 0;
 }
@@ -508,7 +508,7 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv)
 
 
 	compass.init();	 // Initialization
-	compass.set_orientation(MAGORIENTATION);		// set compass's orientation on aircraft
+	compass.set_orientation(MAG_ORIENTATION);		// set compass's orientation on aircraft
 	//compass.set_offsets(0, 0, 0);					// set offsets to account for surrounding interference
 	//int counter = 0;
 	float _min[3], _max[3], _offset[3];
@@ -600,15 +600,13 @@ default_nav()
 void
 default_alt_hold()
 {
-	g.RTL_altitude.set(-1);
-	save_EEPROM_alt_RTL();
+	g.RTL_altitude.set_and_save(-1);
 }
 
 void
 default_frame()
 {
-	g.frame_type = PLUS_FRAME;
-	save_EEPROM_frame();
+	g.frame_type.set_and_save(PLUS_FRAME);
 }
 
 void
@@ -628,7 +626,7 @@ default_flight_modes()
 	g.flight_modes[3] 			= FLIGHT_MODE_4;
 	g.flight_modes[4] 			= FLIGHT_MODE_5;
 	g.flight_modes[5] 			= FLIGHT_MODE_6;
-	save_EEPROM_flight_modes();
+	g.flight_modes.save();
 }
 
 void
@@ -661,7 +659,7 @@ void default_logs()
 		LOGBIT(CURRENT);
 	#undef LOGBIT
 
-	save_EEPROM_logs();
+	g.log_bitmask.save();
 }
 
 
@@ -686,9 +684,7 @@ default_gains()
 
 
 	// stabilize, angle error
-	Serial.printf("b4 %4.2f, ",g.pid_stabilize_roll.kP());
 	g.pid_stabilize_roll.kP(STABILIZE_ROLL_P);
-	Serial.printf("L8R %4.2f\n ",g.pid_stabilize_roll.kP());
 	g.pid_stabilize_roll.kI(STABILIZE_ROLL_I);
 	g.pid_stabilize_roll.kD(0);
 	g.pid_stabilize_roll.imax(STABILIZE_ROLL_IMAX * 100);
@@ -734,8 +730,6 @@ default_gains()
 	g.pid_sonar_throttle.imax(THROTTLE_SONAR_IMAX);
 
 	save_EEPROM_PID();
-	Serial.printf("EL8R %4.2f\n ",g.pid_stabilize_roll.kP());
-
 }
 
 
@@ -746,7 +740,6 @@ default_gains()
 
 void report_current()
 {
-	//print_blanks(2);
 	read_EEPROM_current();
 	Serial.printf_P(PSTR("Current Sensor\n"));
 	print_divider();
@@ -758,7 +751,6 @@ void report_current()
 
 void report_sonar()
 {
-	//print_blanks(2);
 	g.sonar_enabled.load();
 	Serial.printf_P(PSTR("Sonar Sensor\n"));
 	print_divider();
@@ -769,8 +761,6 @@ void report_sonar()
 
 void report_frame()
 {
-	//print_blanks(2);
-	read_EEPROM_frame();
 	Serial.printf_P(PSTR("Frame\n"));
 	print_divider();
 
@@ -790,7 +780,6 @@ void report_frame()
 
 void report_radio()
 {
-	//print_blanks(2);
 	Serial.printf_P(PSTR("Radio\n"));
 	print_divider();
 	// radio
@@ -801,7 +790,6 @@ void report_radio()
 
 void report_gains()
 {
-	//print_blanks(2);
 	Serial.printf_P(PSTR("Gains\n"));
 	print_divider();
 
@@ -839,7 +827,6 @@ void report_gains()
 
 void report_xtrack()
 {
-	//print_blanks(2);
 	Serial.printf_P(PSTR("Crosstrack\n"));
 	print_divider();
 	// radio
@@ -855,7 +842,6 @@ void report_xtrack()
 
 void report_throttle()
 {
-	//print_blanks(2);
 	Serial.printf_P(PSTR("Throttle\n"));
 	print_divider();
 
@@ -875,7 +861,6 @@ void report_throttle()
 
 void report_imu()
 {
-	//print_blanks(2);
 	Serial.printf_P(PSTR("IMU\n"));
 	print_divider();
 
@@ -886,14 +871,9 @@ void report_imu()
 
 void report_compass()
 {
-	//print_blanks(2);
 	Serial.printf_P(PSTR("Compass\n"));
 	print_divider();
 
-	read_EEPROM_compass();
-	//read_EEPROM_compass_declination();
-	//read_EEPROM_compass_offset();
-
 	print_enabled(g.compass_enabled);
 
 	// mag declination
@@ -912,10 +892,8 @@ void report_compass()
 
 void report_flight_modes()
 {
-	//print_blanks(2);
 	Serial.printf_P(PSTR("Flight modes\n"));
 	print_divider();
-	read_EEPROM_flight_modes();
 
 	for(int i = 0; i < 6; i++ ){
 		print_switch(i, g.flight_modes[i]);
@@ -1041,3 +1019,194 @@ print_gyro_offsets(void)
 }
 
 
+
+/***************************************************************************/
+// EEPROM convenience functions
+/***************************************************************************/
+
+
+void read_EEPROM_waypoint_info(void)
+{
+	g.waypoint_total.load();
+	g.waypoint_radius.load();
+	g.loiter_radius.load();
+}
+
+void save_EEPROM_waypoint_info(void)
+{
+	g.waypoint_total.save();
+	g.waypoint_radius.save();
+	g.loiter_radius.save();
+}
+
+/********************************************************************************/
+
+void read_EEPROM_nav(void)
+{
+	g.crosstrack_gain.load();
+	g.crosstrack_entry_angle.load();
+	g.pitch_max.load();
+}
+
+void save_EEPROM_nav(void)
+{
+	g.crosstrack_gain.save();
+ 	g.crosstrack_entry_angle.save();
+	g.pitch_max.save();
+}
+
+/********************************************************************************/
+
+void read_EEPROM_PID(void)
+{
+	g.pid_acro_rate_roll.load_gains();
+	g.pid_acro_rate_pitch.load_gains();
+	g.pid_acro_rate_yaw.load_gains();
+
+	g.pid_stabilize_roll.load_gains();
+	g.pid_stabilize_pitch.load_gains();
+	g.pid_yaw.load_gains();
+
+	g.pid_nav_lon.load_gains();
+	g.pid_nav_lat.load_gains();
+	g.pid_baro_throttle.load_gains();
+	g.pid_sonar_throttle.load_gains();
+
+	// roll pitch
+	g.stabilize_dampener.load();
+
+	// yaw
+	g.hold_yaw_dampener.load();
+ 	init_pids();
+}
+
+void save_EEPROM_PID(void)
+{
+
+	g.pid_acro_rate_roll.save_gains();
+	g.pid_acro_rate_pitch.save_gains();
+	g.pid_acro_rate_yaw.save_gains();
+
+	g.pid_stabilize_roll.save_gains();
+	g.pid_stabilize_pitch.save_gains();
+	g.pid_yaw.save_gains();
+
+	g.pid_nav_lon.save_gains();
+	g.pid_nav_lat.save_gains();
+	g.pid_baro_throttle.save_gains();
+	g.pid_sonar_throttle.save_gains();
+
+	// roll pitch
+	g.stabilize_dampener.save();
+	// yaw
+	g.hold_yaw_dampener.save();
+}
+
+/********************************************************************************/
+
+void save_EEPROM_current(void)
+{
+	g.current_enabled.save();
+	g.milliamp_hours.save();
+}
+
+void read_EEPROM_current(void)
+{
+	g.current_enabled.load();
+	g.milliamp_hours.load();
+}
+
+/********************************************************************************/
+
+void read_EEPROM_radio(void)
+{
+	g.rc_1.load_eeprom();
+	g.rc_2.load_eeprom();
+	g.rc_3.load_eeprom();
+	g.rc_4.load_eeprom();
+	g.rc_5.load_eeprom();
+	g.rc_6.load_eeprom();
+	g.rc_7.load_eeprom();
+	g.rc_8.load_eeprom();
+}
+
+void save_EEPROM_radio(void)
+{
+	g.rc_1.save_eeprom();
+	g.rc_2.save_eeprom();
+	g.rc_3.save_eeprom();
+	g.rc_4.save_eeprom();
+	g.rc_5.save_eeprom();
+	g.rc_6.save_eeprom();
+	g.rc_7.save_eeprom();
+	g.rc_8.save_eeprom();
+}
+
+/********************************************************************************/
+// configs are the basics
+void read_EEPROM_throttle(void)
+{
+	g.throttle_min.load();
+	g.throttle_max.load();
+	g.throttle_cruise.load();
+	g.throttle_fs_enabled.load();
+	g.throttle_fs_action.load();
+	g.throttle_fs_value.load();
+}
+
+void save_EEPROM_throttle(void)
+{
+	g.throttle_min.load();
+	g.throttle_max.load();
+	g.throttle_cruise.save();
+	g.throttle_fs_enabled.load();
+	g.throttle_fs_action.load();
+	g.throttle_fs_value.load();
+}
+
+
+/********************************************************************************/
+/*
+float
+read_EE_float(int address)
+{
+	union {
+		byte bytes[4];
+		float value;
+	} _floatOut;
+
+	for (int i = 0; i < 4; i++)
+		_floatOut.bytes[i] = eeprom_read_byte((uint8_t *) (address + i));
+	return _floatOut.value;
+}
+
+void write_EE_float(float value, int address)
+{
+	union {
+		byte bytes[4];
+		float value;
+	} _floatIn;
+
+	_floatIn.value = value;
+	for (int i = 0; i < 4; i++)
+		eeprom_write_byte((uint8_t *) (address + i), _floatIn.bytes[i]);
+}
+*/
+/********************************************************************************/
+/*
+float
+read_EE_compressed_float(int address, byte places)
+{
+	float scale = pow(10, places);
+
+	int temp 	= eeprom_read_word((uint16_t *) address);
+	return ((float)temp / scale);
+}
+
+void write_EE_compressed_float(float value, int address, byte places)
+{
+	float scale = pow(10, places);
+	int temp 	= value * scale;
+	eeprom_write_word((uint16_t *) 	address, temp);
+}
+*/
\ No newline at end of file
diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde
index 32c9d512d5..08dad51f3b 100644
--- a/ArduCopterMega/system.pde
+++ b/ArduCopterMega/system.pde
@@ -392,7 +392,7 @@ init_compass()
 {
 	dcm.set_compass(&compass);
 	bool junkbool = compass.init();
-	compass.set_orientation(MAGORIENTATION);						// set compass's orientation on aircraft
+	compass.set_orientation(MAG_ORIENTATION);						// set compass's orientation on aircraft
 	Vector3f junkvector = compass.get_offsets();					// load offsets to account for airframe magnetic interference
 }
 
diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde
index 5ccad4edcb..5877bcd035 100644
--- a/ArduCopterMega/test.pde
+++ b/ArduCopterMega/test.pde
@@ -351,6 +351,8 @@ test_fbw(uint8_t argc, const Menu::arg *argv)
 			// IMU
 			// ---
 			read_AHRS();
+			update_trig();
+
 
 			// allow us to zero out sensors with control switches
 			if(g.rc_5.control_in < 600){
@@ -367,6 +369,7 @@ test_fbw(uint8_t argc, const Menu::arg *argv)
 
 			ts_num++;
 			if (ts_num == 5){
+				update_alt();
 				// 10 hz
 				ts_num 				= 0;
 				g_gps->longitude 		= 0;
@@ -474,7 +477,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
 								accels.x, accels.y, accels.z,
 								gyros.x,  gyros.y,  gyros.z);
 			*/
-			Serial.printf_P(PSTR("r: %ld\tp: %ld\t y: %ld\t"),
+			Serial.printf_P(PSTR("r: %ld\tp: %ld\t y: %ld\n"),
 								dcm.roll_sensor,
 								dcm.pitch_sensor,
 								dcm.yaw_sensor);
@@ -717,10 +720,10 @@ test_wp(uint8_t argc, const Menu::arg *argv)
 
 
 	// save the alitude above home option
-	if(g.RTL_altitude == -1){
+	if(g.RTL_altitude < 0){
 		Serial.printf_P(PSTR("Hold current altitude\n"));
 	}else{
-		Serial.printf_P(PSTR("Hold altitude of %dm\n"), (int)g.RTL_altitude);
+		Serial.printf_P(PSTR("Hold altitude of %dm\n"), (int)g.RTL_altitude / 100);
 	}
 
 	Serial.printf_P(PSTR("%d waypoints\n"), (int)g.waypoint_total);
@@ -769,39 +772,25 @@ test_xbee(uint8_t argc, const Menu::arg *argv)
 static int8_t
 test_pressure(uint8_t argc, const Menu::arg *argv)
 {
-	uint32_t sum;
-
-	Serial.printf_P(PSTR("Uncalibrated Abs Airpressure\n"));
-	Serial.printf_P(PSTR("Altitude is relative to the start of this test\n"));
+	Serial.printf_P(PSTR("Uncalibrated relative airpressure\n"));
 	print_hit_enter();
 
-	Serial.printf_P(PSTR("\nCalibrating....\n"));
-	/*
-	for (int i = 1; i < 301; i++) {
-		baro_alt = read_barometer();
-		if(i > 200)
-			sum += abs_pressure;
-		delay(10);
-	}
-	ground_pressure = (float)sum / 100.0;
-	*/
-
 	home.alt = 0;
 	wp_distance = 0;
 	init_barometer();
+	reset_I();
+
+	// to prevent boost from skewing results
+	cos_pitch_x = cos_roll_x = 1;
 
 	while(1){
-		if (millis()-fast_loopTimer > 9) {
+		if (millis() - fast_loopTimer > 100) {
 			delta_ms_fast_loop 	= millis() - fast_loopTimer;
 			G_Dt 				= (float)delta_ms_fast_loop / 1000.f;		// used by DCM integrator
 			fast_loopTimer		= millis();
-
-
-			calc_altitude_error();
-			calc_nav_throttle();
 		}
 
-		if (millis()-medium_loopTimer > 100) {
+		if (millis() - medium_loopTimer > 100) {
 			medium_loopTimer	= millis();
 
 			read_radio();			// read the radio first
@@ -809,14 +798,15 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
 			next_WP.alt =  max(next_WP.alt, 30);
 
 			read_trim_switch();
-			baro_alt = read_barometer();
-			current_loc.alt = baro_alt + home.alt;
-			Serial.printf_P(PSTR("AP: %ld,\tAlt: %ld, \tnext_alt: %ld \terror: %ld, \tcruise: %d, \t out:%d\n"),
-						abs_pressure,
+			update_alt();
+			output_auto_throttle();
+
+			Serial.printf_P(PSTR("Alt: %ld, \tnext_alt: %ld \terror: %ld, \tcruise: %d, \tint: %6.2f \tout:%d\n"),
 						baro_alt,
 						next_WP.alt,
 						altitude_error,
 						(int)g.throttle_cruise,
+						g.pid_baro_throttle.get_integrator(),
 						g.rc_3.servo_out);
 
 			/*
@@ -843,29 +833,35 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
 
 static int8_t
 test_mag(uint8_t argc, const Menu::arg *argv)
-{
-	if(g.compass_enabled) {
-		print_hit_enter();
+	{
+		if(g.compass_enabled) {
+			//Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION);
 
-		while(1){
-			delay(250);
-			compass.read();
-			compass.calculate(0,0);
-			Serial.printf_P(PSTR("Heading: %4.2f, XYZ: %4.2f, %4.2f, %4.2f"),
-						ToDeg(compass.heading),
-						compass.mag_x,
-						compass.mag_y,
-						compass.mag_z);
+			print_hit_enter();
 
-    		if(Serial.available() > 0){
-				return (0);
+			while(1){
+				delay(250);
+				compass.read();
+				compass.calculate(0,0);
+				Vector3f maggy = compass.get_offsets();
+				Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"),
+							(wrap_360(ToDeg(compass.heading) * 100)) /100,
+							compass.mag_x,
+							compass.mag_y,
+							compass.mag_z,
+							maggy.x,
+							maggy.y,
+							maggy.z);
+
+				if(Serial.available() > 0){
+					return (0);
+				}
 			}
+		} else {
+			Serial.printf_P(PSTR("Compass: "));
+			print_enabled(false);
+			return (0);
 		}
-	} else {
-		Serial.printf_P(PSTR("Compass: "));
-		print_enabled(false);
-		return (0);
-	}
 }
 
 void print_hit_enter()