From f5cfea78b0454a31571693ee86c321adcb965410 Mon Sep 17 00:00:00 2001 From: Michael Johnson Date: Fri, 19 Aug 2011 11:47:08 -0700 Subject: [PATCH 1/2] More error checks to avoid div by zero. Change-Id: I18e5b72d02bf5420c14334d3a03f18fa40572d31 --- services/sensorservice/Fusion.cpp | 59 ++++++++++++++++++++++++++++--- services/sensorservice/vec.h | 9 +++++ 2 files changed, 64 insertions(+), 4 deletions(-) diff --git a/services/sensorservice/Fusion.cpp b/services/sensorservice/Fusion.cpp index ff4786bf02b26..e6ca2ccffaa24 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 @@ -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); diff --git a/services/sensorservice/vec.h b/services/sensorservice/vec.h index f74ccc5794347..24f30ff436e2e 100644 --- a/services/sensorservice/vec.h +++ b/services/sensorservice/vec.h @@ -207,6 +207,15 @@ TYPE PURE length(const V& v) { return sqrt(dot_product(v, v)); } +template < + template class V, + typename TYPE, + size_t SIZE +> +TYPE PURE length_squared(const V& v) { + return dot_product(v, v); +} + template < template class V, typename TYPE, From 3a3fca3dcedb8bcbcde5e2c037369b5ee3820646 Mon Sep 17 00:00:00 2001 From: Mathias Agopian Date: Wed, 24 Aug 2011 18:40:33 -0700 Subject: [PATCH 2/2] Fix a few style issues and remove LOG spam Change-Id: I6b6f75373f4ac28f98dea6a6f1c2567a6aa02243 --- services/sensorservice/Fusion.cpp | 50 +++++++++++++++++-------------- 1 file changed, 28 insertions(+), 22 deletions(-) diff --git a/services/sensorservice/Fusion.cpp b/services/sensorservice/Fusion.cpp index e6ca2ccffaa24..0ab86c3e76a4b 100644 --- a/services/sensorservice/Fusion.cpp +++ b/services/sensorservice/Fusion.cpp @@ -50,33 +50,38 @@ static const float magSTDEV = 0.5f; // uT (measured 0.7 / CDD 0.5) static const float SYMMETRY_TOLERANCE = 1e-10f; /* - * Accelerometer updates will not be performed near free fall to avoid ill-conditioning and - * div by zeros. + * 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; +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. + * 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; +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. + * 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; +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. + * 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 = @@ -273,7 +278,6 @@ 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_squared(a) < FREE_FALL_THRESHOLD_SQ) { - LOGW("handleAcc: near free fall, not updating!"); return BAD_VALUE; } @@ -290,29 +294,31 @@ status_t Fusion::handleMag(const vec3_t& m) { // 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!"); + // Also reject if too small since we will get ill-defined (zero mag) + // cross-products below return BAD_VALUE; } if (!checkInitComplete(MAG, m)) return BAD_VALUE; - // Orthogonalize the magnetic field to the gravity field, mapping it into tangent to Earth. + // 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 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. + // 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);