AP_AHRS: added scheduler parameter to init

Required by the AP_AHRS_MPU6000 class which needs to disable timed processes that could interfere with it's communication with the mpu6000

ArduCopter/system.pde

@@ -362,7 +362,7 @@ static void startup_ground(void)
  #endif
 
     // initialise ahrs (may push imu calibration into the mpu6000 if using that device).
-    ahrs.init();
+    ahrs.init(&timer_scheduler);
 
     // setup fast AHRS gains to get right attitude
     ahrs.set_fast_gains(true);

ArduPlane/system.pde

@@ -235,7 +235,7 @@ static void init_ardupilot()
         imu.init(IMU::WARM_START, mavlink_delay, flash_leds, &timer_scheduler);
 
         // initialise ahrs (may push imu calibration into the mpu6000 if using that device).
-        ahrs.init();
+        ahrs.init(&timer_scheduler);
         ahrs.set_fly_forward(true);
 #endif
 

libraries/AP_AHRS/AP_AHRS.h

@@ -41,7 +41,7 @@ public:
     }
 
     // empty init
-    virtual void init() {
+    virtual void init( AP_PeriodicProcess * scheduler = NULL ) {
     };
 
     // Accessors

libraries/AP_AHRS/AP_AHRS_MPU6000.cpp

@@ -28,11 +28,24 @@
 #define SPIN_RATE_LIMIT 20
 
 void
-AP_AHRS_MPU6000::init()
+AP_AHRS_MPU6000::init( AP_PeriodicProcess * scheduler )
 {
+    bool timer_running = false;
+
+    // suspend timer so interrupts on spi bus do not interfere with communication to mpu6000
+    if( scheduler != NULL ) {
+        timer_running = scheduler->running();
+        scheduler->suspend_timer();
+    }
+
     _mpu6000->dmp_init();
     push_gains_to_dmp();
     push_offsets_to_ins();
+
+    // restart timer
+    if( timer_running ) {
+        scheduler->resume_timer();
+    }
 };
 
 // run a full MPU6000 update round

libraries/AP_AHRS/AP_AHRS_MPU6000.h

@@ -35,7 +35,7 @@ public:
     }
 
     // initialisation routine to start MPU6000's dmp
-    void init();
+    void init( AP_PeriodicProcess * scheduler = NULL );
 
     // return the smoothed gyro vector corrected for drift
     Vector3f        get_gyro(void) {

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp

@@ -590,11 +590,6 @@ void AP_InertialSensor_MPU6000::dmp_init()
         return;
     }
 
-    // suspend timer
-    if( _scheduler != NULL ) {
-        _scheduler->suspend_timer();
-    }
-
     // load initial values into DMP memory
     dmp_load_mem();
 
@@ -642,10 +637,6 @@ void AP_InertialSensor_MPU6000::dmp_init()
     // dmp initialisation complete
     _dmp_initialised = true;
 
-    // resume timer
-    if( _scheduler != NULL ) {
-        _scheduler->resume_timer();
-    }
 }
 
 // dmp_reset - reset dmp (required for changes in gains or offsets to take effect)