From 731329fc5571cda70916d9e617f2348f9fc287fe Mon Sep 17 00:00:00 2001 From: Emile Castelnuovo Date: Tue, 30 Dec 2014 11:32:19 +0100 Subject: [PATCH] AP_InertialSensor: correction to AP_InertialSensor_VRBRAIN --- .../AP_InertialSensor_VRBRAIN.cpp | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) 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()) {