diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2015-01-26 20:04:56 +0100 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2015-01-26 20:04:56 +0100 |
| commit | b8fade7dcfa98d0963c7f23529168baa64f7fc1a (patch) | |
| tree | f20bf3e051d39017c3c4c926c55688927de54525 | |
| parent | 9c627255ccc980270fe56b6c4ddeb494e1ce0f50 (diff) | |
| download | px4-firmware-b8fade7dcfa98d0963c7f23529168baa64f7fc1a.tar.gz px4-firmware-b8fade7dcfa98d0963c7f23529168baa64f7fc1a.tar.bz2 px4-firmware-b8fade7dcfa98d0963c7f23529168baa64f7fc1a.zip | |
MPU6K: Improve gyro self test to allow more realistic deviations from nominal state
| -rw-r--r-- | src/drivers/mpu6000/mpu6000.cpp | 27 |
1 files changed, 20 insertions, 7 deletions
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 81d156af1..1c35bae2d 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -921,22 +921,35 @@ MPU6000::gyro_self_test() if (self_test()) return 1; - /* evaluate gyro offsets, complain if offset -> zero or larger than 7 dps */ - if (fabsf(_gyro_scale.x_offset) > 0.12f || fabsf(_gyro_scale.x_offset) < 0.000001f) + const float max_offset = 0.34f; + const float max_scale = 0.3f; + + /* evaluate gyro offsets, complain if offset -> zero or larger than 20 dps. */ + if (fabsf(_gyro_scale.x_offset) > max_offset) return 1; - if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f) + + /* evaluate gyro scale, complain if off by more than 30% */ + if (fabsf(_gyro_scale.x_scale - 1.0f) > max_scale) return 1; - if (fabsf(_gyro_scale.y_offset) > 0.12f || fabsf(_gyro_scale.y_offset) < 0.000001f) + if (fabsf(_gyro_scale.y_offset) > max_offset) return 1; - if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f) + if (fabsf(_gyro_scale.y_scale - 1.0f) > max_scale) return 1; - if (fabsf(_gyro_scale.z_offset) > 0.12f || fabsf(_gyro_scale.z_offset) < 0.000001f) + if (fabsf(_gyro_scale.z_offset) > max_offset) return 1; - if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f) + if (fabsf(_gyro_scale.z_scale - 1.0f) > max_scale) return 1; + /* check if all scales are zero */ + if ((fabsf(_gyro_scale.x_offset) < 0.000001f) && + (fabsf(_gyro_scale.y_offset) < 0.000001f) && + (fabsf(_gyro_scale.z_offset) < 0.000001f)) { + /* if all are zero, this device is not calibrated */ + return 1; + } + return 0; } |