AP_InertialSensor: fixed detection of dead IMU

if a PX4 sensor does not give new data we need to avoid calling
_rotate_and_offset_*() to avoid marking it as healthy. Otherwise if
the MPU6k dies we won't switch to the LSM303D automatically

libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp

@@ -104,14 +104,22 @@ bool AP_InertialSensor_PX4::update(void)
 
     for (uint8_t k=0; k<_num_accel_instances; k++) {
         Vector3f accel = _accel_in[k];
-        _rotate_and_offset_accel(_accel_instance[k], accel);
-        _last_accel_update_timestamp[k] = _last_accel_timestamp[k];
+        // calling _rotate_and_offset_accel sets the sensor healthy,
+        // so we only want to do this if we have new data from it
+        if (_last_accel_timestamp[k] != _last_accel_update_timestamp[k]) {
+            _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);
-        _last_gyro_update_timestamp[k] = _last_gyro_timestamp[k];
+        // calling _rotate_and_offset_accel sets the sensor healthy,
+        // so we only want to do this if we have new data from it
+        if (_last_gyro_timestamp[k] != _last_gyro_update_timestamp[k]) {
+            _rotate_and_offset_gyro(_gyro_instance[k], gyro);
+            _last_gyro_update_timestamp[k] = _last_gyro_timestamp[k];
+        }
     }
 
     if (_last_filter_hz != _imu.get_filter()) {