From 7c288e020b8cec12b20f49a122fd2f1027bf3883 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 16 Nov 2014 12:30:33 +1100 Subject: [PATCH] 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 --- .../AP_InertialSensor/AP_InertialSensor_PX4.cpp | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp index 4d41a15428..4432af65ff 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp +++ b/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()) {