diff --git a/ArduPlane/setup.pde b/ArduPlane/setup.pde
index 605b1c9342..72fd20f096 100644
--- a/ArduPlane/setup.pde
+++ b/ArduPlane/setup.pde
@@ -313,6 +313,10 @@ static int8_t
 setup_accel_scale(uint8_t argc, const Menu::arg *argv)
 {
     cliSerial->println_P(PSTR("Initialising gyros"));
+
+    ahrs.init();
+    ahrs.set_fly_forward(true);
+
     ins.init(AP_InertialSensor::COLD_START, 
              ins_sample_rate,
              flash_leds);
diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde
index 2676ed83c1..94df557e5f 100644
--- a/ArduPlane/system.pde
+++ b/ArduPlane/system.pde
@@ -227,12 +227,13 @@ static void init_ardupilot()
         //----------------
         //read_EEPROM_airstart_critical();
 #if HIL_MODE != HIL_MODE_ATTITUDE
+        ahrs.init();
+        ahrs.set_fly_forward(true);
+
         ins.init(AP_InertialSensor::WARM_START, 
                  ins_sample_rate,
                  flash_leds);
 
-        ahrs.init();
-        ahrs.set_fly_forward(true);
 #endif
 
         // This delay is important for the APM_RC library to work.
@@ -442,6 +443,9 @@ static void startup_INS_ground(bool force_accel_level)
     gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Beginning INS calibration; do not move plane"));
     mavlink_delay(1000);
 
+    ahrs.init();
+    ahrs.set_fly_forward(true);
+
     ins.init(AP_InertialSensor::COLD_START, 
              ins_sample_rate,
              flash_leds);
@@ -453,7 +457,6 @@ static void startup_INS_ground(bool force_accel_level)
         ins.init_accel(flash_leds);
     }
 #endif
-    ahrs.set_fly_forward(true);
     ahrs.reset();
 
 #if HIL_MODE != HIL_MODE_ATTITUDE
diff --git a/ArduPlane/test.pde b/ArduPlane/test.pde
index 4e470bc391..e79b62db4c 100644
--- a/ArduPlane/test.pde
+++ b/ArduPlane/test.pde
@@ -466,6 +466,9 @@ static int8_t
 test_ins(uint8_t argc, const Menu::arg *argv)
 {
     //cliSerial->printf_P(PSTR("Calibrating."));
+    ahrs.init();
+    ahrs.set_fly_forward(true);
+
     ins.init(AP_InertialSensor::COLD_START, 
              ins_sample_rate,
              flash_leds);
@@ -525,6 +528,8 @@ test_mag(uint8_t argc, const Menu::arg *argv)
         cliSerial->println_P(PSTR("Compass initialisation failed!"));
         return 0;
     }
+    ahrs.init();
+    ahrs.set_fly_forward(true);
     ahrs.set_compass(&compass);
     report_compass();