Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
Andrew Tridgell 2014-11-16 12:30:33 +11:00
parent 22237f2530
commit 7c288e020b
1 changed files with 12 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
View File

@ -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()) {