diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_VRBRAIN.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_VRBRAIN.cpp index ce269f9535..9e74088c85 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_VRBRAIN.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_VRBRAIN.cpp @@ -104,14 +104,22 @@ bool AP_InertialSensor_VRBRAIN::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()) {