Rover: ensure ahrs.init() is called

APMrover2/setup.pde

@@ -300,6 +300,7 @@ static int8_t
 setup_accel_scale(uint8_t argc, const Menu::arg *argv)
 {
     cliSerial->println_P(PSTR("Initialising gyros"));
+    ahrs.init();
     ins.init(AP_InertialSensor::COLD_START, 
              ins_sample_rate,
              flash_leds);

APMrover2/system.pde

@@ -425,6 +425,8 @@ 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);
@@ -434,7 +436,6 @@ static void startup_INS_ground(bool force_accel_level)
         // it once via the ground station	
         ins.init_accel(flash_leds);
 	}
-	ahrs.set_fly_forward(true);
     ahrs.reset();
 
 #endif // HIL_MODE_ATTITUDE

APMrover2/test.pde

@@ -425,6 +425,8 @@ 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);
@@ -484,6 +486,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();