diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde
index f9ef6dc785..887ac9fb0a 100644
--- a/ArduCopterMega/setup.pde
+++ b/ArduCopterMega/setup.pde
@@ -12,7 +12,7 @@ static int8_t	setup_frame				(uint8_t argc, const Menu::arg *argv);
 static int8_t	setup_current			(uint8_t argc, const Menu::arg *argv);
 static int8_t	setup_sonar				(uint8_t argc, const Menu::arg *argv);
 static int8_t	setup_compass			(uint8_t argc, const Menu::arg *argv);
-static int8_t	setup_mag_offset		(uint8_t argc, const Menu::arg *argv);
+//static int8_t	setup_mag_offset		(uint8_t argc, const Menu::arg *argv);
 static int8_t	setup_declination		(uint8_t argc, const Menu::arg *argv);
 static int8_t	setup_show				(uint8_t argc, const Menu::arg *argv);
 
@@ -31,7 +31,7 @@ const struct Menu::command setup_menu_commands[] PROGMEM = {
 	{"current",			setup_current},
 	{"sonar",			setup_sonar},
 	{"compass",			setup_compass},
-	{"mag_offset",		setup_mag_offset},
+//	{"mag_offset",		setup_mag_offset},
 	{"declination",		setup_declination},
 	{"show",			setup_show}
 };
@@ -498,7 +498,7 @@ setup_sonar(uint8_t argc, const Menu::arg *argv)
 	return 0;
 }
 
-static int8_t
+/*static int8_t
 setup_mag_offset(uint8_t argc, const Menu::arg *argv)
 {
 	Serial.printf_P(PSTR("\nRotate/Pitch/Roll your ArduCopter until the offset variables stop changing.\n"));
@@ -574,6 +574,7 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv)
 		}
 	}
 }
+*/
 
 
 /***************************************************************************/
diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde
index 2289ffd89c..d881549bfc 100644
--- a/ArduCopterMega/test.pde
+++ b/ArduCopterMega/test.pde
@@ -256,6 +256,7 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
 				if(medium_loopCounter == 5){
 					compass.read();		 				// Read magnetometer
 					compass.calculate(dcm.roll, dcm.pitch);		// Calculate heading
+					compass.null_offsets(dcm.get_dcm_matrix());
 					medium_loopCounter = 0;
 				}
 			}
@@ -300,6 +301,10 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
 			//Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)dcm.roll_sensor/100), ((int)dcm.pitch_sensor/100), ((uint16_t)dcm.yaw_sensor/100));
 
 			if(Serial.available() > 0){
+				if(g.compass_enabled){
+					compass.save_offsets();
+				}
+				report_compass();
 				return (0);
 			}