diff options
author | Michael Johnson <mpj@google.com> | 2011-08-19 11:47:08 -0700 |
---|---|---|
committer | Mathias Agopian <mathias@google.com> | 2012-06-27 17:07:55 -0700 |
commit | 3e87d8dadefaf4b56bf15a15f1b53928d7a12cd2 (patch) | |
tree | ab668b163c12f23f013e4b4402e703a187afa34f /services/sensorservice/Fusion.cpp | |
parent | a01b4e237d57b74689576a3d486a2b2b903e74f4 (diff) | |
download | frameworks_native-3e87d8dadefaf4b56bf15a15f1b53928d7a12cd2.zip frameworks_native-3e87d8dadefaf4b56bf15a15f1b53928d7a12cd2.tar.gz frameworks_native-3e87d8dadefaf4b56bf15a15f1b53928d7a12cd2.tar.bz2 |
More error checks to avoid div by zero.
Change-Id: I18e5b72d02bf5420c14334d3a03f18fa40572d31
Diffstat (limited to 'services/sensorservice/Fusion.cpp')
-rw-r--r-- | services/sensorservice/Fusion.cpp | 59 |
1 files changed, 55 insertions, 4 deletions
diff --git a/services/sensorservice/Fusion.cpp b/services/sensorservice/Fusion.cpp index ff4786b..e6ca2cc 100644 --- a/services/sensorservice/Fusion.cpp +++ b/services/sensorservice/Fusion.cpp @@ -47,9 +47,41 @@ static const float biasVAR = 1e-8; // (rad/s)^2 / s (guessed) static const float accSTDEV = 0.05f; // m/s^2 (measured 0.08 / CDD 0.05) static const float magSTDEV = 0.5f; // uT (measured 0.7 / CDD 0.5) -static const float FREE_FALL_THRESHOLD = 0.981f; static const float SYMMETRY_TOLERANCE = 1e-10f; +/* + * Accelerometer updates will not be performed near free fall to avoid ill-conditioning and + * div by zeros. + * Threshhold: 10% of g, in m/s^2 + */ +static const float FREE_FALL_THRESHOLD = 0.981f; +static const float FREE_FALL_THRESHOLD_SQ = FREE_FALL_THRESHOLD*FREE_FALL_THRESHOLD; + +/* + * The geomagnetic-field should be between 30uT and 60uT. + * Fields strengths greater than this likely indicate a local magnetic disturbance which + * we do not want to update into the fused frame. + */ +static const float MAX_VALID_MAGNETIC_FIELD = 100; // uT +static const float MAX_VALID_MAGNETIC_FIELD_SQ = MAX_VALID_MAGNETIC_FIELD*MAX_VALID_MAGNETIC_FIELD; + +/* + * Values of the field smaller than this should be ignored in fusion to avoid ill-conditioning. + * This state can happen with anomalous local magnetic disturbances canceling the Earth field. + */ +static const float MIN_VALID_MAGNETIC_FIELD = 10; // uT +static const float MIN_VALID_MAGNETIC_FIELD_SQ = MIN_VALID_MAGNETIC_FIELD*MIN_VALID_MAGNETIC_FIELD; + +/* + * If the cross product of two vectors has magnitude squared less than this, we reject it as + * invalid due to alignment of the vectors. + * This threshold is used to check for the case where the magnetic field sample is parallel to + * the gravity field, which can happen in certain places due to magnetic field disturbances. + */ +static const float MIN_VALID_CROSS_PRODUCT_MAG = 1.0e-3; +static const float MIN_VALID_CROSS_PRODUCT_MAG_SQ = + MIN_VALID_CROSS_PRODUCT_MAG*MIN_VALID_CROSS_PRODUCT_MAG; + // ----------------------------------------------------------------------- template <typename TYPE, size_t C, size_t R> @@ -240,8 +272,10 @@ void Fusion::handleGyro(const vec3_t& w, float dT) { status_t Fusion::handleAcc(const vec3_t& a) { // ignore acceleration data if we're close to free-fall - if (length(a) < FREE_FALL_THRESHOLD) + if (length_squared(a) < FREE_FALL_THRESHOLD_SQ) { + LOGW("handleAcc: near free fall, not updating!"); return BAD_VALUE; + } if (!checkInitComplete(ACC, a)) return BAD_VALUE; @@ -253,15 +287,32 @@ status_t Fusion::handleAcc(const vec3_t& a) { status_t Fusion::handleMag(const vec3_t& m) { // the geomagnetic-field should be between 30uT and 60uT - // reject obviously wrong magnetic-fields - if (length(m) > 100) + // reject if too large to avoid spurious magnetic sources + const float magFieldSq = length_squared(m); + if (magFieldSq > MAX_VALID_MAGNETIC_FIELD_SQ) { + LOGW("handleMag: magnetic field too large, not updating!"); + return BAD_VALUE; + } else if (magFieldSq < MIN_VALID_MAGNETIC_FIELD_SQ) { + // Also reject if too small since we will get ill-defined (zero mag) cross-products below + LOGW("handleMag: magnetic field too small, not updating!"); return BAD_VALUE; + } if (!checkInitComplete(MAG, m)) return BAD_VALUE; + // Orthogonalize the magnetic field to the gravity field, mapping it into tangent to Earth. const vec3_t up( getRotationMatrix() * Ba ); const vec3_t east( cross_product(m, up) ); + + // If the m and up vectors align, the cross product magnitude will approach 0. + // Reject this case as well to avoid div by zero problems and ill-conditioning below. + if (length_squared(east) < MIN_VALID_CROSS_PRODUCT_MAG_SQ) { + LOGW("handleMag: magnetic field too aligned with up vector, not updating!"); + return BAD_VALUE; + } + + // If we have created an orthogonal magnetic field successfully, then pass it in as the update. vec3_t north( cross_product(up, east) ); const float l = 1 / length(north); |