diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
index 77f83100e2..e53c615583 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@@ -926,11 +926,31 @@ void AP_InertialSensor::update(void)
     wait_for_sample();
 
     if (!_hil_mode) {
+        for (int8_t i=0; i<INS_MAX_INSTANCES; i++) {
+            // mark sensors unhealthy and let update() in each backend
+            // mark them healthy via _rotate_and_offset_gyro() and
+            // _rotate_and_offset_accel() 
+            _gyro_healthy[i] = false;
+            _accel_healthy[i] = false;
+        }
         for (int8_t i=0; i<INS_MAX_BACKENDS; i++) {
             if (_backends[i] != NULL) {
                 _backends[i]->update();
             }
-        }    
+        }
+        // set primary to first healthy accel and gyro
+        for (int8_t i=0; i<INS_MAX_INSTANCES; i++) {
+            if (_gyro_healthy[i]) {
+                _primary_gyro = i;
+                break;
+            }
+        }
+        for (int8_t i=0; i<INS_MAX_INSTANCES; i++) {
+            if (_accel_healthy[i]) {
+                _primary_accel = i;
+                break;
+            }
+        }
     }
 
     _have_sample = false;
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
index 9257e80228..23e2dbc092 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.h
@@ -125,14 +125,14 @@ public:
     void               set_accel(uint8_t instance, const Vector3f &accel);
 
     // multi-device interface
-    bool get_gyro_health(uint8_t instance) const { return true; }
+    bool get_gyro_health(uint8_t instance) const { return _gyro_healthy[instance]; }
     bool get_gyro_health(void) const { return get_gyro_health(_primary_gyro); }
     bool get_gyro_health_all(void) const;
     uint8_t get_gyro_count(void) const { return _gyro_count; }
     bool gyro_calibrated_ok(uint8_t instance) const { return _gyro_cal_ok[instance]; }
     bool gyro_calibrated_ok_all() const;
 
-    bool get_accel_health(uint8_t instance) const { return true; }
+    bool get_accel_health(uint8_t instance) const { return _accel_healthy[instance]; }
     bool get_accel_health(void) const { return get_accel_health(_primary_accel); }
     bool get_accel_health_all(void) const;
     uint8_t get_accel_count(void) const { return _accel_count; };
@@ -226,10 +226,6 @@ private:
     // Most recent gyro reading
     Vector3f _gyro[INS_MAX_INSTANCES];
 
-    // timestamp of latest gyro and accel readings
-    uint32_t _last_gyro_sample_time_usec[INS_MAX_INSTANCES];
-    uint32_t _last_accel_sample_time_usec[INS_MAX_INSTANCES];
-
     // product id
     AP_Int16 _product_id;
 
@@ -269,6 +265,10 @@ private:
     
     // time between samples in microseconds
     uint32_t _sample_period_usec;
+
+    // health of gyros and accels
+    bool _gyro_healthy[INS_MAX_INSTANCES];
+    bool _accel_healthy[INS_MAX_INSTANCES];
 };
 
 #include "AP_InertialSensor_Backend.h"
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
index ade75ee243..3f7678f816 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
@@ -12,18 +12,18 @@ AP_InertialSensor_Backend::AP_InertialSensor_Backend(AP_InertialSensor &imu) :
 /*
   rotate gyro vector and add the gyro offset
  */
-void AP_InertialSensor_Backend::_rotate_and_offset_gyro(uint8_t instance, const Vector3f &gyro, uint32_t now)
+void AP_InertialSensor_Backend::_rotate_and_offset_gyro(uint8_t instance, const Vector3f &gyro)
 {
     _imu._gyro[instance] = gyro;
     _imu._gyro[instance].rotate(_imu._board_orientation);
     _imu._gyro[instance] -= _imu._gyro_offset[instance];
-    _imu._last_gyro_sample_time_usec[instance] = now;
+    _imu._gyro_healthy[instance] = true;
 }
 
 /*
   rotate accel vector, scale and add the accel offset
  */
