diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2015-01-28 16:29:14 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2015-01-28 16:29:14 +0100 |
commit | 2728889f7886e3ab2fea16941d29e60ece0d4449 (patch) | |
tree | ca9994d71205731ee4bb404175c2cf8f13fcc539 /src/drivers/mpu6000/mpu6000.cpp | |
parent | f23e603d02ba416ae250770cdaad6a859d6bae69 (diff) | |
parent | 1dcc5c49cc75778bcdde770f2d6c2700dd2bec2e (diff) | |
download | px4-firmware-2728889f7886e3ab2fea16941d29e60ece0d4449.tar.gz px4-firmware-2728889f7886e3ab2fea16941d29e60ece0d4449.tar.bz2 px4-firmware-2728889f7886e3ab2fea16941d29e60ece0d4449.zip |
Merge remote-tracking branch 'upstream/master' into ros_messagelayer_merge_attctlposctl
Diffstat (limited to 'src/drivers/mpu6000/mpu6000.cpp')
-rw-r--r-- | src/drivers/mpu6000/mpu6000.cpp | 38 |
1 files changed, 31 insertions, 7 deletions
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 168b34ea9..2be758244 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -921,26 +921,50 @@ MPU6000::gyro_self_test() if (self_test()) return 1; - /* evaluate gyro offsets, complain if offset -> zero or larger than 6 dps */ - if (fabsf(_gyro_scale.x_offset) > 0.1f || fabsf(_gyro_scale.x_offset) < 0.000001f) + /* + * Maximum deviation of 20 degrees, according to + * http://www.invensense.com/mems/gyro/documents/PS-MPU-6000A-00v3.4.pdf + * Section 6.1, initial ZRO tolerance + */ + const float max_offset = 0.34f; + /* 30% scale error is chosen to catch completely faulty units but + * to let some slight scale error pass. Requires a rate table or correlation + * with mag rotations + data fit to + * calibrate properly and is not done by default. + */ + 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.1f || 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.1f || 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; } + /* perform a self-test comparison to factory trim values. This takes about 200ms and will return OK if the current values are within 14% |