Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-07 09:13:32 +08:00
Code Issues Actions 34 Packages Projects Releases Wiki Activity

MPU6K: Improve gyro self test to allow more realistic deviations from nominal state

Browse Source
This commit is contained in:
Lorenz Meier
2015-01-26 20:04:56 +01:00
parent 9c627255cc
commit b8fade7dcf
1 changed files with 22 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/mpu6000/mpu6000.cpp
+22 -9
View File
@@ -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)
return 1;
if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f)
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.y_offset) > 0.12f || fabsf(_gyro_scale.y_offset) < 0.000001f)
return 1;
if (fabsf(_gyro_scale.y_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.z_offset) > 0.12f || fabsf(_gyro_scale.z_offset) < 0.000001f)
if (fabsf(_gyro_scale.y_offset) > max_offset)
return 1;
if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f)
if (fabsf(_gyro_scale.y_scale - 1.0f) > max_scale)
return 1;
if (fabsf(_gyro_scale.z_offset) > max_offset)
return 1;
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;
}