-void AP_InertialSensor_Backend::_rotate_and_offset_accel(uint8_t instance, const Vector3f &accel, uint32_t now)
+void AP_InertialSensor_Backend::_rotate_and_offset_accel(uint8_t instance, const Vector3f &accel)
 {
     _imu._accel[instance] = accel;
     _imu._accel[instance].rotate(_imu._board_orientation);
@@ -33,5 +33,5 @@ void AP_InertialSensor_Backend::_rotate_and_offset_accel(uint8_t instance, const
     _imu._accel[instance].y *= accel_scale.y;
     _imu._accel[instance].z *= accel_scale.z;
     _imu._accel[instance] -= _imu._accel_offset[instance];
-    _imu._last_accel_sample_time_usec[instance] = now;
+    _imu._accel_healthy[instance] = true;
 }
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
index 524e390468..aa652e32c0 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
@@ -58,10 +58,10 @@ protected:
     AP_InertialSensor &_imu;
 
     // rotate gyro vector and offset
-    void _rotate_and_offset_gyro(uint8_t instance, const Vector3f &gyro, uint32_t now);
+    void _rotate_and_offset_gyro(uint8_t instance, const Vector3f &gyro);
 
     // rotate accel vector, scale and offset
-    void _rotate_and_offset_accel(uint8_t instance, const Vector3f &accel, uint32_t now);
+    void _rotate_and_offset_accel(uint8_t instance, const Vector3f &accel);
 
     // backend should fill in its product ID from AP_PRODUCT_ID_*
     int16_t _product_id;
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp
index 2a5e660fb4..bb0daf9bcc 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp
@@ -198,7 +198,6 @@ void AP_InertialSensor_Flymaple::_set_filter_frequency(uint8_t filter_hz)
 bool AP_InertialSensor_Flymaple::update(void) 
 {
     Vector3f accel, gyro;
-    uint32_t now = hal.scheduler->micros();
 
     hal.scheduler->suspend_timer_procs();
     accel = _accel_filtered;
@@ -209,11 +208,11 @@ bool AP_InertialSensor_Flymaple::update(void)
 
     // Adjust for chip scaling to get m/s/s
     accel *= FLYMAPLE_ACCELEROMETER_SCALE_M_S;
-    _rotate_and_offset_accel(_accel_instance, accel, now);
+    _rotate_and_offset_accel(_accel_instance, accel);
 
     // Adjust for chip scaling to get radians/sec
     gyro *= FLYMAPLE_GYRO_SCALE_R_S;
-    _rotate_and_offset_gyro(_gyro_instance, gyro, now);
+    _rotate_and_offset_gyro(_gyro_instance, gyro);
 
     if (_last_filter_hz != _imu.get_filter()) {
         _set_filter_frequency(_imu.get_filter());
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp
index 0f6c7c9815..edc2672991 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp
@@ -265,7 +265,6 @@ void AP_InertialSensor_L3G4200D::_set_filter_frequency(uint8_t filter_hz)
 bool AP_InertialSensor_L3G4200D::update(void) 
 {
     Vector3f accel, gyro;
-    uint32_t now = hal.scheduler->micros();
 
     hal.scheduler->suspend_timer_procs();
     accel = _accel_filtered;
@@ -276,11 +275,11 @@ bool AP_InertialSensor_L3G4200D::update(void)
 
     // Adjust for chip scaling to get m/s/s
     accel *= ADXL345_ACCELEROMETER_SCALE_M_S;
-    _rotate_and_offset_accel(_accel_instance, accel, now);
+    _rotate_and_offset_accel(_accel_instance, accel);
 
     // Adjust for chip scaling to get radians/sec
     gyro *= L3G4200D_GYRO_SCALE_R_S;
-    _rotate_and_offset_gyro(_gyro_instance, gyro, now);
+    _rotate_and_offset_gyro(_gyro_instance, gyro);
 
     if (_last_filter_hz != _imu.get_filter()) {
         _set_filter_frequency(_imu.get_filter());
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
index d6ffd2c009..bea8dd60ea 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
@@ -271,7 +271,6 @@ bool AP_InertialSensor_MPU6000::update( void )
 
     // we have a full set of samples
     uint16_t num_samples;
-    uint32_t now = hal.scheduler->micros();
     Vector3f accel, gyro;
 
     hal.scheduler->suspend_timer_procs();
@@ -284,10 +283,10 @@ bool AP_InertialSensor_MPU6000::update( void )
     hal.scheduler->resume_timer_procs();
 
     gyro *= _gyro_scale / num_samples;
-    _rotate_and_offset_gyro(_gyro_instance, gyro, now);
+    _rotate_and_offset_gyro(_gyro_instance, gyro);
 
     accel *= MPU6000_ACCEL_SCALE_1G / num_samples;
-    _rotate_and_offset_accel(_accel_instance, accel, now);
+    _rotate_and_offset_accel(_accel_instance, accel);
 
     if (_last_filter_hz != _imu.get_filter()) {
         if (_spi_sem->take(10)) {
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp
index 6e6bf62674..f89d5c66f5 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp
@@ -1125,7 +1125,6 @@ void AP_InertialSensor_MPU9150::_accumulate(void)
 bool AP_InertialSensor_MPU9150::update(void) 
 {
     Vector3f accel, gyro;
-    uint32_t now = hal.scheduler->micros();
 
     hal.scheduler->suspend_timer_procs();
     accel = _accel_filtered;
@@ -1134,10 +1133,10 @@ bool AP_InertialSensor_MPU9150::update(void)
     hal.scheduler->resume_timer_procs();
 
     accel *= MPU9150_ACCEL_SCALE_2G;
-    _rotate_and_offset_accel(_accel_instance, accel, now);
+    _rotate_and_offset_accel(_accel_instance, accel);
 
     gyro *= MPU9150_GYRO_SCALE_2000;
-    _rotate_and_offset_gyro(_gyro_instance, gyro, now);
+    _rotate_and_offset_gyro(_gyro_instance, gyro);
 
     if (_last_filter_hz != _imu.get_filter()) {
         _set_filter_frequency(_imu.get_filter());
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
index fe46d82da0..e77d38f7ff 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
@@ -266,8 +266,6 @@ bool AP_InertialSensor_MPU9250::_init_sensor(AP_InertialSensor::Sample_rate samp
  */
 bool AP_InertialSensor_MPU9250::update( void )
 {
-    uint32_t now = hal.scheduler->micros();
-
     // pull the data from the timer shared data buffer
     uint8_t idx = _shared_data_idx;
     Vector3f gyro = _shared_data[idx]._gyro_filtered;
@@ -288,8 +286,8 @@ bool AP_InertialSensor_MPU9250::update( void )
     gyro.rotate(ROTATION_YAW_180);
 #endif
 
-    _rotate_and_offset_gyro(_gyro_instance, gyro, now);
-    _rotate_and_offset_accel(_accel_instance, accel, now);
+    _rotate_and_offset_gyro(_gyro_instance, gyro);
+    _rotate_and_offset_accel(_accel_instance, accel);
 
     if (_last_filter_hz != _imu.get_filter()) {
         _set_filter(_imu.get_filter());
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp
index b65b11ae15..d2b856a775 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp
@@ -97,7 +97,6 @@ bool AP_InertialSensor_Oilpan::_init_sensor(AP_InertialSensor::Sample_rate sampl
 bool AP_InertialSensor_Oilpan::update()
 {
     float adc_values[6];
-    uint32_t now = hal.scheduler->micros();
 
     apm1_adc.Ch6(_sensors, adc_values);
 
@@ -106,14 +105,14 @@ bool AP_InertialSensor_Oilpan::update()
     v(_sensor_signs[0] * ( adc_values[0] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_x,
       _sensor_signs[1] * ( adc_values[1] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_y,
       _sensor_signs[2] * ( adc_values[2] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_z);
-    _rotate_and_offset_gyro(_gyro_instance, v, now);
+    _rotate_and_offset_gyro(_gyro_instance, v);
 
     // copy accels to frontend
     v(_sensor_signs[3] * (adc_values[3] - OILPAN_RAW_ACCEL_OFFSET),
       _sensor_signs[4] * (adc_values[4] - OILPAN_RAW_ACCEL_OFFSET),
       _sensor_signs[5] * (adc_values[5] - OILPAN_RAW_ACCEL_OFFSET));
     v *= OILPAN_ACCEL_SCALE_1G;
-    _rotate_and_offset_accel(_accel_instance, v, now);
+    _rotate_and_offset_accel(_accel_instance, v);
 
     return true;
 }
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
index f37b86c448..fc0ef65aad 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
@@ -124,17 +124,15 @@ bool AP_InertialSensor_PX4::update(void)
     // get the latest sample from the sensor drivers
     _get_sample();
 
-    uint32_t now = hal.scheduler->micros();
-
     for (uint8_t k=0; k<_num_accel_instances; k++) {
         Vector3f accel = _accel_in[k];
-        _rotate_and_offset_accel(_accel_instance[k], accel, now);
+        _rotate_and_offset_accel(_accel_instance[k], accel);
         _last_accel_update_timestamp[k] = _last_accel_timestamp[k];
     }
 
     for (uint8_t k=0; k<_num_gyro_instances; k++) {
         Vector3f gyro = _gyro_in[k];
-        _rotate_and_offset_gyro(_gyro_instance[k], gyro, now);
+        _rotate_and_offset_gyro(_gyro_instance[k], gyro);
         _last_gyro_update_timestamp[k] = _last_gyro_timestamp[k];
     }